/*  Evil robot brain! Finite state machine version.  */
/* Accept comms over serial link. Non-blocking waits. Manual overrides, multi-sensors, averaging reads  */
/* And everything is in functions!   Bitwise motor controll and sensors.  And main loop profiling.  */
/* Semi-ingeligant object avoidtance. Digital and analog collision decoding logic.  */
/*  SIZE:  6.5k of 30.7k max = 21%                        */
// Binary sketch size: 7406 bytes (of a 30720 byte maximum)

/* FSM  states */
#define state_pause        0
#define state_move         1
#define state_explore      2
#define state_sequence     3
#define state_error        127  

/* Directions */
#define dir_none       0
#define dir_forward    4
#define dir_forwleft   5
#define dir_forwright  6
#define dir_back       8
#define dir_backleft   9   
#define dir_backright  10
#define dir_spinleft   13
#define dir_spinright  14

/* Collisions - status bits*/       
#define coll_frontleft    1
#define coll_frontright   2     // 3 = front square collision
#define coll_rearleft     4
#define coll_rearright    8     // 12  = reverse square collision
#define coll_analog       16    // Triggerd by analog distance sensors
#define coll_angle        32    // Triggerd by accelromiter

/* Motor controll pins. */
const byte LeftSpeedPin = 6; //M1 Speed Control
const byte RightSpeedPin = 5; //M2 Speed Control
const byte LeftDirPin = 8; //M1 Direction Control
const byte RightDirPin = 7; //M2 Direction Control

const byte LeftEncoderPin = 11;
const byte RightEncoderPin = 12;
byte LeftEncoderState;
byte RightEncoderState;
int LeftEncoderCount;
int RightEncoderCount;


/* Motor speeds */
byte MRight_max=255;
byte MLeft_max=255;          // Left slightly faster, so reduse top speed.

/* Main blue LEDs, inverted. HIGH = off */
const byte ledPin = 13;    // Flashing LEDs!

/* Battery monitoring */
const byte batt1Pin = A6;
int batt1Count=0;
byte batt1State=0;
const byte batt2Pin = A7;
int batt2Count=0;
byte batt2State=0;

/* "User" buttons with LEDs */
const byte button1Pin = 4;
const byte button1Led = 3;
byte button1State=1;
byte button1Count=10;

/* "User" buttons with LEDs */
//const byte button2Pin = 11;
//byte button2State=1;
//byte button2Count=10;
//const byte button3Pin = 12;



/* Edge detection, IR distance, digital */
const int edgeSenstivity = 100;
const byte edge1Pin = 2;  // First/Right
byte edge1State=0;
byte edge1Count=5;
const byte edge2Pin = 9;   // Second/Left
byte edge2State=0;
byte edge2Count=5;
const byte edge3Pin = 10;   // Third/Back
byte edge3State=0;
byte edge3Count=5;

/* IR or Sonic distance sensors, analog */
const byte distThreshold = 1;   // 'Is a bump' threshold
const byte dist1Pin = A0;       // Right Front IR
byte dist1Value=0;
const byte dist2Pin = A1;       // Left Front IR
byte dist2Value=0;

/* State machine controlls */
byte state=state_pause;                  // Make sure the 'bot is in a known state 
byte laststate=state_pause;
byte setupstate=state_pause;

/* Movement and avoidance  */
unsigned long movecount=0;           // How long to move for
byte movedir=0;                      // What direction to move
byte moveprev=0;
unsigned long lastbump;
byte avoid=0;
byte coll_status=0;      // Objet located.  Needs defines for direction.
byte coll_dist=0;        // Analog distance from object

//byte array[30];          // Array of 30 bytes, one for direction, two for distanc 

/* System counters. */
unsigned long movetimer=0;
unsigned long sensortimer=0;
unsigned long debugtimer=0;

/* Profiling counters  */
unsigned int profilecount=0;
unsigned int profilestore=0;

/* Error modes/settings  */
byte boterror=0;
byte flashcount;

