Stap 5: Arduino code - robot lopen
Lijn volgeling robot operatie script
Braian
***/
/ * motoren output pinnen * /
Const int heer = 6; Rechts motor vooruit pin
Const int ML = 5; Linker motor vooruit pin
/ * sensor input pinnen * /
Const int sensorPins [] {A0, A1, A2} =; rechts, midden, links
/ * andere input en output pinnen * /
Const int ledPin = 13; LED geeft aan of de search-modus ingeschakeld is
Const int potPin = A5; voor het aanpassen van links-rechts-fout
/ * arrays voor het opslaan van sensor en berekende gegevens * /
int sensorReadings [3]; sensor lezingen rechts, centreren, links
float sensorValues [3]; sensor lezingen rechts, centreren, links
int potAdjust = 0; sensor evenwicht aanpassing met de potmeter waarde
/ * tellers, helpers enz * /
int i; algemene teller in for-lussen
int unknownCount = 0; teller van opeenvolgende lijn-onbekend-Staten
int s; PWM waarde voor snelheid
/ *** INSTELLINGEN *** /
Const int turnThreshold = 15; start op die verschil tussen links en rechts sensorreadings glad draaien (1000 eenheden schaal formulier wit op zwart)
Const int quickturnThreshold = 20; start steile draaien op welke verschil tussen linker- en sensorreadings
Const int indicateThreshold = 2; op welke lezing te overwegen lijn bekend
Const int moveLength = 55; Hoe lang moet een beweging nemen nadat snelheid heeft opgedaan (afhankelijk van het volgende van drie variabelen)
en voordat nieuwe controle van sensor staat moet worden aangeroepen (in milliseconden)
tijd het neemt om snelheid = [(maxSpeed-startSpeed) / speedStep] (in millis) huidige: 9
Const int maxSpeed = 160; Max PWM waarde voor motoren
voor het starten van de betere verhogen we de waarde van de PWM in tijd:
Const int startSpeed = 80; PWM waaruit u om te beginnen, bijvoorbeeld 1/2 van max
Const int speedStep = 10; Hoeveel PWM waarden te verhogen na elke 1 milliseconde
Const int pauseLength = 140; pauze tussen bewegingen voor betere sensorgegevens
int unknownMax = 25; na hoeveel opeenvolgende "lijn onbekend" measurings om te beginnen Zoek lijn routine
/ * SENSOR correcties voor witpunt en de "eenheid": rechts, midden, links
DEZE WAARDEN KOPIËREN UIT KALIBRATIE PROGRAMMA SERIËLE OUTPUT * /
Const int correcties [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 pinnen * /
pinMode (MIJNHEER, uitvoer);
pinMode (ML, uitvoer);
/ * sensor pinnen * /
pinMode (sensorPins [0], INPUT);
pinMode (sensorPins [1], INPUT);
pinMode (sensorPins [2], INPUT);
pinMode (potPin, INPUT);
} / / / setup
void loop {}
readSensors();
Als
(
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 ++;
}
else {}
unknownCount = 0;
}
Als (unknownCount < = unknownMax) {}
rideLine();
}
else {}
searchLine();
}
} / / / loop
VOID readSensors() {//sets array sensorValues [] sensor lezingen met berekende correcties
voor (ik = 0; ik < = 2; i ++) {}
sensorReadings [i] = analogRead(sensorPins[i]);
sensorValues [i] = (sensorReadings [i] - corrections[i]) / unitSteps [i];
}
potAdjust = (analogRead(potPin) - 45) * 10;
} / / /readSensors
ongeldig goStraight() {}
Verhoog geleidelijk de snelheid
voor (s = startSpeed; s < = maxSpeed; s = s + speedStep) {}
analogWrite (MIJNHEER, s);
analogWrite (ML, s);
delay(1);
}
volle snelheid
analogWrite (MIJNHEER, s);
analogWrite (ML, s);
delay(moveLength);
digitalWrite (MR, laag);
digitalWrite (ML, laag);
Stop
delay(pauseLength);
}
VOID goLeft (int turnspeed) {//turnspeed = 0 -> vloeiend draaien, anders steile draaien
digitalWrite (ML, laag);
Verhoog geleidelijk de snelheid
voor (s = startSpeed; s < = maxSpeed; s = s + speedStep) {}
analogWrite (MIJNHEER, s);
if(turnspeed==0) {}
analogWrite (ML, s * 0.4);
}
delay(1);
}
volle snelheid
analogWrite (MIJNHEER, s);
if(turnspeed==0) {}
analogWrite (ML, s * 0.4);
}
delay(moveLength);
digitalWrite (MR, laag);
digitalWrite (ML, laag);
Stop
delay(pauseLength);
}
VOID goRight (int turnspeed) {//turnspeed = 0 -> vloeiend draaien, anders steile draaien
digitalWrite (MR, laag);
Verhoog geleidelijk de snelheid
voor (s = startSpeed; s < = maxSpeed; s = s + speedStep) {}
analogWrite (ML, s);
if(turnspeed==0) {}
analogWrite (MIJNHEER, s * 0.4);
}
delay(1);
}
volle snelheid
analogWrite (ML, s);
if(turnspeed=0) {}
analogWrite (MIJNHEER, s * 0.4);
}
delay(moveLength);
digitalWrite (ML, laag);
digitalWrite (MR, laag);
Stop
delay(pauseLength);
}
ongeldig rideLine() {}
verschil tussen links en rechts sensoren aangeeft, rekening houdend potentiometer accountinstellingen, turn dienovereenkomstig
Als (sensorValues [0] - sensorValues [2] > turnThreshold + potAdjust & & sensorValues [0] - sensorValues [2] < quickturnThreshold + potAdjust) {}
goRight(0);
}
anders als (sensorValues [0] - sensorValues [2] > quickturnThreshold + potAdjust) {}
goRight(1);
}
anders als (sensorValues [2] - sensorValues [0] > turnThreshold - potAdjust & & sensorValues [2] - sensorValues [0] < quickturnThreshold - potAdjust) {}
goLeft(0);
}
anders als (sensorValues [2] - sensorValues [0] > quickturnThreshold - potAdjust) {}
goLeft(1);
}
else {}
goStraight();
}
} / / /rideLine()
ongeldig searchLine() {}
digitalWrite (ledPin, hoge); aangeven van modus met LED
int searchVar = 1; Helper variabele voor het verhogen van de straal van de beweging zoeken
terwijl
(
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
) {
Als (searchVar < = 5) {//go rechte voor een tijdje, in het geval dat eigenlijk niet verloren
goStraight();
}
else {//rightward verkeer met toenemende straal door het invoegen van rechte sequenties
voor (ik = 0; ik < 7; i ++) {}
goRight(1);
}
voor (ik = 0; ik < searchVar/5; i ++) {}
goStraight();
}
}
searchVar ++; aangeven van voltooide zoeken naar routine state
readSensors(); krijgen nieuwe lezingen
}
unknownCount = 0; teller reset
digitalWrite (ledPin, laag); einde van de zoekmodus aangeven
} / / /searchLine()
