Raden efter Arduino Robot (6 / 7 steg)
Steg 6: kod
Koden har för roboten i princip bara tar avläsningar från line sensor sedan tolkar det var det på raden och berättar var och en av motorerna hur snabbt att köra. Vår robot har 8 sensor vilket innebär att om vi antar det är alltid på samma sida av linjen det finns 9 möjliga positioner 0-8 som representeras av hur många av sensorerna är på.
Vi vill att roboten vara centrerad över kanten på linjen så vi inställt "börvärde" på 4 som är utrymmet mellan den 4 och 5 sensorn. PID-styrenheter arbete genom att ta några önskat värde och där du är och tar till att av de två som är "fel". Registeransvarige portion sedan omvandlare felet i att en användbar utgång. I detta fall är felet lika med setpoint minus position. Steget är att välja trim konstanter anropa Kp och Kd i koden. Dessa är också samtal proportionellt och derivat konstanter och de kommer att vara unik för din robot baserad på din motors och dina exakta mått men våra siffror borde få dig nära. KP är PWM skillnaden mellan de två motorerna per enhet av fodra felet och multipliceras med felet. Till exempel om sensorn läser 0 felet är 4 menande är roboten längst till höger du vill ha den vänstra motorn att stoppa en PWM noll och rätt motor att köra full fart. Om Kp är 64 64 * fel är 256 och subtraheras från den normala varvtal (255) vänster kommer att komma till en helt stilla. Som roboten börjar vända felet kommer att vara mindre och motor skillnaden blir mindre tills sensorn ser 4 igen och det är full fart framåt.
Detta låter bra rätt? Inte riktigt eftersom roboten redan svängande och det tar några stopp det kommer att svänga rätt passera 4. sensorn och den 5: e. För att förhindra detta måste vi bromsa mycket snabbare eftersom vi närmare normvärdet. Det är där Kd kommer att spela. KD är multiplicerad med förändringen i fel och läggas till den Kp-delen. Det här exemplet ska vi plocka upp där vi slutade från ovan där nu sensorn läser 1. Så felet är 3 och den proportionella kontrollen är talande vänstra motorn köras vid 63 och höger på 255. Ändringen i fel -1 (3-4) och våra Kd är 16 -16 totalt. När lägger till den proportionella delen får du ett varvtal av 47 på vänstra hjulet och 255 till höger. Alla detaljer kan ses i pid-funktionen i koden och leka med Kp en Kd värden för att se hur snabbt du kan göra din bot.
Tur för dig gjorde vi svåra för dig. Kopiera och klistra in koden nedan i Arduino IDE och ladda upp den till din styrelse och din linje följande robot bör vara redo att rulla!
#define NUM_SENSORS 8
#define avgSpeed 255
int leftWheelf = 3;
int leftWheelr = 5;
int rightWheelf = 6;
int rightWheelr = 9;
int setpoint = 4, val;
osignerade långa lastTime = 0, timeChange = 0;
int Sampletime = 20, outMax = 255, outMin =-255;
flyta fel, sumerr, lastError, utgång, ITerm, DTerm;
flyta 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 tröskel = 200.
byte incomingByte;
int bias = 5;
void setup() {
Serial.BEGIN(115200);
pinMode(4,INPUT);
}
void loop() {
Serial.println(digitalRead(4));
om (digitalRead(4) == true) {
unsigned int Wsum = 0;
int summa = 0;
för (int jag = 0; jag < NUM_SENSORS; i ++) {
sensorValue[i]=analogRead(sensor[i]);
om (sensorValue [i] < tröskel) sensorValue [i] = 1;
annat sensorValue [i] = 0;
Serial.Print(i); Serial.Print(":"); Serial.println(sensorValue[i]);
Delay(250);
}
för (int jag = 0; jag < NUM_SENSORS; i ++) {
summa = sensorValue [i] + summan;
POS = summa;
}
Serial.println(POS);
Delay(100);
timeChange = millis ()-lastTime;
om (timeChange > = Sampletime) {
PID(POS);
}
}
annat {
om (Serial.available() > 0) {
incomingByte = Serial.read();
}
om (incomingByte == "w") {
Forward();
}
annat if (incomingByte == 's ") {
Reverse();
}
annat if (incomingByte == hade ") {
rightTurn();
}
annat if (incomingByte == "a") {
leftTurn();
}
annat {
Brake();
}
}
}
Annullera pid (int val) {
fel = setpoint-val;
ITerm +=(Ki*error);
DTerm=Kd*(error-lastError)/(Sampletime/1000.0);
lastError = fel;
IF(ITerm > outMax) ITerm = outMax;
annars om (ITerm < outMin) ITerm = outMin;
output = Kp * fel + ITerm + DTerm;
om (output > outMax) output = outMax;
annars om (ut < outMin) output = outMin;
lastTime=millis();
Serial.println(val);
om (output > 0) {
analogWrite(leftWheelf,avgSpeed);
analogWrite(rightWheelf,avgSpeed);
analogWrite(leftWheelr,abs(output));
analogWrite(rightWheelr,0);
}
annars om (ut < 0) {
analogWrite(leftWheelf,avgSpeed);
analogWrite(rightWheelf,avgSpeed);
analogWrite(leftWheelr,0);
analogWrite(rightWheelr,abs(output));
}
annat {
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);
}
{Ogiltig 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);
}