bobbycar/controller/controller.ino

//https://github.com/rogerclarkmelbourne/Arduino_STM32   in arduino/hardware
//Board: Generic STM32F103C series
//Upload method: serial
//20k RAM 64k Flash
//may need 3v3 from usb ttl converter (hold down flash button while connecting). Holding down the power button is not needed in this case.
//Sometimes reconnecting the usb ttl converter to the pc helps just before pressing the upload button

// RX(green) is A10 , TX (blue) ist A9   (3v3 level)
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
//set boot0 back to 0 to run program on powerup

// ########################## DEFINES ##########################
#define SERIAL_CONTROL_BAUD   38400       // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
#define SERIAL_BAUD         115200      // [-] Baud rate for built-in Serial (used for the Serial Monitor)
#define START_FRAME         0xAAAA       // [-] Start frme definition for reliable serial communication

//#define DEBUG_RX                        // [-] Debug received data. Prints all bytes to serial (comment-out to disable)

//#define MAXADCVALUE 4095
#define ADC_CALIB_THROTTLE_LOWEST 1900 //a bit above maximum adc value if throttle it not touched
#define ADC_CALIB_THROTTLE_MIN 2000 //minimum adc value that should correspond to 0 speed
#define ADC_CALIB_THROTTLE_MAX 3110 //maximum adc value that should correspond to full speed

#define ADC_CALIB_BRAKE_MIN 800 //minimum adc value that should correspond to 0 speed
#define ADC_CALIB_BRAKE_MAX 2400 //maximum adc value that should correspond to full speed

#define PIN_STARTLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor
uint8_t startled=0;
#define PIN_STARTBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed
#define STARTBUTTON_DOWN digitalRead(PIN_STARTBUTTON)

#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
#define PIN_THROTTLE PA4
#define PIN_BRAKE PA5

#define PIN_ENABLE PB9

#define PIN_MODESWITCH PB5 // LOW if pressed in ("down")
#define MODESWITCH_DOWN !digitalRead(PIN_MODESWITCH)
#define PIN_MODELED_GREEN PA12
#define PIN_MODELED_RED PA11
uint8_t modeled_green=0;
uint8_t modeled_red=0;
long last_ledupdate=0;

#define PIN_RELAISFRONT PB14 //connected to relais which presses the powerbutton of the hoverboard for the front wheels
#define PIN_RELAISREAR PB15 //connected to relais which presses the powerbutton of the hoverboard for the rear wheels

#define DEBOUNCETIME 20 //time to not check for inputs after key press
#define BUTTONTIMEHOLD 750 //time for button hold
long millis_lastinput=0; //for button debounce
long timebuttonpressed_start;
boolean button_start=false;
boolean button_hold_start=false;


#define TIME_AUTOPOWEROFF 600000 //600000 = 10 minutes

long loopmillis=0; //only use one millis reading each loop
long last_looptime=0; //for looptiming
#define LOOPTIME 10 //how often the loop(s) should run
long millis_lastchange=0; //for poweroff after some time with no movement
#define MAXBRAKERATE 7 //maximum rate for braking (loop timing)

String errormessage=""; //store some error message to print

//Mode change variables
uint8_t state_modechange=0;
long state_modechange_time=0;

long millis_lastadc=0;
#define ADC_READTIME 10 //time interval to read adc (for filtering)
#define ADC_THROTTLE_FILTER 0.4 //low value = slower change
#define ADC_BRAKE_FILTER 0.4 //low value = slower change
  
int adc_throttle_raw=0; //raw throttle value from adc
float adc_throttle=0; //filtered value

int adc_brake_raw=0; //raw throttle value from adc
float adc_brake=0; //filtered value

uint16_t out_speedFL=0;
uint16_t out_speedFR=0;
uint16_t out_speedRL=0;
uint16_t out_speedRR=0;


long last_send = 0;

boolean board1Enabled=false;
boolean board2Enabled=false;


