ROAMING ROBOT (6 / 7 steg)
Steg 6: Koden
Här är koden som används med Robot (det är skrivet i C och måste distribueras på 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 betraktas som en potentiell nedgång
#define MIN_LIGHT 300 //Level av ljus för att aktivera LED för ljus
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; / / (i cm) används för hinder skatteundandragande
int leftDistance, rightDistance; avstånd på vardera sidan
Servo panMotor; "nacke" servo
lång varaktighet; tid det tar för att få PING signal
void setup()
{
pinMode(BUMPER_LEFT_PIN,INPUT);
pinMode(BUMPER_RIGHT_PIN,INPUT);
pinMode (LED_PIN, OUTPUT);
}
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)
{
tillbaka 1. Släpp nuvarande (bestäms med IR-sensorn som avstånd)
}
annat
{
Return 0;
}
}
void lightDarkness()
{
LDRValue = analogRead(LDR_PIN);
IF(LDRValue < MIN_LIGHT)
{
digitalWrite(LED_PIN,HIGH); Det är mörkt, slå på ljuset
}
annat
{
digitalWrite(LED_PIN,LOW);
}
}
int checkLeftBumper()
{
bpLeft = digitalRead(BUMPER_LEFT_PIN);
IF(bpLeft == High)
{
HIT!
tillbaka 1.
}
annat
{
Return 0;
}
}
int checkRightBumper()
{
bpRight = digitalRead(BUMPER_RIGHT_PIN);
IF(bpRight == High)
{
HIT!
tillbaka 1.
}
annat
{
Return 0;
}
}
void compareDistance() {
om (leftDistance > rightDistance) //if vänster hindras mindre
{
turnLeft();
}
annars om (rightDistance > leftDistance) //if höger hindras mindre
{
turnRight();
}
annat //if de hindras lika
{
moveBack();
}
}
lång ping()
{
Skicka Ping puls
pinMode (PING_PIN, OUTPUT);
digitalWrite (PING_PIN, låg);
delayMicroseconds(2);
digitalWrite (PING_PIN, hög);
delayMicroseconds(5);
digitalWrite (PING_PIN, låg);
Få tid det tar för att få eko av Ping skickas
pinMode (PING_PIN, indata);
längd = pulseIn (PING_PIN, hög);
Konvertera varaktighet till avstånd (cm)
returnera varaktighet / 29 / 2;
}
void loop()
{
lightDarkness();
IF(checkDrop() == 1)
{
moveBack();
compareDistance();
}
annat
IF(checkLeftBumper() == 1)
{
moveBack();
compareDistance();
}
annat if(checkRightBumper() == 1)
{
moveBack();
compareDistance();
}
annat
{
moveStop();
int distanceFwd = ping();
om (distanceFwd > dangerThresh) //if sökväg är klart
{
moveForward();
}
annat //if väg är blockerad
{
moveStop();
panMotor.attach(SERVO_PIN);
panMotor.write(20); ser rätt
Delay(250);
rightDistance = ping(); Skanna till höger
panMotor.write(160); Titta vänster
Delay(600);
leftDistance = ping(); Skanna till vänster
panMotor.write(90); se till att centrum
Delay(300);
panMotor.detach();
compareDistance();
}
}
}