Roving robot

Forum 2005-2010 (read only) General Exhibition
11 
#define LeftWheelPin 6
#define RightWheelPin 5
#define ArmServoPin 4
#define SonarPin 0

#define runningLED 2
#define errorLED 7

short ArmAngle, dAngleMax,dAngleMin, dAngleMaxTemp, dAngleMinTemp;
short ArmAngleRange, ArmAngleTarget;
short LeftSpeed;
short RightSpeed;
short ArmTrim = 0;
int distance, dmax, dmin, dmaxtemp, dmintemp;

unsigned long lastPulseL;
unsigned long lastPulseR;
unsigned long lastPulseA;

int i, k;
unsigned short refreshTime = 15;
unsigned long timeLast, timeLastFast;
unsigned long timeLastSlow;
unsigned int timeStart;

// --------------------------------------------------- LED flashing 
void flashLED(short pin, short times) {
  for (i = 0; i<=times; i++) {
    digitalWrite(pin, HIGH);
    delay(100);
    digitalWrite(pin, LOW);
    delay(500);  
  }
} 

// ---------------------------------------------------- Refreshes all servo pulses
void updateServos() {

  if (millis() - lastPulseL >= refreshTime) {
    digitalWrite(LeftWheelPin, HIGH);
    delayMicroseconds((LeftSpeed+90)*11+500);
    digitalWrite(LeftWheelPin, LOW);
    lastPulseL = millis();
  }

  if (millis() - lastPulseR >= refreshTime) {
    digitalWrite(RightWheelPin, HIGH);
    delayMicroseconds((-RightSpeed+90)*11+500);
    digitalWrite(RightWheelPin, LOW);
    lastPulseR = millis();
  }

  if (millis() - lastPulseA >= refreshTime) {
    digitalWrite(ArmServoPin, HIGH);
    delayMicroseconds((ArmAngle+ArmTrim+90)*11+500);
    digitalWrite(ArmServoPin, LOW);
    lastPulseA = millis();
  }

} // end updateServos

// ----------------------------------------------------- Rover move and turn function
void move(int speed, int turnSpeed) {
  //LeftSpeed = speed - turnSpeed;
  //RightSpeed = speed + turnSpeed;

  if (LeftSpeed < 30 && LeftSpeed < (speed - turnSpeed)) {
    LeftSpeed++;
    //LeftSpeed = ((LeftSpeed*10)+1)/10
  }
  else if (LeftSpeed > -30 && LeftSpeed > (speed - turnSpeed)) {
    LeftSpeed--;
    //LeftSpeed = ((LeftSpeed*10)-1)/10;
  }

  if (RightSpeed < 30 && RightSpeed < (speed + turnSpeed)) {
    RightSpeed++;
  }
  else if (RightSpeed > -30 && RightSpeed > (speed + turnSpeed)) {
    RightSpeed--;
  }

  //if (speed == 0 && turnSpeed == 0) {
  //  LeftSpeed = 0;
  //  RightSpeed = 0;
  //}
}    // End move

// -------------------------------------------------- Update ArmAngle
void armturn() {

  if (ArmAngle < 90 && ArmAngle < ArmAngleTarget) {
    ArmAngle = ArmAngle ++;
  }
  else if (ArmAngle > -90 && ArmAngle > ArmAngleTarget) {
    ArmAngle = ArmAngle --;
  }
}

// ------------------------------------------------- sonar distance function
long ping() {
  long raw;

  raw = analogRead(SonarPin)+analogRead(SonarPin)+analogRead(SonarPin)+analogRead(SonarPin);
  raw = raw/4;
  return raw + raw*123/512;
}


// -------------------------------------------------- Main navigation function 
void roving() {
  int speed, turn;
  randomSeed(millis());
  
  if (dmax > 300 && abs(dAngleMax) < 10) {
    ArmAngleRange = 15;
    speed = 20;
    turn = -dAngleMax;
  }    
  else if (dmax > 60) {
    ArmAngleRange = 60;
    speed = 16;
    turn = -dAngleMax/4;
  }  
  else if (dmax > 30) {
    ArmAngleRange = 70;
    speed = dmax/5 - 6;
    turn = -dAngleMax/24;
  }
  else if (dmax <= 30) {
    ArmAngleRange = 80;
    speed = 0;
    turn = -dAngleMax/20;
    digitalWrite(runningLED, HIGH);
  }
  if ((dmin <= 19)&&(abs(dAngleMin)<60)) {
    ArmAngleRange = 80;
    digitalWrite(errorLED, HIGH);
    speed = -4;
    if (dAngleMin !=0) {
      turn = dAngleMin/20;
    }
    else {
      if (random(0,1)) {
        turn = -2;
      }
      else {
        turn = 2;
      }
    }
  }
  /*
  else {
    move(0,0);
    flashLED(errorLED,5);
  }
  */
  move(speed, turn);
}  


void setup() {
  pinMode(LeftWheelPin, OUTPUT);
  pinMode(RightWheelPin, OUTPUT);
  pinMode(ArmServoPin, OUTPUT);
  pinMode(errorLED, OUTPUT);
  pinMode(runningLED, OUTPUT);
  pinMode(SonarPin, INPUT);
  Serial.begin(9600);

  LeftSpeed = 0;
  RightSpeed = 0;
  ArmAngleRange = 80;
  ArmAngleTarget = 80;
  distance = 50;
  dmaxtemp = 20;
  dmintemp = 1000;

  flashLED(runningLED,5);

  timeStart = millis();

  move(0,0);
}

// -------------------------------------------------- Main Loop
void loop() {

  digitalWrite(errorLED, LOW); 

  if (millis() - timeLast > 100) {
    distance = ping();
    
    
    if (distance < dmintemp) {
      dmintemp = distance;
      dAngleMinTemp = ArmAngle;
    }
    if (distance > dmaxtemp) {
      dmaxtemp = distance;
      dAngleMaxTemp = ArmAngle;
    }
    roving();   
    timeLast = millis();
    updateServos();
  }

  if ((millis() - timeLastSlow > 500) && (abs(ArmAngle) >= abs(ArmAngleTarget))) {
    if (ArmAngleTarget > 0) {    
      ArmAngleTarget = ArmAngleRange*-1;
    }
    else {
      ArmAngleTarget = ArmAngleRange;
    }
    dAngleMax = dAngleMaxTemp;
    dAngleMin = dAngleMinTemp;
    
    dmax = dmaxtemp;
    dmin = dmintemp;
    dmaxtemp = 10;
    dmintemp = 1000;
    
    /*
    Serial.print("Angles: ");
    Serial.print(dAngleMax,DEC);
    Serial.print(", ");
    Serial.print(dAngleMin,DEC);
    Serial.print(" - Distances: ");
    Serial.print(dmax,DEC);
    Serial.print(", ");
    Serial.println(dmin,DEC);
    */
    timeLastSlow = millis();
    updateServos();
  }

  if (millis() - timeLastFast > 4) { 
    armturn();
    timeLastFast = millis();
  }


  //updateServos();
  //Serial.println(distance,DEC); 
  digitalWrite(runningLED, LOW);

  updateServos();
}