// Global variables for serial communication
//Serial1 (Rear)
uint8_t idx1 = 0;                        // Index for new data pointer
uint16_t bufStartFrame1;                 // Buffer Start Frame
byte *p1;                                // Pointer declaration for the new received data
byte incomingByte1;
byte incomingBytePrev1;
long lastValidDataSerial1_time;
long board1lastPoweron=0; //mainly for failcheck
long board1lastPoweroff=0;
long board1lastFeedbackMinSpeed;
boolean board1lastFeedbackMinSpeed_above=false;

//Same for Serial2 (Front)
uint8_t idx2 = 0;                        // Index for new data pointer
uint16_t bufStartFrame2;                 // Buffer Start Frame
byte *p2;                                // Pointer declaration for the new received data
byte incomingByte2;
byte incomingBytePrev2;
long lastValidDataSerial2_time;
long board2lastPoweron=0; //mainly for failcheck
long board2lastPoweroff=0;
long board2lastFeedbackMinSpeed;
boolean board2lastFeedbackMinSpeed_above=false;

typedef struct{
   uint16_t start;
   int16_t  speedLeft;
   int16_t  speedRight;
   uint16_t checksum;
} SerialCommand;
SerialCommand Command1;
SerialCommand Command2;

typedef struct{
   uint16_t start;
   int16_t  cmd1;
   int16_t  cmd2;
   int16_t  speedR;
   int16_t  speedL;
   int16_t  speedR_meas;
   int16_t  speedL_meas;
   int16_t  batVoltage;
   int16_t  boardTemp;
   int16_t  curL_DC;
   int16_t  curR_DC;
   int16_t  checksum;
} SerialFeedback;
SerialFeedback Feedback1;
SerialFeedback NewFeedback1;
SerialFeedback Feedback2;
SerialFeedback NewFeedback2;


enum mode{booting, idle, on, error, off};
/*
 * idle: controller is on, hoverboards are off
 * on: hoverbaords are on and happy
 * error: some error occured, stop everything and show errors
 * off: shutdown triggered. will power down latch soon
 */
mode currentmode; //current active mode
mode requestmode; //change this variable to initiate a mode change

mode last_requestmode=off; //for printout
mode last_currentmode=off; //for printout

// ########################## SETUP ##########################
void setup() 
{

  Serial.begin(115200); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)

  Serial1.begin(SERIAL_CONTROL_BAUD); //control.  A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear)
  Serial2.begin(SERIAL_CONTROL_BAUD); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front)

  // Pin Setup
  pinMode(PIN_STARTLED, OUTPUT); //MODE=PWM (needs testing, mcu locks up when using writePWM()
  pinMode(PIN_ENABLE, OUTPUT);
  digitalWrite(PIN_ENABLE, HIGH); //keep power on
  pinMode(PIN_STARTBUTTON, INPUT_PULLUP);
  pinMode(PIN_MODESWITCH, INPUT_PULLUP);
  pinMode(PIN_MODELED_GREEN, OUTPUT);
  pinMode(PIN_MODELED_RED, OUTPUT);
  pinMode(PIN_RELAISFRONT, OUTPUT);
  pinMode(PIN_RELAISREAR, OUTPUT);
  pinMode(PIN_THROTTLE, INPUT);
  pinMode(PIN_BRAKE, INPUT);

  Serial.println("Initialized"); 

  currentmode = booting; //start in idle mode
  requestmode = currentmode;

  millis_lastchange=millis();
}


