Stap 10: Programmeren van uw Robot
Nu dat de robot is voltooid, zult u wenst te programmeren zijn gedrag.
Bekijk de code voor deze robot.
#include
maken van servo objecten Servo leftMotor; Servo rightMotor;
Const int serialPeriod = 250; alleen afdrukken naar de seriële console elke 1/4 seconde unsigned long timeSerialDelay = 0;
Const int loopPeriod = 20; een periode van 20 MS = een frequentie van 50Hz unsigned long timeLoopDelay = 0;
Geef de trig & echo pinnen gebruikt voor de ultrasone sensoren const int ultrasonic2TrigPin = 8; Const int ultrasonic2EchoPin = 9;
int ultrasonic2Distance; int ultrasonic2Duration;
definiëren van de Staten #define DRIVE_FORWARD 0 #define TURN_LEFT 1
int staat = DRIVE_FORWARD; 0 = station uit (standaard), 1 = links
void setup {Serial.begin(9600); / / ultrasone sensor pin configuraties pinMode (ultrasonic2TrigPin, OUTPUT); pinMode (ultrasonic2EchoPin, INPUT); leftMotor.attach(13); rightMotor.attach(12);}
void loop {debugOutput(); / / prenten foutopsporing berichten naar de seriële console-if(millis() - timeLoopDelay > = loopPeriod) {readUltrasonicSensors(); / / lezen en opslaan van de gemeten afstanden stateMachine(); timeLoopDelay = millis();}}
VOID stateMachine() {if(state == DRIVE_FORWARD) / / geen obstakels gedetecteerd {als (ultrasonic2Distance > 6 || ultrasonic2Distance < 0) / / als er niets voor ons (Opmerking: ultrasonicDistance negatief voor sommige ultrasoon zal worden als er niets in bereik) {/ / drive voorwaartse rightMotor.write(180); leftMotor.write(0);} anders / / er is een object voorliggende {staat = TURN_LEFT;}} anders if(state == TURN_LEFT) / / obstakel aangetroffen--linksaf {unsigned long timeToTurnLeft = 1100; / / duurt ongeveer 1,1 seconden te draaien 90 graden unsigned long turnStartTime = millis(); / / save de tijd dat we begonnen met het draaien
while((Millis()-turnStartTime) < timeToTurnLeft) / / verblijf in deze lus totdat timeToTurnLeft (1,1 seconden) is verstreken {/ / draai links rightMotor.write(180); leftMotor.write(180);} staat = DRIVE_FORWARD; } }
VOID readUltrasonicSensors() {/ / ultrasone 2 digitalWrite (ultrasonic2TrigPin, hoge); delayMicroseconds(10); / / houdt de trig pin hoog voor ten minste 10us digitalWrite (ultrasonic2TrigPin, laag); ultrasonic2Duration = pulseIn (ultrasonic2EchoPin, hoge); ultrasonic2Distance = (ultrasonic2Duration/2) / 29;}
VOID debugOutput() {if((millis() - timeSerialDelay) > serialPeriod) {Serial.print ("ultrasonic2Distance:"); Serial.Print(ultrasonic2Distance); Serial.Print("cm"); Serial.println(); timeSerialDelay = millis(); } }