/* Make sure all I/O is set up and ready to use. */ 
void setup() {
pinMode(9, OUTPUT);            // Speeker 'enable'

pinMode(ledPin, OUTPUT);		// Status LED
digitalWrite(ledPin, HIGH);              // Light off.

// Batteries
pinMode(batt1Pin,INPUT);      // Main
digitalWrite(batt1Pin, LOW);
pinMode(batt2Pin,INPUT);     // Motor
digitalWrite(batt2Pin, LOW);

// Motors stopped.
analogWrite (LeftSpeedPin,0);
analogWrite (RightSpeedPin,0);

// Wheel encoders
pinMode(LeftEncoderPin,INPUT);
digitalWrite(LeftEncoderPin, HIGH);
pinMode(RightEncoderPin,INPUT);
digitalWrite(RightEncoderPin, HIGH);

long lastbump=millis();                // Setup non-bump-detect.

int i;                                // Robot controlls.
for(i=5;i<=8;i++)
pinMode(i, OUTPUT);

pinMode(button1Pin,INPUT);           // 'user' button.
digitalWrite(button1Pin, HIGH);      // With pullup.
pinMode(button1Led,OUTPUT);          // And a built in LED!

//pinMode(button2Pin,INPUT);           // 'user' button.
//pinMode(button3Pin,INPUT);           // 'user' button.

pinMode(edge1Pin,INPUT);
pinMode(edge2Pin,INPUT);
pinMode(edge3Pin,INPUT);
digitalWrite(edge3Pin, HIGH);

Serial.begin(19200);                // Serial link
Serial.println("Robot started. Commands: asdw and er+-");  // Test link works.

}


/* Main loop. No blocking calls or commands allowed at all!! */
void loop() {

  if ((millis() - sensortimer)  >= 1) {        // Every 1mS             
    readsensors();                             // Read in all sensors
    colldetect();                              // Collision detection from sensors stuff.
    sensortimer=millis();
    // Once every mS lets save the profile count.
    profilestore=profilecount;
    profilecount=0;
  }

  // How many times around the main loop.
  profilecount++;
  
  if (Serial.available() != 0) {
    serialinput();                              // Deal with serial port input 
  }

  if ((millis() - debugtimer)  >= 1000) {
    Serial.print("DEBUG:");
    debugmessage();                            // Throw out debug message every so often, once a second.//
 //   debugtimer=millis();                     // Reset this in debugmessage()
  }

  if(setupstate != state){          // Keep an eye out for state changes.
    Serial.print("STATE:");
    debugmessage();
  }

  // Start main state machine selection.
  switch(state) {                  
    case state_pause:
      state=dostate_pause();
    break; 
    case state_move:
      state=dostate_move();
    break;
    case state_explore:
      state=dostate_explore();
    break;
    case state_sequence:
      state=dostate_sequence();
    break;
    case state_error:
      state=dostate_error(); 
    break;
    default:             // In an unknown state, force to known.
      Serial.println(") !!INAVALID STATE!! Setting state_pause:");
      Serial.print("      ");
      debugmessage();
      state=state_pause;
      laststate=state_pause; 
    break;
  }  // End state switch

}   // End of loop();


/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */

