Paso 6: El código
Aquí está el código que se utiliza con el Robot (está escrito en C y debe desplegarse en el Arduino):
#include "DualMC33926MotorShield.h"
#include "Servo.h"
#define IR_PIN A4
#define SERVO_PIN A2
#define PING_PIN 5
#define BUMPER_LEFT_PIN 11
#define BUMPER_RIGHT_PIN 6
#define LDR_PIN A5
#define LED_PIN 3
#define IR_DROP 450 //Distance considera una caída de potencial
#define MIN_LIGHT 300 //Level de la luz para encender el LED para la luz
DualMC33926MotorShield md;
const int M1Speed = 300;
const int M2Speed = 300;
int IRDist = 0;
int bpLeft = 0;
int bpRight = 0;
int LDRValue = 0;
const int dangerThresh = 12; / / (en cm) se utiliza para la evitación del obstáculo
int leftDistance, rightDistance; Distancias a ambos lados
Servo panMotor; servo 'cuello'
de larga duración; tiempo que tarda en recibir la señal del PING
void setup()
{
pinMode(BUMPER_LEFT_PIN,INPUT);
pinMode(BUMPER_RIGHT_PIN,INPUT);
pinMode (LED_PIN, salida);
}
void moveStop()
{
MD.init();
md.setSpeeds(0,0);
}
void moveForward()
{
MD.init();
moveStop();
md.setSpeeds(M1Speed,M2Speed);
Delay(250);
}
turnLeft() vacío
{
MD.init();
moveStop();
md.setSpeeds(M1Speed,-M2Speed);
Delay(250);
}
turnRight() vacío
{
MD.init();
moveStop();
md.setSpeeds(-M1Speed,M2Speed);
Delay(250);
}
void moveBack()
{
MD.init();
moveStop();
md.setSpeeds(-M1Speed,-M2Speed);
Delay(250);
}
int checkDrop()
{
pinMode(IR_PIN,INPUT);
IRDist = analogRead(IR_PIN);
if(IRDist < IR_DROP)
{
retorno 1; Caída actual (determinada con sensor de distancia infrarrojo)
}
otra cosa
{
return 0;
}
}
void lightDarkness()
{
LDRValue = analogRead(LDR_PIN);
if(LDRValue < MIN_LIGHT)
{
digitalWrite(LED_PIN,HIGH); Es oscura, encender la luz
}
otra cosa
{
digitalWrite(LED_PIN,LOW);
}
}
int checkLeftBumper()
{
bpLeft = digitalRead(BUMPER_LEFT_PIN);
if(bpLeft == High)
{
¡ HIT!
retorno 1;
}
otra cosa
{
return 0;
}
}
int checkRightBumper()
{
bpRight = digitalRead(BUMPER_RIGHT_PIN);
if(bpRight == High)
{
¡ HIT!
retorno 1;
}
otra cosa
{
return 0;
}
}
void compareDistance() {}
Si (leftDistance > rightDistance) //if izquierda es menos obstruida
{
turnLeft();
}
else if (rightDistance > leftDistance) //if derecho es menos obstruida
{
turnRight();
}
else //if que igualmente se obstruyeron
{
moveBack();
}
}
be larga
{
Enviar pulsos de Ping
pinMode (PING_PIN, salida);
digitalWrite (PING_PIN, bajo);
delayMicroseconds(2);
digitalWrite (PING_PIN, alto);
delayMicroseconds(5);
digitalWrite (PING_PIN, bajo);
Conseguir la duración que tarda en recibir eco de Ping enviado
pinMode (PING_PIN, entrada);
duración = pulseIn (PING_PIN, alto);
Duración a convertir distancia (cm)
vuelta duración / 29 / 2;
}
void loop()
{
lightDarkness();
if(checkDrop() == 1)
{
moveBack();
compareDistance();
}
otra cosa
if(checkLeftBumper() == 1)
{
moveBack();
compareDistance();
}
otra cosa if(checkRightBumper() == 1)
{
moveBack();
compareDistance();
}
otra cosa
{
moveStop();
int distanceFwd = be;
Si (distanceFwd > dangerThresh) //if camino es claro
{
moveForward();
}
else //if camino es bloqueado
{
moveStop();
panMotor.attach(SERVO_PIN);
panMotor.write(20); mira a la derecha
Delay(250);
rightDistance = be; analizar a la derecha
panMotor.write(160); mirar a la izquierda
Delay(600);
leftDistance = be; exploración a la izquierda
panMotor.write(90); mirada hacia el centro
Delay(300);
panMotor.detach();
compareDistance();
}
}
}