// ########################## LOOP ##########################
void loop() {
  loopmillis=millis(); //read millis for this cycle

  ReceiveSerial1(); // Check for new received data
  ReceiveSerial2(); // Check for new received data

  handleInputs();
  if (button_start) {
    Serial.println("button_start");
  }
  if (button_hold_start) {
    Serial.println("button_hold_start");
  }
  
  handleModeChange(); //mode changes
  if (last_requestmode!=requestmode) {
    Serial.print("requestmode="); Serial.println(modeToString(requestmode)); 
    last_requestmode=requestmode;
  }
  if (last_currentmode!=currentmode) {
    Serial.print("currentmode="); Serial.println(modeToString(currentmode));
    last_currentmode=currentmode;
  }

  ledUpdate();
  
  modeloops();
  

  if (loopmillis - last_send > SENDPERIOD) {
    last_send=loopmillis;

    if (currentmode!=off || currentmode!=idle) { //if boards should be powered on
      SendSerial2(out_speedFL,out_speedFR); //Front
      SendSerial1(out_speedRL,out_speedRR); //Rear
    }
    if (currentmode==on) {
      Serial.print("lastData1="); Serial.print(loopmillis-lastValidDataSerial1_time); Serial.print(", lastData2=");Serial.print(loopmillis-lastValidDataSerial2_time); Serial.print(", speedFL="); Serial.println(out_speedFL);
    }
  }

  

  if (currentmode!=error) { //keep last errormessage
    failChecks();
  }
}


void handleInputs()
{
  //Short press (true when button short pressed, on release)
  button_start=false;

  //long press (true when button is held down for BUTTONTIMEHOLD, on time elapsed)
  button_hold_start=false;

  if (loopmillis-millis_lastinput>DEBOUNCETIME) //Button debouncing
  {
    //Trigger
    if (timebuttonpressed_start == 0 && STARTBUTTON_DOWN){ //first time pressed down. (low when pressed)
      timebuttonpressed_start=loopmillis; //set time of button press
      millis_lastinput=loopmillis;
    }else if(timebuttonpressed_start != 0 && !STARTBUTTON_DOWN){ //button released (was pressed)
      if (loopmillis-timebuttonpressed_start < BUTTONTIMEHOLD){ //short press
        button_start=true;
      }
      timebuttonpressed_start=0; //re-enable after short press and release from hold
      millis_lastinput=loopmillis;
    }else if(loopmillis-timebuttonpressed_start >= BUTTONTIMEHOLD && timebuttonpressed_start>0){ //held down long enough and not already hold triggered
      button_hold_start=true;
      timebuttonpressed_start=-1; //-1 as flag for hold triggered
    }
  }

  if ( button_start || button_hold_start) {
    millis_lastchange=loopmillis; //for auto poweroff
    millis_lastinput=loopmillis; //for debouncing
  }

  if (loopmillis-millis_lastadc>ADC_READTIME) {
    adc_throttle_raw = analogRead(PIN_THROTTLE);
    adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER;
  
    adc_brake_raw = analogRead(PIN_BRAKE);
    adc_brake = adc_brake*(1-ADC_BRAKE_FILTER) + adc_brake_raw*ADC_BRAKE_FILTER;
    
    if (adc_throttle_raw >= ADC_CALIB_THROTTLE_MIN || adc_brake_raw >= ADC_CALIB_BRAKE_MIN) { //throttle or brake pressed
      millis_lastchange=loopmillis; 
    }
    millis_lastadc=loopmillis;
  }

  if (loopmillis-millis_lastchange>TIME_AUTOPOWEROFF){
    requestmode = off;
  }
}

