Arduino code for LFR - Lesko's first robot

[Lesko]

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.

Code: [Select]

#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;
}