Paso 10: Programar tu Robot
Ahora que el robot haya terminado, usted querrá programar su comportamiento.
Compruebe el código para este robot.
#include
crear objetos de servo Servo leftMotor; Servo rightMotor;
const int serialPeriod = 250; solo impresión en la consola serial cada segundo 1/4 sin firmar largo timeSerialDelay = 0;
const int loopPeriod = 20; un periodo de 20 MS = a una frecuencia de 50Hz timeLoopDelay larga sin signo = 0;
especificar el trig & echo pines utilizados para los sensores de ultrasonidos const int ultrasonic2TrigPin = 8; const int ultrasonic2EchoPin = 9;
int ultrasonic2Distance; int ultrasonic2Duration;
definir los Estados #define DRIVE_FORWARD 0 #define TURN_LEFT 1
int estado = DRIVE_FORWARD; 0 = unidad hacia adelante (por defecto), 1 = girar a la izquierda
void setup() {Serial.begin(9600); / / sensor ultrasónico pin configuraciones pinMode (ultrasonic2TrigPin, salida); pinMode (ultrasonic2EchoPin, entrada); leftMotor.attach(13); rightMotor.attach(12);}
void loop() {debugOutput(); / / impresiones de depuración los mensajes a la consola serial if(millis() - timeLoopDelay > = loopPeriod) {readUltrasonicSensors(); / / Lea y guarde las distancias medidas stateMachine(); timeLoopDelay = millis();}}
void stateMachine() {if(state == DRIVE_FORWARD) / / ningún obstáculo detectado {si (ultrasonic2Distance > 6 || ultrasonic2Distance < 0) / / si no hay nada delante de nosotros (Nota: ultrasonicDistance será negativo para algunos ultrasonidos si hay nada en la gama) {/ / unidad adelante rightMotor.write(180); leftMotor.write(0);} más / hay un objeto frente a nosotros {estado = TURN_LEFT;}} más if(state == TURN_LEFT) / / obstáculo detectado--girar a la izquierda {timeToTurnLeft largo sin signo = 1100; / / tarda alrededor de 1.1 segundos para girar a 90 grados sin firmar largo turnStartTime = millis(); / / guardar el momento en que empezamos a girar
while((Millis()-turnStartTime) < timeToTurnLeft) / / quedarse en este bucle hasta que haya transcurrido timeToTurnLeft (1,1 segundos) {/ / girar a la izquierda rightMotor.write(180); leftMotor.write(180);} estado = DRIVE_FORWARD; } }
void readUltrasonicSensors() {/ / ultrasonido 2 digitalWrite (ultrasonic2TrigPin, HIGH); delayMicroseconds(10); / / debe tener el pin trig alto para por lo menos 10us digitalWrite (ultrasonic2TrigPin, LOW); ultrasonic2Duration = pulseIn (ultrasonic2EchoPin, HIGH); ultrasonic2Distance = (ultrasonic2Duration/2) / 29;}
void debugOutput() {if((millis() - timeSerialDelay) > serialPeriod) {Serial.print ("ultrasonic2Distance:"); Serial.Print(ultrasonic2Distance); Serial.Print("cm"); Serial.println(); timeSerialDelay = millis(); } }