void handleModeChange() {
  if (currentmode==requestmode) { //## Not currently changing modes ##
    switch (currentmode) { //mode dependant
      case booting: //on startup.  active while start button is still pressed
        if (button_start) { //button first release
          requestmode=idle; //start in idle state
          state_modechange=0; //reset state for safety
        }//TODO else if (button_hold_start) { requestmode=on; }
        break;
      case idle:
        if (button_hold_start){  //long press
          requestmode=on; //long press switches betweeen idle and on
          state_modechange=0; //start at state 0
        }
        if (button_start) { //short press
          requestmode=off; 
          state_modechange=0;
        }
        break;
      case on:
        if (button_hold_start){  //long press
          requestmode=idle; //long press switches betweeen idle and on
          state_modechange=0; //start at state 0
        }
        if (button_start) { //short press
          requestmode=off;
          state_modechange=0;
        }
        break;
      case error:
        if (button_start) { //short press
          requestmode=off;
          state_modechange=0;
        }
        break;
      case off:
        break;
      default:
        currentmode=error; //something else? -> error
    }
  }else{ // ## Change requested ## 
    switch (requestmode) { //mode changes
      case booting:
        requestmode=error;
        currentmode=requestmode;
        errormessage="Change to booting mode cannot be requested";
        break;
      case idle: case on: case off: //similar for on, idle and off
          if (currentmode == booting) { //coming from booting mode
            currentmode=idle; //switch directly without powering boards
            requestmode=currentmode; //make shure it stay in this mode
            state_modechange=0;
            break;
          }
          if ( (state_modechange>0 || (requestmode==idle && boardsPowered()) || (requestmode==off && boardsPowered()) || (requestmode==on && !boardsPowered()) )) { //power cylce in progress OR need to power on/off boards
            //Hoverboard powering
            switch(state_modechange) {
              case 0: 
                if (requestmode==on && (adc_throttle > ADC_CALIB_THROTTLE_LOWEST || adc_brake > ADC_CALIB_BRAKE_MIN) ) { //requested to turn on but throttle or brake is pressed
                  state_modechange=0;
                  requestmode=currentmode; //abort modechange
                  //TODO: led show aborted modechange
                }else{ //everythings fine, turn on/off
                  digitalWrite(PIN_RELAISFRONT,HIGH); //simulate hoverboard power button press
                  state_modechange++;
                  state_modechange_time=loopmillis; //set to current time
                  Serial.println("PIN_RELAISFRONT,HIGH");
                }
                break;
              case 1:
                if (loopmillis - state_modechange_time > 200) { //wait some time
                  digitalWrite(PIN_RELAISFRONT,LOW); //release simulated button
                  state_modechange++;
                  state_modechange_time=loopmillis; //set to current time
                  Serial.println("PIN_RELAISFRONT,LOW");
                }
                break;
              case 2:
                if (loopmillis - state_modechange_time > 200) { //wait some time
                  digitalWrite(PIN_RELAISREAR,HIGH); //simulate hoverboard power button press
                  state_modechange++;
                  state_modechange_time=loopmillis; //set to current time
                  Serial.println("PIN_RELAISREAR,HIGH");
                }
                break;
              case 3:
                if (loopmillis - state_modechange_time > 200) { //wait some time
                  digitalWrite(PIN_RELAISREAR,LOW); //release simulated button
                  state_modechange++;
                  state_modechange_time=loopmillis; //set to current time
                  Serial.println("PIN_RELAISREAR,LOW");
                }
                break;
              case 4:
                if (loopmillis - state_modechange_time > 1000) { //wait some time after turning on/off
                  state_modechange++;
                  state_modechange_time=loopmillis; //set to current time
                  Serial.println("Waiting finished");
                }
                break;
              case 5: 
                // ### Request On ###
                if (requestmode==on) {//wait for both boards to send feedback
                  state_modechange++;
                  board1Enabled=true; //assume board is online
                  board1lastPoweron=loopmillis; //save time at which board was powered on
                  board2Enabled=true; //assume board is online
                  board2lastPoweron=loopmillis; //save time at which board was powered on
                // ### Request Idle or Off (both power boards off) ###
                }else if(requestmode==idle || requestmode==off) {
                  state_modechange++;
                  board1Enabled=false; //assume board is offline
                  board1lastPoweroff=loopmillis; //save time at which board was powered off
                  board2Enabled=false; //assume board is offline
                  board2lastPoweroff=loopmillis; //save time at which board was powered off
                  
                  Serial.println("finished");
                }else{ //if changed off from error mode
                  state_modechange++;
                }
                break;
              default: //finished modechange
                currentmode=requestmode;
                state_modechange=0;
                break;
            }
          }else{
            currentmode=requestmode;
            state_modechange=0; //for safety
            //Should not happen
            Serial.print("Warning: power cycle not needed. board1Enabled="); Serial.print(board1Enabled); Serial.print("board2Enabled="); Serial.println(board2Enabled);
          }
          
        break;
  
        
      case error:
        currentmode=error; //stay in this mode
        break;
      default:
        currentmode=error;
    }
  }
}



