diff --git a/Documentation/Projet/Arduino/PID_direction.ino b/Documentation/Projet/Arduino/PID_direction.ino new file mode 100644 index 0000000000000000000000000000000000000000..8eb36225f031c55690cc78ab2cce6cf20c86d6d5 --- /dev/null +++ b/Documentation/Projet/Arduino/PID_direction.ino @@ -0,0 +1,86 @@ +#include <PID_v2.h> + +#define DirPin A6 +// Droite 180 Gauche 850 + +#define PontG 5 +#define PontD 6 + +double Kp=1,Ki=0,Kd=0;//ces valeurs ne sont pas les bonnes + +double Setpoint, Input, Output; //valeur visé pour les roues dans [-127,127], valeur réelle des roues dans [-127,127] et valeur de la commande des roues +PID myPID(&Input, &Output, &Setpoint,Kp,Ki,Kd, DIRECT); +bool turning; +int zone_morte=150; +int marge = 5; + +void setup() { + // put your setup code here, to run once: + Serial.begin(9600); + + //configurer le PID + myPID.SetMode(AUTOMATIC); + myPID.SetOutputLimits(-255,255); + + //demander de tourner + demarrer_virage(100); +} + +void loop() { + + + if (turning) { + //on utilise un PID pour tourner + Input = 127.0/335.0*(analogRead(DirPin)-180)-127;//on ramène l'angle du potentiomètre à l'interval [-127,127] + myPID.Compute();//on calcul l'output pour notre PID + Serial.println(Input); + + + + //si on tombe dans la zone morte et que le virage est presque fini, on s'arrête là + if (abs(Output)<zone_morte && abs(Input-Setpoint)<marge) { + turning = false; + stop_direction(); + }//sinon on tourne très légèrement pour atteindre la valeur souhaitée + else if (abs(Output)<zone_morte) { + Output = 1.2*zone_morte*sign(Output); + } + + //on applique la commande + if (Output<0) { + droite(abs(Output)); + } + else { + gauche(abs(Output)); + } + } +} + + +void demarrer_virage(int value) {//value est dans [-127,127] + turning=true; + Setpoint=value; +} + +//pour value<85, ça ne tourne plus, même quand les roues ne touchent pas le sol +void gauche(int value) { + analogWrite(PontG, value); + analogWrite(PontD, 0); +} + +void droite(int value) { + analogWrite(PontD, value); + analogWrite(PontG, 0); +} + +void stop_direction() { + analogWrite(PontD, 0); + analogWrite(PontG, 0); +} + +int sign(int value) { + if (value>=0) { + return 1; + } + return -1; +} \ No newline at end of file