Linefollower robot från Arduino och skräp - tankar och koden (5 / 5 steg)
Steg 5: Arduino kod - robot kör
Efterföljare robot drift kommandoradsskript
Mikko
***/
/ * motors produktionen stift * /
CONST int herr = 6; Rätt motor framåt pin
CONST int ML = 5; Vänster motor framåt pin
/ * sensor ingång stift * /
CONST int sensorPins [] = {A0, A1, A2}; höger, center, vänster
/ * andra indata- och stift * /
CONST int ledPin = 13. LED indikerar om sökläget är på
CONST int potPin = A5; för att justera vänster höger fel
/ * matriser för lagring av sensor och beräknade data * /
int sensorReadings [3]. sensormätningar höger, mitten, vänster
float sensorValues [3]. sensormätningar höger, mitten, vänster
int potAdjust = 0; sensor balans justering med potentiometer värde
/ * räknare, helpers etc * /
int i; de allmänna counter i för loopar
int unknownCount = 0; motverka varandra linje-unknown staternas
int s; PWM-värde för hastighet
/ *** INSTÄLLNINGAR *** /
CONST int turnThreshold = 15. starta på vilken skillnaden mellan vänster och höger sensorreadings smidig roterande (1000 enheter skala form vit till svart)
CONST int quickturnThreshold = 20; starta på vilken skillnaden mellan vänster och höger sensorreadings branta svarvning
CONST int indicateThreshold = 2; på vilken behandlingen att överväga linje känd
CONST int moveLength = 55. Hur lång tid ska en rörelse ta efter hastighet har uppnåtts (beror på följande tre variabler)
och innan ny kontroll av sensor status bör kallas (i millisekunder)
tid det tar att få fart = [(maxSpeed-startSpeed) / speedStep] (i millis) nuvarande: 9
CONST int MaxOmdr = 160; Max PWM värde för motorer
för bättre start öka vi PWM värdet i tid:
CONST int startSpeed = 80. PWM som vill starta, 1/2 max till exempel
CONST int speedStep = 10; Hur många PWM värden att öka efter varje 1 millisekund
CONST int pauseLength = 140; paus mellan drag för bättre sensor läsning
int unknownMax = 25; efter hur många varandra "line okänd" measurings startar sökningen linje rutin
/ * SENSOR korrigeringar för vitpunkt och "enhet steg": höger, center, vänster
KOPIERA VÄRDENA FRÅN KALIBRERING PROGRAM SERIAL OUTPUT * /
CONST int korrigeringar [3] = {38,38,31}.
CONST float unitSteps [3] = {0.06,0.01,0.02};
CONST float betweenReadings [2] = {219.30,233.33};
/**/
void setup() {
/ * motor stift * /
pinMode (herr, OUTPUT);
pinMode (ML, OUTPUT);
/ * sensor stift * /
pinMode (sensorPins [0], ingång);
pinMode (sensorPins [1], ingång);
pinMode (sensorPins [2], ingång);
pinMode (potPin, ingång);
} / / / setup
void loop() {
readSensors();
om
(
sensorValues [0] + sensorValues [1] < 0,1 * betweenReadings [0] ||
sensorValues [1] + sensorValues [2] < 0,1 * betweenReadings [1] ||
sensorValues [0] < indicateThreshold ||
sensorValues [1] < indicateThreshold ||
sensorValues [2] < indicateThreshold
) {
unknownCount ++;
}
annat {
unknownCount = 0;
}
om (unknownCount < = unknownMax) {
rideLine();
}
annat {
searchLine();
}
} / / / slinga
void readSensors() {//sets array sensorValues [] till sensormätningar med beräknade rättelser
för (jag = 0; jag < = 2; i ++) {
sensorReadings [i] = analogRead(sensorPins[i]);
sensorValues [i] = (sensorReadings [i] - corrections[i]) / unitSteps [i];
}
potAdjust = (analogRead(potPin) - 45) * 10;
} / / /readSensors
void goStraight() {
öka hastigheten successivt
för (s = startSpeed; s < = MaxOmdr; s = s + speedStep) {
analogWrite (herr, s);
analogWrite (ML, s);
Delay(1);
}
full fart
analogWrite (herr, s);
analogWrite (ML, s);
Delay(moveLength);
digitalWrite (MR, låg);
digitalWrite (ML, låg).
Stanna
Delay(pauseLength);
}
void goLeft (int turnspeed) {//turnspeed = 0 -> smidig svarvning, annars brant svarvning
digitalWrite (ML, låg).
öka hastigheten successivt
för (s = startSpeed; s < = MaxOmdr; s = s + speedStep) {
analogWrite (herr, s);
IF(turnspeed==0) {
analogWrite (ML, s * 0,4);
}
Delay(1);
}
full fart
analogWrite (herr, s);
IF(turnspeed==0) {
analogWrite (ML, s * 0,4);
}
Delay(moveLength);
digitalWrite (MR, låg);
digitalWrite (ML, låg).
Stanna
Delay(pauseLength);
}
void goRight (int turnspeed) {//turnspeed = 0 -> smidig svarvning, annars brant svarvning
digitalWrite (MR, låg);
öka hastigheten successivt
för (s = startSpeed; s < = MaxOmdr; s = s + speedStep) {
analogWrite (ML, s);
IF(turnspeed==0) {
analogWrite (herr, s * 0,4);
}
Delay(1);
}
full fart
analogWrite (ML, s);
IF(turnspeed=0) {
analogWrite (herr, s * 0,4);
}
Delay(moveLength);
digitalWrite (ML, låg).
digitalWrite (MR, låg);
Stanna
Delay(pauseLength);
}
void rideLine() {
som anger skillnaden mellan höger och vänster sensorer, med potentiometer kontoinställningar, vänd därefter
om (sensorValues [0] - sensorValues [2] > turnThreshold + potAdjust & & sensorValues [0] - sensorValues [2] < quickturnThreshold + potAdjust) {
goRight(0);
}
annars om (sensorValues [0] - sensorValues [2] > quickturnThreshold + potAdjust) {
goRight(1);
}
annars om (sensorValues [2] - sensorValues [0] > turnThreshold - potAdjust & & sensorValues [2] - sensorValues [0] < quickturnThreshold - potAdjust) {
goLeft(0);
}
annars om (sensorValues [2] - sensorValues [0] > quickturnThreshold - potAdjust) {
goLeft(1);
}
annat {
goStraight();
}
} / / /rideLine()
void searchLine() {
digitalWrite (ledPin, hög); Ange sökläge med LED
int searchVar = 1; helper variabel för att öka Sök rörelse radien
medan
(
sensorValues [0] + sensorValues [1] < 0,1 * betweenReadings [0] & &
sensorValues [1] + sensorValues [2] < 0,1 * betweenReadings [1] & &
sensorValues [0] < indicateThreshold & &
sensorValues [1] < indicateThreshold & &
sensorValues [2] < indicateThreshold
) {
om (searchVar < = 5) {//go rakt ett tag, i de fall faktiskt inte förlorat
goStraight();
}
annat {//rightward rörelse med ökande radie genom att infoga raka sekvenser
för (jag = 0; jag < 7; i ++) {
goRight(1);
}
för (jag = 0; jag < searchVar/5; i ++) {
goStraight();
}
}
searchVar ++; indikera avslutade Sök rutinmässiga tillstånd
readSensors(); få nya avläsningar
}
unknownCount = 0; Återställ räknaren
digitalWrite (ledPin, låg); Ange slutet av sökläge
} / / /searchLine()