void modeloops() {
  if (loopmillis - last_looptime >= LOOPTIME) {
    last_looptime=loopmillis;
    //loop_test(); //for testing (adc calibration prints). comment out following switch case
    
    switch (currentmode) { //mode changes
      case booting: 
        break;
      case idle: 
        loop_idle();
        break;
      case on:
        loop_on();
        break;
      case error:
        loop_error();
        break;
      case off:
        loop_off();
        break;
    }
  }
}

void loop_idle() {
  out_speedFL=out_speedFR=out_speedRR=out_speedRL=0; //stop motors
}

void loop_on() {
  int _maxspeed=1000;
  int _maxbrake=400;
  if (MODESWITCH_DOWN) {
    _maxspeed=200;
    _maxbrake=200;
  }
  int16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed);

  int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking

  int16_t speedvalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque

  out_speedFL=speedvalue;
  out_speedFR=speedvalue;
  out_speedRL=speedvalue;
  out_speedRR=speedvalue;
  
}

void loop_error() {
  out_speedFL=out_speedFR=out_speedRR=out_speedRL=0; //stop motors
  Serial.print("Error:"); Serial.println(errormessage);
}

void loop_test() {
  Serial.print("adc_throttle_raw="); Serial.print(adc_throttle_raw);
  Serial.print(", adc_brake_raw="); Serial.print(adc_brake_raw);
  
  int _maxspeed=1000;
  int _maxbrake=400;
  if (MODESWITCH_DOWN) {
    _maxspeed=200;
    _maxbrake=200;
  }
  int16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed);

  
  int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking

  int16_t speedvalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque
  Serial.print(", throttle="); Serial.print(throttlevalue); Serial.print(", brake="); Serial.print(brakevalue); Serial.print(", speed="); Serial.println(speedvalue);
}

void loop_off() {
  //loop enters when boards are sucessfully turned off
  digitalWrite(PIN_ENABLE, LOW); //cut own power
  
}

boolean boardsPowered()
{
  return (board1Enabled && board2Enabled); //true if both boards enabled
}

