Skip to content
Snippets Groups Projects
Select Git revision
  • 76363398c634b9fc641b82b282d306310fb3fd67
  • main default protected
  • Localisation
3 results

PID_direction.ino

Blame
  • user avatar
    Ouhouuhu authored
    76363398
    History
    PID_direction.ino 1.87 KiB
    #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;
    }