/* Read in sensors
Every 1ms, read in all sensors, each sensor has a countdown value and a 'next mode' value. On change of value, reset countdown,
set 'next mode'. On same value, decrement coutndown. when coutndown is zero(thus sensor has given same value severl times),
set 'sensor's new value'.  Analog values need an averaging somehow..
*/
void readsensors() {
  // button1State, button1Pin, button1Led 
  if (digitalRead(button1Pin) != button1State) {
    button1Count--;

    if(button1Count==0) {
      button1State=digitalRead(button1Pin);
      if(button1State==LOW){                      // When button is down, run toggle routine.
        if(state==state_pause) {
          state=state_explore;
          digitalWrite(button1Led, HIGH);
        }
        else {
          state=state_pause;
          digitalWrite(button1Led, LOW);
        }
      }
    }
  }
  else {
    button1Count=30;	
  }

//  Remote control buttons   
/*    if (digitalRead(button2Pin)) {
      digitalWrite(button1Led, HIGH);
      state=state_explore;
    }
    else if(digitalRead(button3Pin)) {
      digitalWrite(button1Led, LOW);
      state=state_pause;
    }
*/

// Wheel encoders
/* start counter -> ignore changes until counter reaches min amount
 look for transision, reset counter,  incriment 'moved onwards'
 Some sort of averaging?
*/


/* wait min time to allow for hestreasus, when next trigger event, save rotational speed counter.
  TODO: compare both wheels and adjust speeds to match.
*/
if(LeftEncoderCount > 5){
  if(LeftEncoderState != digitalRead(LeftEncoderPin)) {
    Serial.println(LeftEncoderCount,DEC);
    LeftEncoderState=digitalRead(LeftEncoderPin);
    LeftEncoderCount=0;
  }
  else {
    LeftEncoderCount++;
  }
}
else {
    LeftEncoderCount++;
}


/*
if(digitalRead(LeftEncoderPin) != current state){
  if(

 Serial.print(digitalRead(LeftEncoderPin));
byte LeftEncoderState;
byte RightEncoderState;  
  
*/


  // Edge detect 1 
  if (digitalRead(edge1Pin)==HIGH) {
    edge1Count--;
    if(edge1Count==0) {
      edge1State=HIGH;
    }
  }
    else {
      edge1Count=edgeSenstivity;
      edge1State=LOW;	
    }

  // Edge detect 2 
  if (digitalRead(edge2Pin) == HIGH) {
    edge2Count--;
    if(edge2Count==0) {
      edge2State=HIGH;
    }
  }
    else {
      edge2Count=edgeSenstivity;
      edge2State=LOW;	
    }
    
  // Edge detect 3 
  if (digitalRead(edge3Pin) == HIGH) {
      edge3Count--;
    if(edge3Count==0) {
      edge3State=HIGH;
    }
  }
    else {
      edge3Count=edgeSenstivity;
      edge3State=LOW;	
    }
  
  // Battery protect, only throw an error after more than 300ms at less than 3.0v
  // LiPo batt: Max:859(4.2v) Nom:747(3.7v) Min:613(3.0v)
  batt1State=map(constrain(analogRead(batt1Pin),613,900),613,900,1,10);
  if (batt1State <= 1 ) {
    batt1Count--;
    if(batt1Count==0) {
      boterror=1;                  // If it's still a problem, toggle flag.
      laststate=state;
      state=state_error;
    }
  }
    else {
      batt1Count=500;        // 300 ms	
    }

  batt2State=map(constrain(analogRead(batt2Pin),613,900),613,900,1,10);
  if (batt2State <= 1 ) {
    batt2Count--;
    if(batt2Count==0) {
      boterror=1;                  // If it's still a problem, toggle flag.
      laststate=state;
      state=state_error;
    }
  }
    else {
      batt2Count=500;        // 300 ms	
    }


  // Analong distance sensors.
  // Right IR
  dist1Value=map(analogRead(dist1Pin),100,600,1,10);
  // Left IR
  dist2Value=map(analogRead(dist2Pin),100,600,1,10);
}  // End readsensors()

/* ----------------------------------------------------------------------------------------------------------------------- */

// Collision, bump or object detection. Both hit somthing and from what direction.
// Needs to include OR for left + right at same time, ie, front square collision.
void colldetect() {
  // Blank to ensure clean start
  coll_status=0;
  coll_dist=0;
  
  // Digital sensors all OR-ed together.
  if(edge1State == HIGH) {      //  Right front edge
    coll_status=+coll_frontright;
  }
  if(edge2State == HIGH) {      //  Left front edge
    coll_status=+coll_frontleft;
  }  
//  if(edge3State == HIGH) {      //  Both rear edges
//    coll_status=+coll_rearleft+coll_rearleft;
//  }
  if(coll_status) {    // Don't even bother with analog sensors.
    coll_dist=0;        // Digital,  so no distance.
    return;            // We want to get away from the edge first!
  }
  
  // Analog sensors
  if(dist1Value>distThreshold || dist2Value>distThreshold){     // Somthing is infront of a sensor. Value sets 'senstivity'
    if(dist1Value>dist2Value){      // Is right closer than left?
      coll_status=+coll_frontright+coll_analog;   // Set right status
      coll_dist=dist1Value;          // And store sensor distance for same.
    }      
    else {
      coll_status=+coll_frontleft+coll_analog;  // Left status
      coll_dist=dist2Value;
    }
  }
  
} // end colldetect()

/* ----------------------------------------------------------------------------------------------------------------------- */