void failChecks() 
{
  #define FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME 3000 //time to start failchecking boardpower after board poweroff
  #define FAILCHECK_RECEIVERECENT_TIME 1000 //timeout .should be less than FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME
  // ## Check if board is really offline ##
  if (!board1Enabled) { //board should be offline
    if (loopmillis-board1lastPoweroff > FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME){ //wait some time before checking if board did power off
      if (loopmillis-lastValidDataSerial1_time < FAILCHECK_RECEIVERECENT_TIME) { //new message received recently?
        errormessage="Board 1 should be offline but feedback received";
        Serial.println(errormessage);
        requestmode=error;
      }
    }
  }
  if (!board2Enabled) { //board should be offline
    if (loopmillis-board2lastPoweroff > FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME){ //wait some time before checking if board did power off
      if (loopmillis-lastValidDataSerial2_time < FAILCHECK_RECEIVERECENT_TIME) { //new message received recently?
        errormessage="Board 2 should be offline but feedback received";
        Serial.println(errormessage);
        requestmode=error;
      }
    }
  }

  #define MINSPEED_FOR_FEEDBACK 250 //speed at which feedback output should be expected
  #define RESETSPEED_FOR_FEEDBACK 50 //should be less than MINSPEED_FOR_FEEDBACK. speed at which board2lastFeedbackMinSpeed will be able to be reset
  if (!board2lastFeedbackMinSpeed_above && ( abs(out_speedFL) > MINSPEED_FOR_FEEDBACK || abs(out_speedFR) > MINSPEED_FOR_FEEDBACK ) ){
    board2lastFeedbackMinSpeed=loopmillis; //front is board 2
    board2lastFeedbackMinSpeed_above=true;
  }
  if (board2lastFeedbackMinSpeed_above && abs(out_speedFL) < RESETSPEED_FOR_FEEDBACK && abs(out_speedFR) < RESETSPEED_FOR_FEEDBACK) {  //if speed of both wheels goes below a threshold, board2lastFeedbackMinSpeed will be able to reset
    board2lastFeedbackMinSpeed_above=false;
  }

  if (!board1lastFeedbackMinSpeed_above && ( abs(out_speedRL) > MINSPEED_FOR_FEEDBACK || abs(out_speedRR) > MINSPEED_FOR_FEEDBACK ) ){
    board1lastFeedbackMinSpeed=loopmillis; //rear is board 1
    board1lastFeedbackMinSpeed_above=true;
  }
  if (board1lastFeedbackMinSpeed_above && abs(out_speedRL) < RESETSPEED_FOR_FEEDBACK && abs(out_speedRR) < RESETSPEED_FOR_FEEDBACK) {  //if speed of both wheels goes below a threshold, board2lastFeedbackMinSpeed will be able to reset
    board1lastFeedbackMinSpeed_above=false;
  }

  #define FAILCHECK_WAITCHECK_AFTER_MINSPEED_TIME 3000 //time to start failchecking boardpower after minimum throttle that should give some feedback
  // ## Check if board is online (when it should send feedback) ##
  if (board1Enabled) { //board should be online
    if (loopmillis-board1lastFeedbackMinSpeed > FAILCHECK_WAITCHECK_AFTER_MINSPEED_TIME) { //wait some time before checking
      if (board1lastFeedbackMinSpeed_above && loopmillis-lastValidDataSerial1_time > FAILCHECK_RECEIVERECENT_TIME) { //speed still high enough but no new messages recently received?
        errormessage="Board 1 should be online and give feedback but didnt";
        Serial.println(errormessage);
        requestmode=error;
      }
    }
  }
  if (board2Enabled) { //board should be online
    if (loopmillis-board2lastFeedbackMinSpeed > FAILCHECK_WAITCHECK_AFTER_MINSPEED_TIME) { //wait some time before checking
      if (board2lastFeedbackMinSpeed_above && loopmillis-lastValidDataSerial2_time > FAILCHECK_RECEIVERECENT_TIME) { //speed still high enough but no new messages recently received?
        errormessage="Board 2 should be online and give feedback but didnt";
        Serial.println(errormessage);
        requestmode=error;
      }
    }
  }
  
}

String modeToString(uint8_t m) {
  if (m==idle) return "idle";
  if (m==off) return "off";
  if (m==error) return "error";
  if (m==on) return "on";
  if (m==booting) return "booting";
}


