Stap 6: Code
De code heeft voor de robot in principe enkel lezingen van de sensor lijn neemt dan het interpreteert waar het is op de lijn en vertelt elk van de motoren hoe snel te lopen. Onze robot heeft 8 sensor, wat betekent dat als we aannemen dat het is altijd aan dezelfde kant van de lijn zijn er 9 mogelijk posities 0-8, die worden vertegenwoordigd door hoeveel van de sensoren zijn op.
Wij willen de robot te worden gecentreerd over de rand van de lijn, dus we gaan naar de "setpoint" ingesteld op 4, die de ruimte tussen de 4e en 5e sensor. PID controllers werk door het nemen van enkele gewenste waarde en waar je bent en neemt het verschil van de twee die is de 'fout'. De controller gedeelte dan conversieprogramma's de fout in naar een bruikbare output. In dit geval is de fout gelijk aan het setpoint minus positie. De stap is het selecteren van de tuning constanten oproep Kp en Kd in de code. Dit zijn ook de oproep de evenredige en afgeleide constanten en ze zullen uniek zijn voor uw robot op basis van uw motoren en uw exacte meting al onze nummers moet krijg je dicht. KP is de hoeveelheid PWM verschil tussen de twee motoren per eenheid van lijn fout en wordt vermenigvuldigd met de fout. Bijvoorbeeld als de sensor leest 0 de fout is 4 zin is de robot ook uiterst rechts wil je de linker motor om te stoppen met een PWM van nul en de juiste motor draaien volle snelheid. Als Kp 64 64 * fout is 256 en afgetrokken van de normale motor snelheid (255) links aan een volledig einde zal komen. Als de robot begint te draaien zal de fout zal worden minder en de motor verschil minder totdat de sensor 4 opnieuw ziet en het is volle snelheid vooruit.
Dit klinkt geweldig toch? Niet helemaal omdat de robot is al swingend en duurt enkele stop die het juiste zal swing langs de 4e sensor en de 5e. Om dit te voorkomen moeten we een stuk sneller als we dichter bij de setpoint vertragen. Dit is waar Kd komt te spelen. KD is vermenigvuldigd met de verandering in de fout en toegevoegd aan het Kp-deel. Voor dit voorbeeld zullen we halen waar we gebleven above waar nu de sensor is het lezen van 1. De fout is dus 3 en de proportionele controle vertelt de linker motor te draaien op 63 en juiste op 255. De verandering in fout is -1 (3-4) en onze Kd is 16 voor een totaal van -16. Wanneer toevoegt aan het proportionele deel krijg je een motorsnelheid van 47 op het linker wiel en 255 aan de rechterkant. Alle details kan worden gezien in de pid-functie in de code en spelen rond met het Kp een Kd waarden om te zien hoe snel kun je je bot.
Gelukkig voor jou hebben we het harde deel voor u. Kopieer en plak de onderstaande code in de Arduino IDE en uploaden naar je board, en uw lijn volgende robot moet klaar om te rollen!
#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); }
