Stap 6: De Code
Hier is de code die wordt gebruikt met de Robot (het is geschreven in C en moet worden ingezet op de 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 beschouwd als een mogelijke daling
#define MIN_LIGHT 300 //Level van licht om te schakelen LED voor licht
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; / / (in cm) gebruikt voor het obstakel te vermijden
int leftDistance, rightDistance; afstanden aan weerszijden
Servo panMotor; 'nek' servo
lange duur; tijd die het duurt om een PING-signaal ontvangt
VOID Setup
{
pinMode(BUMPER_LEFT_PIN,INPUT);
pinMode(BUMPER_RIGHT_PIN,INPUT);
pinMode (LED_PIN, uitvoer);
}
VOID moveStop()
{
MD.init();
md.setSpeeds(0,0);
}
VOID moveForward()
{
MD.init();
moveStop();
md.setSpeeds(M1Speed,M2Speed);
delay(250);
}
VOID turnLeft()
{
MD.init();
moveStop();
md.setSpeeds(M1Speed,-M2Speed);
delay(250);
}
VOID turnRight()
{
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)
{
return 1; Drop aanwezig (bepaald met IR-afstandssensor)
}
anders
{
keren 0;
}
}
VOID lightDarkness()
{
LDRValue = analogRead(LDR_PIN);
if(LDRValue < MIN_LIGHT)
{
digitalWrite(LED_PIN,HIGH); Het is donker, het licht inschakelen
}
anders
{
digitalWrite(LED_PIN,LOW);
}
}
int checkLeftBumper()
{
bpLeft = digitalRead(BUMPER_LEFT_PIN);
if(bpLeft == High)
{
HIT!
return 1;
}
anders
{
keren 0;
}
}
int checkRightBumper()
{
bpRight = digitalRead(BUMPER_RIGHT_PIN);
if(bpRight == High)
{
HIT!
return 1;
}
anders
{
keren 0;
}
}
ongeldig compareDistance() {}
Als (leftDistance > rightDistance) //if links minder wordt belemmerd
{
turnLeft();
}
anders als (rightDistance > leftDistance) //if recht minder wordt belemmerd
{
turnRight();
}
anders //if die ze even zijn geblokkeerd
{
moveBack();
}
}
lange ping()
{
Verzenden van de Ping pulse
pinMode (PING_PIN, uitvoer);
digitalWrite (PING_PIN, laag);
delayMicroseconds(2);
digitalWrite (PING_PIN, hoge);
delayMicroseconds(5);
digitalWrite (PING_PIN, laag);
Duur duurt te ontvangen krijgen echo voor Ping verzonden
pinMode (PING_PIN, INPUT);
duur = pulseIn (PING_PIN, hoge);
Duur converteren naar afstand (cm)
retourneren van duur / 29 / 2;
}
void loop
{
lightDarkness();
if(checkDrop() == 1)
{
moveBack();
compareDistance();
}
anders
if(checkLeftBumper() == 1)
{
moveBack();
compareDistance();
}
anders if(checkRightBumper() == 1)
{
moveBack();
compareDistance();
}
anders
{
moveStop();
int distanceFwd = ping();
Als (distanceFwd > dangerThresh) //if pad is duidelijk
{
moveForward();
}
anders //if pad is geblokkeerd
{
moveStop();
panMotor.attach(SERVO_PIN);
panMotor.write(20); ziet er goed uit
delay(250);
rightDistance = ping(); scannen naar rechts
panMotor.write(160); Kijk links
delay(600);
leftDistance = ping(); scannen naar links
panMotor.write(90); kijken naar het centrum
delay(300);
panMotor.detach();
compareDistance();
}
}
}
