bobbycar/controller/controller.ino

// *******************************************************************
//  Arduino Nano 3.3V example code
//  for   https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC
//
//  Copyright (C) 2019-2020 Emanuel FERU <aerdronix@gmail.com>
//
// *******************************************************************
// INFO:
// • This sketch uses the the Serial Software interface to communicate and send commands to the hoverboard
// • The built-in (HW) Serial interface is used for debugging and visualization. In case the debugging is not needed,
//   it is recommended to use the built-in Serial interface for full speed perfomace.
// • The data packaging includes a Start Frame, checksum, and re-syncronization capability for reliable communication
// 
// CONFIGURATION on the hoverboard side in config.h:
// • Option 1: Serial on Left Sensor cable (long wired cable)
//   #define CONTROL_SERIAL_USART2
//   #define FEEDBACK_SERIAL_USART2
//   // #define DEBUG_SERIAL_USART2
// • Option 2: Serial on Right Sensor cable (short wired cable) - recommended, so the ADCs on the other cable are still available
//   #define CONTROL_SERIAL_USART3
//   #define FEEDBACK_SERIAL_USART3
//   // #define DEBUG_SERIAL_USART3
// *******************************************************************

//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_MIN 2000
#define ADC_CALIB_THROTTLE_MAX 3120

#define PIN_POWERLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor
#define PIN_POWERBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed
#define POWERBUTTON_DOWN digitalRead(PIN_POWERBUTTON)

#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

#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

int testcounter=0;

long last_send = 0;


// Global variables
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;

//Same for Serial2
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;


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  checksum;
} SerialFeedback;
SerialFeedback Feedback1;
SerialFeedback NewFeedback1;
SerialFeedback Feedback2;
SerialFeedback NewFeedback2;

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

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

  Serial1.begin(38400); //control.  A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector
  Serial2.begin(38400); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector
  
    
  Serial1.begin(SERIAL_CONTROL_BAUD);
  pinMode(PIN_POWERLED, OUTPUT);
  pinMode(PIN_ENABLE, OUTPUT);
  digitalWrite(PIN_ENABLE, HIGH); //keep power on
  pinMode(PIN_POWERBUTTON, 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"); 
}

// ########################## SEND ##########################
void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight)
{
  // Create command
  Command1.start    = (uint16_t)START_FRAME;
  Command1.speedLeft    = (int16_t)uSpeedLeft;
  Command1.speedRight    = (int16_t)uSpeedRight;
  Command1.checksum = (uint16_t)(Command1.start ^ Command1.speedLeft ^ Command1.speedRight);
  Serial1.write((uint8_t *) &Command1, sizeof(Command1)); 
}
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight)
{
  // Create command
  Command2.start    = (uint16_t)START_FRAME;
  Command2.speedLeft    = (int16_t)uSpeedLeft;
  Command2.speedRight    = (int16_t)uSpeedRight;
  Command2.checksum = (uint16_t)(Command2.start ^ Command2.speedLeft ^ Command2.speedRight);
  Serial2.write((uint8_t *) &Command2, sizeof(Command2)); 
}