void serialinput() {
  char val = Serial.read();
  Serial.println(val, DEC);
switch(val) // Perform an action depending on the command
{
  case 'w'://Move Forward
    state=state_move;
    setupstate=state_pause;
    laststate=state_pause;
    movedir=dir_forward;
    movecount=10000;
  break;
  case 's': //Move Backwards
    state=state_move;
    setupstate=state_pause;
    laststate=state_pause;
    movedir=dir_back;
    movecount=10000;
  break;
  case 'a'://Turn Left
    state=state_move;
    setupstate=state_pause;
    laststate=state_pause;
    if(movedir==dir_forwleft) {
      movedir=movedir + 8;
    }
    else {
      movedir=dir_forwleft;
    }
    movecount=10000;
  break;
  case 'd'://Turn Right
    state=state_move;
    setupstate=state_pause;
    laststate=state_pause;
    if(movedir==dir_forwright) {
      movedir=movedir + 8;
    }
    else {
      movedir=dir_forwright;
    }
    movecount=10000;
  break;
  case 'e': // Explore
    state=state_explore;
  break;
  case 'r':  // Reset states and everything.
    lastbump=0;
    state=state_pause;
    laststate=state_pause;
    setupstate=state_pause;
    movecount=0;
    movedir=0;
    boterror=0;
    avoid=0;
    changedir(movedir); 
  break;
  case '+':
    MLeft_max++;
    analogWrite (LeftSpeedPin,MLeft_max);
  break;
  case '-':
    MLeft_max--;
    analogWrite (LeftSpeedPin,MLeft_max);
  break;
  default:
    state=state_pause;
  break;
  }
} // end serialinput();

/* ----------------------------------------------------------------------------------------------------------------------- */

// Control motors. Send the robot off in the right Speed and direction. Or stop it.
// All done bitwise. Blaim Issac. 0:Off, 1:Left 2:Right 4:Forward 8:Reverse 16+: 4 bits of speed. 
// To impliment: movedir -> moveprev 
void changedir(byte dir) {

  // LEDs on if theres somthing non-zero.
  if(dir) {
    digitalWrite(ledPin, LOW);
  }
  
  // Save current max speeds.
  byte MRight_speed=MRight_max;
  byte MLeft_speed=MLeft_max;

  // If speed control bytes are there, change max speeds.
  byte dirspeed=dir&B11110000;
  if(dirspeed) {
    MRight_speed=MRight_max/2;
    MLeft_speed=MLeft_max/2;
  }
  
  // Isolate track direction bytes, the two LSBs
  byte dirtrack=(dir&B00000011)^B00000011;
  
  // Motor direction controll. Off and Spin are exceptions.
  dir=dir&B00001100;  
  switch(dir) {            
    case B00000000:        // 0: Off
      analogWrite (LeftSpeedPin,0);        // Motors both off.
      analogWrite (RightSpeedPin,0);
      digitalWrite(ledPin, HIGH);          // LEDs off too.
      return;
    break;
    case B00000100:    // 3: Forward 
      digitalWrite(RightDirPin,LOW);
      digitalWrite(LeftDirPin,LOW); 
    break;
    case B00001000:   // 8: Reverse 
      digitalWrite(RightDirPin,HIGH);
      digitalWrite(LeftDirPin,HIGH);
    break;
    case B00001100:  // 11: Spin -- Anoying exception to clean logic
      analogWrite (LeftSpeedPin,MLeft_speed);              // Set speeds here.    
      analogWrite (RightSpeedPin,MRight_speed);            // We'll need to bail early.
      if(dirtrack&B00000001) {            // Spin Left
        digitalWrite(RightDirPin,HIGH);
        digitalWrite(LeftDirPin,LOW);
      }
      else {                              // Maybe right?
        digitalWrite(RightDirPin,LOW);
        digitalWrite(LeftDirPin,HIGH);
      }
      return;        // Leave before we get to speed and select logic.      
    break;
  } 
  
  //  What track has power, switch on/off as required.
  if(dirtrack&B00000001) {    // Left track logic
    analogWrite (LeftSpeedPin,MLeft_speed);    
  }
  else {
    analogWrite (LeftSpeedPin,0);
  }
  
  if(dirtrack&B00000010) {    // Right track logic
    analogWrite (RightSpeedPin,MRight_speed); 
  }
  else {
     analogWrite (RightSpeedPin,0);
  }

}  // end changedir();

/* ----------------------------------------------------------------------------------------------------------------------- */