void ledUpdate() {
  #define LEDUPDATETIME 20
  #define FASTERRORBLINKDELAY 100 //period for startled to blink on error
  if (loopmillis - last_ledupdate >= LEDUPDATETIME) {
    last_ledupdate=loopmillis;
    // ## StartLed ##
    uint8_t _ledbrightness;
    switch (currentmode) { //modeLed for different currentmodes
      case booting: //Startled dimmed
        startled=255;
        break;
      case idle: //Breathing Startled
        _ledbrightness=uint8_t( (loopmillis/10)%(512) );
        startled=_ledbrightness<=255 ? _ledbrightness : (512)-_ledbrightness; //reverse if >255 to go down again
        break;
      case on: //Startled on
        startled=255;
        break;
      case error: //Startled blink
        startled=(loopmillis/FASTERRORBLINKDELAY)%2==0 ? 0 : 255; // Blink led
        break;
      case off: //Startled off
        startled=0; 
        break;
    }

    // ## ModeLed ##
    if (currentmode!=requestmode) { //ongoing modechange
      modeled_green=0; modeled_red=0; //ModeLed=Off
    }else{
      switch (currentmode) { //modeLed for different currentmodes
        case booting: 
          modeled_green=255; modeled_red=0; //ModeLed=Green
          break;
        case idle: 
          modeled_green=255; modeled_red=255; //ModeLed=Yellow
          break;
        case on:
          modeled_green=255; modeled_red=0; //ModeLed=Green
          break;
        case error:
          modeled_green=0; modeled_red=255; //ModeLed=Red
          break;
        case off:
          modeled_green=255; modeled_red=255; //ModeLed=Yellow
          break;
      }
    }
    /*
    pwmWrite(PIN_MODELED_GREEN, map(modeled_green, 0, 255, 0, 65535));
    pwmWrite(PIN_MODELED_RED, map(modeled_red, 0, 255, 0, 65535));
    pwmWrite(PIN_STARTLED, map(startled, 0, 255, 0, 65535));
    */
    digitalWrite(PIN_MODELED_GREEN, modeled_green<127? true:false); //red and green inverted (common anode)
    digitalWrite(PIN_MODELED_RED, modeled_red<127? true:false); //red and green inverted (common anode)
    digitalWrite(PIN_STARTLED, startled>127? true:false);
  }
}




/*
// Old loop
void loopold() {
  //selfTest(); //start selftest, does not return

  ReceiveSerial1(); // Check for new received data

  if (millis()>2000 && STARTBUTTON_DOWN) {
    poweronBoards();
  }
    
  if (millis() - last_send > SENDPERIOD) {
    //Serial.print("powerbutton="); Serial.print(STARTBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);
    
    int _read=analogRead(PIN_THROTTLE);
    
    int16_t speedvalue=constrain( map(_read, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, 1000 ) ,0, 1000);

    if (MODESWITCH_DOWN) {
      SendSerial1(speedvalue,0);
      SendSerial2(speedvalue,0);
      Serial.print("L_");
    }else{
      SendSerial1(0,speedvalue);
      SendSerial2(0,speedvalue);
      Serial.print("R_");
    }
    Serial.print("millis="); Serial.print(millis()); Serial.print(", adcthrottle="); Serial.print(_read); 
    Serial.print(", 1.L="); Serial.print(Command1.speedLeft); Serial.print(", 1.R="); Serial.print(Command1.speedRight);
    Serial.print(", 2.L="); Serial.print(Command2.speedLeft); Serial.print(", 2.R="); Serial.println(Command2.speedRight);

    last_send = millis();

    digitalWrite(PIN_STARTLED, !digitalRead(PIN_STARTLED));
    if (testcounter%3==0) {
      digitalWrite(PIN_MODELED_GREEN, !digitalRead(PIN_MODELED_GREEN));
    }
    if (testcounter%5==0) {
      digitalWrite(PIN_MODELED_RED, !digitalRead(PIN_MODELED_RED));
    }
    
    testcounter++;


    //Print Motor values
    Serial.print("cmd1");
    Serial.print(", ");  Serial.print("cmd2");
    Serial.print(",");  Serial.print("speedR");
    Serial.print(",");  Serial.print("speedL");
    Serial.print(", ");  Serial.print("speedR_meas");
    Serial.print(",");  Serial.print("speedL_meas");
    Serial.print(", ");  Serial.print("batVoltage");
    Serial.print(", ");  Serial.println("boardTemp");
    Serial.println();
    Serial.print("1: "); Serial.print(Feedback1.cmd1);
    Serial.print(", ");  Serial.print(Feedback1.cmd2);
    Serial.print(",");  Serial.print(Feedback1.speedR);
    Serial.print(",");  Serial.print(Feedback1.speedL);
    Serial.print(", ");  Serial.print(Feedback1.speedR_meas);
    Serial.print(",");  Serial.print(Feedback1.speedL_meas);
    Serial.print(", ");  Serial.print(Feedback1.batVoltage);
    Serial.print(", ");  Serial.println(Feedback1.boardTemp);
    Serial.println();
    Serial.print("2: "); Serial.print(Feedback2.cmd1);
    Serial.print(", ");  Serial.print(Feedback2.cmd2);
    Serial.print(",");  Serial.print(Feedback2.speedR);
    Serial.print(",");  Serial.print(Feedback2.speedL);
    Serial.print(", ");  Serial.print(Feedback2.speedR_meas);
    Serial.print(",");  Serial.print(Feedback2.speedL_meas);
    Serial.print(", ");  Serial.print(Feedback2.batVoltage);
    Serial.print(", ");  Serial.println(Feedback2.boardTemp);
  }

  if (millis()>30000 && STARTBUTTON_DOWN) {
    poweroff();
  }

}

// ########################## END ##########################


void poweroff() {

  //TODO: trigger Relais for Board 1
  // Wait for board to shut down
  //TODO: trigger Relais for Board 2
  // Wait for board to shut down
  
  //Timeout error handling

  digitalWrite(PIN_ENABLE, LOW); //poweroff own latch

  delay(1000);
  Serial.println("Still powered");
  //still powered on: set error status "power latch error"
}

void poweronBoards() {
  digitalWrite(PIN_RELAISFRONT,HIGH);
  delay(200);digitalWrite(PIN_RELAISFRONT,LOW);
  delay(50);
  digitalWrite(PIN_RELAISREAR,HIGH);
  delay(200);digitalWrite(PIN_RELAISREAR,LOW);
}


*/