// ########################## RECEIVE ##########################
void ReceiveSerial1()
{
  // Check for new data availability in the Serial buffer
  if ( Serial1.available() ) {
    incomingByte1    = Serial1.read();                                 // Read the incoming byte
    bufStartFrame1 = ((uint16_t)(incomingBytePrev1) << 8) +  incomingByte1;  // Construct the start frame    
  }
  else {
    return;
  }

  // If DEBUG_RX is defined print all incoming bytes
  #ifdef DEBUG_RX
    Serial.print(incomingByte1);
    return;
  #endif      
  
  // Copy received data
  if (bufStartFrame1 == START_FRAME) {                     // Initialize if new data is detected
    p1     = (byte *)&NewFeedback1;
    *p1++  = incomingBytePrev1;
    *p1++  = incomingByte1;   
    idx1   = 2;    
  } else if (idx1 >= 2 && idx1 < sizeof(SerialFeedback)) {  // Save the new received data
    *p1++  = incomingByte1; 
    idx1++;
  } 
  
  // Check if we reached the end of the package
  if (idx1 == sizeof(SerialFeedback)) {    
    uint16_t checksum;
    checksum = (uint16_t)(NewFeedback1.start ^ NewFeedback1.cmd1 ^ NewFeedback1.cmd2 ^ NewFeedback1.speedR ^ NewFeedback1.speedL
          ^ NewFeedback1.speedR_meas ^ NewFeedback1.speedL_meas ^ NewFeedback1.batVoltage ^ NewFeedback1.boardTemp);
  
    // Check validity of the new data
    if (NewFeedback1.start == START_FRAME && checksum == NewFeedback1.checksum) {
      // Copy the new data
      memcpy(&Feedback1, &NewFeedback1, sizeof(SerialFeedback));
    }
    idx1 = 0;  // Reset the index (it prevents to enter in this if condition in the next cycle)
    /*
      // Print data to built-in Serial
      Serial.print("1: ");   Serial.print(Feedback.cmd1);
      Serial.print(" 2: ");  Serial.print(Feedback.cmd2);
      Serial.print(" 3: ");  Serial.print(Feedback.speedR);
      Serial.print(" 4: ");  Serial.print(Feedback.speedL);
      Serial.print(" 5: ");  Serial.print(Feedback.speedR_meas);
      Serial.print(" 6: ");  Serial.print(Feedback.speedL_meas);
      Serial.print(" 7: ");  Serial.print(Feedback.batVoltage);
      Serial.print(" 8: ");  Serial.println(Feedback.boardTemp);
    } else {
      Serial.println("Non-valid data skipped");
    }*/
    
  }
  
  // Update previous states
  incomingBytePrev1  = incomingByte1;
}
void ReceiveSerial2()
{
  // Check for new data availability in the Serial buffer
  if ( Serial2.available() ) {
    incomingByte2    = Serial2.read();                                 // Read the incoming byte
    bufStartFrame2 = ((uint16_t)(incomingBytePrev2) << 8) +  incomingByte2;  // Construct the start frame    
  }
  else {
    return;
  }

  // If DEBUG_RX is defined print all incoming bytes
  #ifdef DEBUG_RX
    Serial.print(incomingByte2);
    return;
  #endif      
  
  // Copy received data
  if (bufStartFrame2 == START_FRAME) {                     // Initialize if new data is detected
    p2     = (byte *)&NewFeedback2;
    *p2++  = incomingBytePrev2;
    *p2++  = incomingByte2;   
    idx2   = 2;    
  } else if (idx2 >= 2 && idx2 < sizeof(SerialFeedback)) {  // Save the new received data
    *p2++  = incomingByte2; 
    idx2++;
  } 
  
  // Check if we reached the end of the package
  if (idx2 == sizeof(SerialFeedback)) {    
    uint16_t checksum;
    checksum = (uint16_t)(NewFeedback2.start ^ NewFeedback2.cmd1 ^ NewFeedback2.cmd2 ^ NewFeedback2.speedR ^ NewFeedback2.speedL
          ^ NewFeedback2.speedR_meas ^ NewFeedback2.speedL_meas ^ NewFeedback2.batVoltage ^ NewFeedback2.boardTemp);
  
    // Check validity of the new data
    if (NewFeedback2.start == START_FRAME && checksum == NewFeedback2.checksum) {
      // Copy the new data
      memcpy(&Feedback2, &NewFeedback2, sizeof(SerialFeedback));
    }
    idx2 = 0;  // Reset the index (it prevents to enter in this if condition in the next cycle)
    
  }
  
  // Update previous states
  incomingBytePrev2  = incomingByte2;
}

// ########################## LOOP ##########################



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

  ReceiveSerial1(); // Check for new received data

  if (millis()>2000 && POWERBUTTON_DOWN) {
    poweronBoards();
  }
    
  if (millis() - last_send > SENDPERIOD) {
    //Serial.print("powerbutton="); Serial.print(POWERBUTTON_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_POWERLED, !digitalRead(PIN_POWERLED));
    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 && POWERBUTTON_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_POWERLED");
  digitalWrite(PIN_POWERLED,HIGH); delay(TESTTIME); digitalWrite(PIN_POWERLED,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_POWERLED,HIGH);
  digitalWrite(PIN_MODELED_GREEN,LOW);
  digitalWrite(PIN_MODELED_RED,LOW);
  digitalWrite(PIN_RELAISFRONT,HIGH);
  digitalWrite(PIN_RELAISREAR,HIGH);
  delay(TESTTIME*5);
  digitalWrite(PIN_POWERLED,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(POWERBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);

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

  
  
}