void debugmessage() {
    Serial.print("(");
  Serial.print(millis(),HEX);

  Serial.print(")(");
  Serial.print(profilestore,DEC);
  
  Serial.print("): Bat(");
//  Serial.print(analogRead(A0)*0.00489,DEC);
  Serial.print(batt1State,DEC);
  Serial.print(":");
  Serial.print(batt2State,DEC);
  Serial.print(") St(");
  Serial.print(state,DEC);
  Serial.print("-(");
  Serial.print(laststate,DEC);  
  Serial.print(") Mov(");
  Serial.print(movecount,DEC);
  Serial.print(") Dir(");
  Serial.print(movedir,BIN);
  Serial.print(") Spd(");
  Serial.print(MRight_max,DEC);
  Serial.print(":");
  Serial.print(MLeft_max,DEC);
  Serial.print(") Avd(");
  Serial.print(avoid,BIN);  
  Serial.print(") Coll(");
  Serial.print(coll_status,DEC);  
  Serial.print(":");
  Serial.print(coll_dist,DEC);
  Serial.print(") Inp(E1:");
  Serial.print(edge1State,DEC);
  Serial.print(" E2:"); 
  Serial.print(edge2State,DEC); 
  Serial.print(" E3:"); 
  Serial.print(edge3State,DEC);   
  Serial.print(" DR:"); 
  Serial.print(dist1Value,DEC);
  Serial.print(" DL  :"); 
  Serial.print(dist2Value,DEC);
  Serial.print(") Err(");
  Serial.print(boterror,DEC);

  Serial.print(") W(");
  Serial.print(digitalRead(LeftEncoderPin));
  Serial.print(":");
  Serial.print(digitalRead(RightEncoderPin));
  
  Serial.println(")");

  debugtimer=millis();
}  // end debugmessage();

/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */

/* Pause state. */
byte dostate_pause() {
  if(setupstate != state) {
    if ( movecount != 0 ) {       // Debug, we got here because of a sensor trigger with no other laststate.
      Serial.print("STOP :");
      debugmessage();
      movedir=0;
      movecount=0;
      changedir(movedir);
    }
  setupstate=state;
  }
return state_pause;
} // end dostate_pause();

/* ----------------------------------------------------------------------------------------------------------------------- */

/* Travel  */
byte dostate_move() { 
  if(setupstate != state) {
    if(movedir==dir_none) {
      return state_pause;
    }
    changedir(movedir);
    lastbump=millis();            // 'reset' bump timer.
    setupstate=state;
  }

  if ((millis() - movetimer)  >= 1) {
    movecount--;            // This will be driven by the wheel encoders later
    movetimer=millis();      // For now, it's just a milisecond timer.
  }
  
  // Bump/Fall and detect-backoff
  byte detectmode=avoid&B00001111;
  if(detectmode >= 9) {              // Avoid state + backoff bit.
    if(coll_status == 0) {
      Serial.println("Backed off.");
      return laststate;
    }
  }
  else {
    if (coll_status >= 1) {		// Have we hit somthing?
//      if ((millis() - lastbump)  >= 100) {        // And motors have been mmoving for a bit
        return laststate;    	        	// We're done moving
//      }
    }
  }
  
  // Stuck detect.  No bumps for too long. Throw an error!
  if ((millis() - lastbump)  >= 30000) {
    boterror=2;                  // So you know this triggerd it
    laststate=state;
    return state_error;
  }

  // Moved the required distance, we're done.
  if (movecount==0) { 
    return laststate;
  }
  
return state_move;
} // end dostate_move();

/* ----------------------------------------------------------------------------------------------------------------------- */


// Has avoid only complete when forward is sevel counts, has to allow continuing in same direction.
// Needs to use analog distance sensors to  turn before collision, no need to reverse.
// Needs 'turn until sensors clear' mode. And know diffrence beeween front and left/right collisions.

// 'avoid' must maintain direction state to allow above, including reversing and left/right planned turns. Done bitwise.
// First three bytes, 'avoid' state machine. Zero is 'no avoid'
// Status: Bit 4 is 'back off untill clear', bit 5 is left(0) vs. right(1). Bit 6 is 'allready tried'. Bits 7 and 8 are spare.

// Edge avoid: reverse until sensors clear plus a bit, turn in direction. 
//  if edge reverse -> left or right (random if none) -> turn until sensors clear + bit -> 

