Étape 6: Code
Le code pour le robot fondamentalement juste prend des lectures du capteur de ligne, puis l’interprète a où il se trouve sur la ligne et raconte la chacun des moteurs rapidités à exécuter. Notre robot a 8 capteur qui signifie si nous supposons que c’est toujours du même côté de la ligne il y a 9 possibles positions 0-8 qui sont représentées par le nombre des capteurs sont sur.
Nous voulons que le robot est centrée sur le bord de la ligne, alors nous allons définir le « point de consigne » à 4, qui est l’espace entre le capteur de 4e et 5e. Travail de contrôleurs PID en prenant certains désiré valeur et où vous êtes et prend la différence des deux qui est le « erreur ». Le contrôleur portion puis convertisseurs de l’erreur dans une sortie utilisable. Dans ce cas, l’erreur est égale à la consigne moins position. L’étape consiste à sélectionner le réglage appel constantes Kp et Kd dans le code. Ce sont également appel la proportionnelle et constantes dérivées et ils seront uniques à votre robot basé sur vos moteurs et votre mesure exacte, bien que nos chiffres, vous devriez obtenir proches. KP est le montant de PWM de différence entre les deux moteurs par unité d’erreur de ligne et est multipliée par l’erreur. Par exemple, si le capteur mesure 0 l’erreur est 4 sens le robot est trop à droit vous souhaiterez peut-être que le moteur gauche pour arrêter un PWM de zéro et le moteur droit à pleine vitesse. Si Kp est 64 64 * erreur est 256 et soustraite de la vitesse du moteur normale (255) gauche viendra à un arrêt complet. Comme le robot se met à tourner l’erreur sera moindre et la différence de moteur sera moins jusqu'à ce que le capteur voit 4 fois et c’est à plein régime.
Cela semble grand droit ? Pas tout à fait étant donné que le robot se balance déjà et il faut quelques arrêt qu'il balancera droite passer le capteur de la 4ème et la 5ème. Pour éviter ce problème nous devons ralentir beaucoup plus vite parce que nous nous rapprochons de la consigne. C’est là qu’intervient Kd à jouer. KD est multiplié par la variation de l’erreur et ajouté à la partie de Kp. Pour cet exemple nous allons choisir là où nous avons laissé par dessus où maintenant le capteur est la lecture 1. L’erreur est donc 3 et la commande proportionnelle est révélateur du moteur gauche à courir à droite à 255 et 63. La variation de l’erreur est -1 (3-4) et notre Kd est de 16 pour un total de -16. Alors ajouter à la partie proportionnelle que vous obtenez une vitesse du moteur sur la roue gauche et 255 de 47 sur la droite. Tous les détails sont visibles dans la fonction pid dans le code et jouent avec le processus de Kimberley une Kd valeurs pour savoir à quelle vitesse vous pouvez faire votre bot.
Heureusement pour vous nous avons fait la partie la plus difficile pour vous. Copiez et collez le code ci-dessous dans l’IDE Arduino et transférez-le sur votre planche et votre ligne robot suivant devrait être prêt à rouler !
#define NUM_SENSORS 8 #define avgSpeed 255 int leftWheelf=3; int leftWheelr = 5; int rightWheelf=6; int rightWheelr = 9; int setpoint=4, val; unsigned long lastTime=0, timeChange=0; int Sampletime=20, outMax=255, outMin=-255; float error,sumerr,lastError,output,ITerm,DTerm; float Kp=avgSpeed/4, Ki=0, Kd=.25*Kp; int pos; unsigned int sensor[]={A0,A1,A2,A3,A4,A5,A6,A7}; unsigned int sensorValue[NUM_SENSORS]; int threshold = 200; byte incomingByte; int bias=5; void setup() { Serial.begin(115200); pinMode(4,INPUT); } void loop() { // Serial.println(digitalRead(4)); if (digitalRead(4)== true) { unsigned int Wsum=0; int sum=0; for (int i=0;i<NUM_SENSORS;i++) { sensorValue[i]=analogRead(sensor[i]); if (sensorValue[i] < threshold) sensorValue[i]=1; else sensorValue[i]=0; //Serial.print(i); Serial.print(": "); Serial.println(sensorValue[i]); //delay(250); } for (int i=0;i<NUM_SENSORS;i++) { sum=sensorValue[i]+sum; pos=sum; } // Serial.println(pos); //delay(100); timeChange = millis()-lastTime; if (timeChange >= Sampletime){ pid(pos); } } else { if (Serial.available() > 0) { incomingByte = Serial.read(); } if (incomingByte == 'w') { forward(); } else if (incomingByte == 's') { reverse(); } else if (incomingByte == 'd') { rightTurn(); } else if (incomingByte == 'a') { leftTurn(); } else { brake(); } } } void pid (int val) { error=setpoint-val; ITerm+=(Ki*error); DTerm=Kd*(error-lastError)/(Sampletime/1000.0); lastError=error; if(ITerm > outMax) ITerm=outMax; else if (ITerm < outMin) ITerm=outMin; output=Kp*error+ITerm+DTerm; if (output>outMax) output=outMax; else if (output<outMin) output=outMin; lastTime=millis(); Serial.println(val); if (output>0) { analogWrite(leftWheelf,avgSpeed); analogWrite(rightWheelf,avgSpeed); analogWrite(leftWheelr,abs(output)); analogWrite(rightWheelr,0); } else if (output<0) { analogWrite(leftWheelf,avgSpeed); analogWrite(rightWheelf,avgSpeed); analogWrite(leftWheelr,0); analogWrite(rightWheelr,abs(output)); } else { analogWrite(leftWheelf,avgSpeed); analogWrite(rightWheelf,avgSpeed); analogWrite(leftWheelr,0); analogWrite(rightWheelr,0); } } void forward() { analogWrite(leftWheelf, avgSpeed - bias); analogWrite(leftWheelr, 0); analogWrite(rightWheelr, 0); analogWrite(rightWheelf, avgSpeed); } void leftTurn() { analogWrite(leftWheelf,0); analogWrite(leftWheelr, 0); analogWrite(rightWheelr,0); analogWrite(rightWheelf, avgSpeed); } void rightTurn() { analogWrite(leftWheelf,avgSpeed); analogWrite(leftWheelr, 0); analogWrite(rightWheelr,0); analogWrite(rightWheelf, 0); } void reverse() { analogWrite(leftWheelf, 0); analogWrite(leftWheelr, avgSpeed-bias); analogWrite(rightWheelr, avgSpeed); analogWrite(rightWheelf, 0); } void brake() { analogWrite(leftWheelf, 0); analogWrite(leftWheelr, 0); analogWrite(rightWheelr, 0); analogWrite(rightWheelf, 0); }
