Select Git revision
PID_direction.ino
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;
}