byte dostate_explore() {
  byte avoid_state=avoid&B00000111;
  
//    if(setupstate != state) {
//      movedir=dir_forward;
      setupstate=state;
//      avoid=0;
//    }

  switch(avoid_state){

  case 0:   // Robot moving forward normaly
  
    if ( movecount != 0 ) {          // A move stopped before time!
      // Left vs. Right    
      if(coll_status&B00000001) {    // Left sensor     
        avoid=B00100001;      // hit right sensor
      }
      else {
        avoid=B00000000;    // hit left sensor
      }
//      if(coll_status&B00001000){      // Analog sensor bump
      if(coll_dist >= 1 ) {          
        Serial.println("Analog turn-before-hit");
//        movedir=dir_none;       // Simply stop the robot breifly
        avoid=avoid|B00000010;  //  Avoid state 2 - spin
//        movecount=10;          // Allmost no amonunt
//        laststate=state;
      }
      else {     // Digital sensor
        avoid=avoid|B00000001;    // Avoid mode 1.
      } 
//      return state_move;
         
    }
    else {          // We haven't hit anything but the move counter ran out, reset it!
      movedir=dir_forward;
      movecount=30000;
      laststate=state;
      return state_move;
    }
  break;

  case 1:        // Reverse robot until sensors are clear then some more. 
    if(avoid&B00001000) {  // Have we backed off allready? Yes? Back off more.
      Serial.println("Backed off and reversing.");
      movedir=dir_back;
      avoid=avoid&B11110000;  // Clear 'back off' flag. And blank state.
      avoid=avoid|B00000010;  //  Avoid state 2 - reverse 
      movecount=1000;          // Half the robot's length and a bit
      laststate=state;
      return state_move;
    }
    else {   // Reverse robot until sensors clear
      Serial.println("Back off now.");
      avoid=avoid|B00001000;    // Set 'back off' flag. 
      movedir=dir_back; 
      movecount=300;
      laststate=state;
      return state_move;
    }
  break;

  case 2:   // Turn random number of degrees in other direction.
  Serial.println("Spinning!");
    if(avoid&B00100000) {  // Right
      movedir=dir_spinright;
    }
    else {   // Left
       movedir=dir_spinleft; 
    }
      avoid=avoid&B11111000;
      avoid=avoid|B00000011;  //  Avoid state 3
      movecount=random(500,1000);
      laststate=state;
      return state_move;
  break;

  case 3:  // Go ahead some distance, check.
      Serial.println("Testing forward.");
      movedir=dir_forward;
      avoid=avoid&B11111000;
      avoid=avoid|B00000100;  //  Avoid state 4
      movecount=500;
      laststate=state;
      return state_move;
  break;    
  
  case 4: // Test if we need to stop avoiding
    if ( movecount != 0 ) {    // We hit somthing anyway!  Need to add: Continue to turn 180 test. 
      Serial.println("We hit somthing again!");
      if(coll_status&B00001000) {    // Analog
        avoid=avoid|B00000010;  //  Avoid state 2 - spin
      } 
      else {
        avoid=avoid&B11111000;
        avoid=avoid|B00000001;  //  Back to state 1
      }
    }
    else {
      Serial.println("Avoid over!");
      avoid=B00000000;  //  Fully clear avoid state, back to 0.
    }        
  break;  
//  default:              // We are not avoiding

  break;
  
  }

return state_explore;
}  // End dostate_explore();

/* ----------------------------------------------------------------------------------------------------------------------- */

// Run through an array of commands(three bytes, direction byte+distance int) and execute them in order.
byte dostate_sequence() {
  
  
return state_pause;
};  // End dostate_sequence()
/* ----------------------------------------------------------------------------------------------------------------------- */

// Handle an critical error, mostly pause and wait
byte dostate_error() {
    if(setupstate != state){    // We've had an error! Kill the 'bot
    movedir=0;
    changedir(movedir);    // Switch off motors    
 //   digitalWrite(9,HIGH);        // Enable speeker
    setupstate=state;
  }
//    digitalWrite(13, HIGH);
//    tone(10,800);              // Beep and let the world know!
//    delay(500);
//    noTone(10);
  
    flashcount=boterror;  // Flash the error code
    while(flashcount>0){
      digitalWrite(13, LOW);
      delay(100);
      digitalWrite(13, HIGH);
      delay(300);
      flashcount--;
    }
return state_error;
} // End dostate_error();

/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */