direction commandé par PID

Documentation/Projet/Arduino/PID_direction.ino

+#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