void selfTest() {
  digitalWrite(PIN_ENABLE,HIGH); //make shure latch is on
  Serial.println("Entering selftest");
  #define TESTDELAY 1000 //delay between test
  #define TESTTIME 500 //time to keep tested pin on

  delay(TESTDELAY); Serial.println("PIN_STARTLED");
  digitalWrite(PIN_STARTLED,HIGH); delay(TESTTIME); digitalWrite(PIN_STARTLED,LOW);

  delay(TESTDELAY); Serial.println("PIN_MODELED_GREEN");
  digitalWrite(PIN_MODELED_GREEN,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_GREEN,HIGH);

  delay(TESTDELAY); Serial.println("PIN_MODELED_RED");
  digitalWrite(PIN_MODELED_RED,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_RED,HIGH);

  delay(TESTDELAY); Serial.println("PIN_RELAISFRONT");
  digitalWrite(PIN_RELAISFRONT,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISFRONT,LOW);

  delay(TESTDELAY); Serial.println("PIN_RELAISREAR");
  digitalWrite(PIN_RELAISREAR,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISREAR,LOW);

  delay(TESTDELAY); Serial.println("ALL ON");
  digitalWrite(PIN_STARTLED,HIGH);
  digitalWrite(PIN_MODELED_GREEN,LOW);
  digitalWrite(PIN_MODELED_RED,LOW);
  digitalWrite(PIN_RELAISFRONT,HIGH);
  digitalWrite(PIN_RELAISREAR,HIGH);
  delay(TESTTIME*5);
  digitalWrite(PIN_STARTLED,LOW);
  digitalWrite(PIN_MODELED_GREEN,HIGH);
  digitalWrite(PIN_MODELED_RED,HIGH);
  digitalWrite(PIN_RELAISFRONT,LOW);
  digitalWrite(PIN_RELAISREAR,LOW);
  delay(TESTDELAY);

  Serial.println("Powers off latch at millis>=60000");
  Serial.println("Inputs:");
  while(true) { //Keep printing input values forever
    delay(100);
    Serial.print("millis="); Serial.print(millis()); Serial.print(", throttle ADC="); Serial.println(analogRead(PIN_THROTTLE));
    Serial.print("powerbutton down="); Serial.print(STARTBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);

    while (millis()>=60000) {
      digitalWrite(PIN_ENABLE, LOW); //poweroff own latch
      Serial.println(millis());
    }
  }

  
  
}