Hello again, this is the code for my LFR robot. I published a video of it some days ago (
http://www.societyofrobots.com/robotforum/index.php?topic=14417.0). So if any of you have any comments on it, it would be great.
#include <Servo.h>
#include <LED.h>
#include <Button.h>
Servo RS; // right servo
Servo LS; // left servo
Servo HS; // head servo
LED led = LED(13);
Button RB = Button(4, PULLDOWN); // right bumper
Button LB = Button(2, PULLDOWN); // left bumper
int HLDR = 1; // head LDR pin
int RBval = 0;
int LBval = 0;
int HLDRval;
int LDRscanVal[2] = {0, 0}; // {Left, Right}
int RSzero = 97; // righ servo zero position
int LSzero = 72; // left servo zero position
int HSzero = 81; // head servo zero position
int maxSpeed = 20;
int minSpeed = 2;
int Lspeed = 0;
int Rspeed = 0;
int dir; //direction
////// TIMMING DATA //////
// startHeadScanning() timming and vars
// head servo
long headDelay = 505; // move head every millisecunds
long headMoveTime; // next time to move the head
int LookAngle = 35; // turn angle of a head (Left == + ; Right == -)
// head LDR
long LDRreadDelay = 455; // secunds to delay reading of LDR after head was moved
long LDRreadTime = 0; // next time to read the LDR
boolean LDRreadGo = false; // if true: head is not moving
// BumperHit timming and vars
long backupDelay = 1000;
long backupTime = 0;
boolean avoid = false;
////////////// SETUP //////////////
void setup() {
Serial.begin(9600);
randomSeed(analogRead(0));
RS.attach(6); // attaches right servo on pin 6
LS.attach(5); // attaches left servo on pin 5
HS.attach(3); // attach head servo on pin 3
RS.write(RSzero); // center all servos
LS.write(LSzero); //
HS.write(HSzero); //
led.blink(1000);
led.off();
delay(1000);
}
////////////// END SETUP //////////////
////////////// LOOP //////////////
void loop() {
led.on();
// Nothing hit, keep driving
if (noneHit() && !avoid) {
startHeadScanning();
followEmpty();
Serial.println("noneHit == 1");
}
// Something hit, stop and think about it
else if (anyHit() && !avoid) {
goZero();
delay(250); //???????????
dir = think();
avoid = true;
backupTime = millis() + backupDelay;
Serial.println("anyHit == 1");
}
// Avoid the obsticle, based on thinking
else if (avoid) {
Avoid();
}
goMove(Lspeed, Rspeed);
Serial.print(Lspeed); Serial.print(", ");Serial.println(Rspeed);
}
////////////// END LOOP //////////////
////////////////////////////////////////
void Avoid() {
if (millis() <= backupTime) {
if (dir == 0) {
Lspeed = -maxSpeed;
Rspeed = -maxSpeed + 13;
}
else if (dir == 1) {
Lspeed = -maxSpeed + 13;
Rspeed = -maxSpeed;
}
else if (dir == 2) {
Lspeed = -maxSpeed;
Rspeed = -maxSpeed;
}
}
else {avoid = false;}
Serial.println("avoid == 1");
}
int think() {
int opt[2] = {0,0}; // L & R free
// acquire and process data
int LDRcenter = readLDRavr(5);
int Lval = LDRscanVal[0] - LDRcenter;
int Rval = LDRscanVal[1] - LDRcenter;
// brain
if (leftHit()) {opt[1] = opt[1]+1;}
if (rightHit()) {opt[0] = opt[0]+1;}
if (LDRscanVal[0] > LDRcenter) {opt[0] = opt[0]+1;}
if (LDRscanVal[1] > LDRcenter) {opt[1] = opt[1]+1;}
if (LDRscanVal[0] < LDRcenter) {opt[1] = opt[1]+1;}
if (LDRscanVal[1] < LDRcenter) {opt[0] = opt[0]+1;}
if (Lval > Rval) {opt[0] = opt[0]+1;}
if (Lval < Rval) {opt[1]= opt[1]+1;}
// add randomness
int rrr = random(0,2);
opt[rrr] = opt[rrr] + random(0,2);
if (opt[0] > opt[1]) {return 0;} // 0 == Left
else if (opt[0] < opt[1]) {return 1;} // 1 == Right
else if (opt[0] == opt[1]) {return 2;} // 2 == Straight back
}
void followEmpty() {
if (LDRscanVal[0] > LDRscanVal[1]) {
Lspeed = maxSpeed - 13;
Rspeed = maxSpeed;
}
else if (LDRscanVal[0] < LDRscanVal[1]) {
Lspeed = maxSpeed;
Rspeed = maxSpeed - 13;
}
else {
Lspeed = maxSpeed;
Rspeed = maxSpeed;
}
}
void startHeadScanning() {
// move head
if (millis() >= headMoveTime) {
lookAngle(LookAngle);
headMoveTime = millis() + headDelay;
LDRreadTime = millis() + LDRreadDelay;
LDRreadGo = true;
LookAngle = LookAngle * -1;
}
// read LDR
if (millis() >= LDRreadTime && LDRreadGo) {
// if looking to the right
if (LookAngle > 0) {
LDRscanVal[1] = readLDRavr(5);
}
// if looking to the left
else if (LookAngle < 0) {
LDRscanVal[0] = readLDRavr(5);
}
LDRreadGo = false;
}
}
///////////////////////////////////////////////////
/////////////// ROBOT FUNCTIONS ///////////////////
///////////////////////////////////////////////////
////// ROBOT MOTION //////
void goMove(int Lspeed, int Rspeed) {
LS.write(LSzero - Lspeed);
RS.write(RSzero + Rspeed);
}
void goStop() {
LS.write(LSzero);
RS.write(RSzero);
}
void goZero() {
LS.write(LSzero);
RS.write(RSzero);
HS.write(HSzero);
}
////// HEAD MOTION ///////
void lookForward() {
HS.write(HSzero);
}
void lookAngle(int angle) {
HS.write(HSzero + angle);
}
////////////////////////////////////////////////////
////////////////// SENSOR FUNCTIONS ////////////////
////////////////////////////////////////////////////
////// BUMPERS ///////
int rightHit() {
if (RB.isPressed()) {return true;}
else {return false;}
}
boolean leftHit() {
if (LB.isPressed()) {return true;}
else {return 0;}
}
boolean anyHit() {
if (rightHit() || leftHit()) {return true;}
else {return false;}
}
boolean noneHit() {
if (!rightHit() && !leftHit()) {return true;}
else {return false;}
}
boolean allHit() {
if (rightHit() && leftHit()) {return true;}
else {return false;}
}
////// HEAD LDR ///////
int readLDR() {
int val = analogRead(HLDR);
return val;
}
int readLDRavr(int n) {
int val = 0;
for (int i=0; i<n; i++) {
val += readLDR();
}
return val/n;
}