/*  Evil robot brain! Now with spin and error detect!  */
/* LiPo batt: Max:859(4.2v) Nom:747(3.7v) Min:613(3.0v)  */

int E1 = 6; //M1 Speed Control
int E2 = 5; //M2 Speed Control
int M1 = 8; //M1 Direction Control
int M2 = 7; //M2 Direction Control
const int bumpPin = 12;     // the number of the pushbutton pin
const int m1_max=255;          // Motor speeds. M1 is right, M2 is left.
const int m2_max=250;          // m2 is slightly faster, so reduse top speed.
int avoid=0;
int boterror=0;
unsigned long lastbump;

void setup()
{
pinMode(9, OUTPUT);            // Speeker 'enable'
pinMode(13, OUTPUT);		// Avoid status LED
pinMode(bumpPin, INPUT);	// Bump sensor
analogWrite (E2,m2_max);
analogWrite (E1,m1_max);
// Serial.begin(9600);            // For debug
lastbump=millis();                // Setup non-bump-detect.

}
void loop()
{
if (digitalRead(bumpPin) == HIGH) {		// Have we hit somthing?
  avoid=1;			// Yes! Avoid it!
}

if (avoid == 1) {
  digitalWrite(13, HIGH);	// Avoid start

  digitalWrite(M1,HIGH);
  digitalWrite(M2,HIGH);	// Reverse robot for 1 second
  delay(1000);	

  digitalWrite(M2,LOW);  
  delay(random(100,1000));	// One motor on, one motor off. **SPIN!!**
  digitalWrite(M2,HIGH);

//  analogWrite (E2,0);		// Pretend to be a roomba; 
//  delay(random(200,5000));	// Switch off one motor for a random 
//  analogWrite (E2,m2_max);	// Number of seconds.

  avoid=0;
  digitalWrite(13, LOW);	// Avoid done
  lastbump=millis();            // 'reset' bump timer.
}
else {				// No? Then keep going, full speed ahead!
  digitalWrite(M1,LOW);
  digitalWrite(M2,LOW);
}

// Battery protect
//   Serial.println(analogRead(A0), DEC);
if (analogRead(A0) <= 650 ) {  // If batt is lower than 3.0v
 delay(50);
 if (analogRead(A0) <= 650 ) {      // Delay then try again. Just to be sure.
   boterror=1;                  // If it's still a problem, toggle flag.
 }
}

// Stuck detect.  No bumps for 30 seconds
if ((millis() - lastbump)  >= 30000) {
  boterror=2;                  // So you know this triggerd it
}

if ( boterror != 0 ) {  // We've had an error! Kill the 'bot
  analogWrite (E2,0);        // Switch off motors
  analogWrite (E1,0);
  
  digitalWrite(9,HIGH);        // Enable speeker
  while(1==1) {                // -- Start forever --
    digitalWrite(13, HIGH);
    tone(10,800);              // Beep and let the world know!
    delay(500);
    noTone(10);
  
    int flashcount=boterror;  // Flash the error code
    while(flashcount>0){
      digitalWrite(13, LOW);
      delay(100);
      digitalWrite(13, HIGH);
      delay(300);
      flashcount--;
    }
  }                          // -- End forever --
}


}   // End of program