Étape 6: Le Code
Voici le code utilisé avec le Robot (c’est écrit en C et doit être déployée sur l’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 considéré comme une chute de tension
#define MIN_LIGHT 300 //Level de la lumière pour allumer les LED pendant light
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) utilisé pour l’évitement d’obstacles
int leftDistance, rightDistance ; distance de chaque côté
Servo panMotor ; servo de « cou »
longue durée ; temps que nécessaire pour recevoir le signal de PING
void setup()
{
pinMode(BUMPER_LEFT_PIN,INPUT) ;
pinMode(BUMPER_RIGHT_PIN,INPUT) ;
pinMode (LED_PIN, sortie) ;
}
Sub moveStop()
{
MD.init() ;
md.setSpeeds(0,0) ;
}
Sub moveForward()
{
MD.init() ;
moveStop() ;
md.setSpeeds(M1Speed,M2Speed) ;
Delay(250) ;
}
Sub turnLeft()
{
MD.init() ;
moveStop() ;
md.setSpeeds(M1Speed,-M2Speed) ;
Delay(250) ;
}
Sub turnRight()
{
MD.init() ;
moveStop() ;
md.setSpeeds(-M1Speed,M2Speed) ;
Delay(250) ;
}
Sub moveBack()
{
MD.init() ;
moveStop() ;
md.setSpeeds(-M1Speed,-M2Speed) ;
Delay(250) ;
}
int checkDrop()
{
pinMode(IR_PIN,INPUT) ;
IRDist = analogRead(IR_PIN) ;
if(IRDist < IR_DROP)
{
Return 1 ; Laisser tomber présents (déterminé avec capteur de distance infrarouge)
}
d’autre
{
return 0 ;
}
}
Sub lightDarkness()
{
LDRValue = analogRead(LDR_PIN) ;
if(LDRValue < MIN_LIGHT)
{
digitalWrite(LED_PIN,HIGH) ; Elle est sombre, allumez la lumière
}
d’autre
{
digitalWrite(LED_PIN,LOW) ;
}
}
int checkLeftBumper()
{
bpLeft = digitalRead(BUMPER_LEFT_PIN) ;
if(bpLeft == High)
{
HIT !
Return 1 ;
}
d’autre
{
return 0 ;
}
}
int checkRightBumper()
{
bpRight = digitalRead(BUMPER_RIGHT_PIN) ;
if(bpRight == High)
{
HIT !
Return 1 ;
}
d’autre
{
return 0 ;
}
}
void compareDistance() {}
Si (leftDistance > rightDistance) //if gauche est moins obstruée
{
turnLeft() ;
}
ElseIf (rightDistance > leftDistance) //if droit est moins obstruée
{
turnRight() ;
}
d’autre //if qu'ils sont tout aussi obstrués
{
moveBack() ;
}
}
ping() longue
{
Envoyer des impulsions de Ping
pinMode (PING_PIN, sortie) ;
digitalWrite (PING_PIN, basse) ;
delayMicroseconds(2) ;
digitalWrite (PING_PIN, élevé) ;
delayMicroseconds(5) ;
digitalWrite (PING_PIN, basse) ;
Obtenir la durée qu’il faut pour recevoir écho de Ping envoyés
pinMode (entrée, PING_PIN) ;
durée = pulseIn (PING_PIN, élevé) ;
Convertir la durée en distance (cm)
durée de retour / 29 / 2 ;
}
void loop()
{
lightDarkness() ;
if(checkDrop() == 1)
{
moveBack() ;
compareDistance() ;
}
d’autre
if(checkLeftBumper() == 1)
{
moveBack() ;
compareDistance() ;
}
d’autre if(checkRightBumper() == 1)
{
moveBack() ;
compareDistance() ;
}
d’autre
{
moveStop() ;
int distanceFwd = ping() ;
Si (distanceFwd > dangerThresh) //if chemin est clair
{
moveForward() ;
}
d’autre //if chemin est bloqué
{
moveStop() ;
panMotor.attach(SERVO_PIN) ;
panMotor.write(20) ; regarder à droite
Delay(250) ;
rightDistance = ping() ; Scan vers la droite
panMotor.write(160) ; Regardez à gauche
Delay(600) ;
leftDistance = ping() ; numérisation vers la gauche
panMotor.write(90) ; coup d’oeil vers le centre
Delay(300) ;
panMotor.detach() ;
compareDistance() ;
}
}
}