bobbycar/controller_teensy/src/main.cpp

407 lines
15 KiB
C++
Raw Normal View History

#include <Arduino.h>
2021-03-09 23:59:54 +00:00
/*
Connections:
Tennsy Pin, Pin Name, Connected to
10, Tx2, Hoverboard RX(Green)
9, Rx2, Hoverboard TX(Blue)
8, Tx3, Hoverboard RX(Green)
7, Rx3, Hoverboard TX(Blue)
*/
// ########################## DEFINES ##########################
2021-03-02 22:48:20 +00:00
#define SERIAL_CONTROL_BAUD 115200 // [-] 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)
2021-03-02 22:48:20 +00:00
#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication
2021-03-20 20:20:17 +00:00
#define SERIAL_LOG_BAUD 115200 // baud rate for logging output
bool log_update=true;
unsigned long last_log_send=0;
#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
2021-03-20 20:20:17 +00:00
bool controllers_connected=false;
2021-03-02 22:48:20 +00:00
2021-03-20 15:13:16 +00:00
#define PIN_THROTTLE A7
const uint16_t calib_throttle_min = 400; //better a bit too high than too low
const uint16_t calib_throttle_max = 790;
#define PIN_BRAKE A8
const uint16_t calib_brake_min = 100;//better a bit too high than too low
const uint16_t calib_brake_max = 600;
int16_t throttle_pos=0;
int16_t brake_pos=0;
unsigned long last_adcread=0;
#define ADCREADPERIOD 25
2021-03-20 15:13:16 +00:00
#define PIN_START A9
#define PIN_LED_START 2 //Enginge start led
#define PIN_LATCH_ENABLE A6
#define PIN_MODE_SWITCH 3
#define PIN_MODE_LEDG 4
#define PIN_MODE_LEDR 5
2021-03-09 23:59:54 +00:00
unsigned long last_send = 0;
2021-03-02 22:48:20 +00:00
unsigned long last_receive = 0;
2021-03-20 20:20:17 +00:00
float filtered_currentAll=0;
2021-03-09 18:45:36 +00:00
int16_t cmd_send=0;
int16_t last_cmd_send=0;
// Global variables for serial communication
typedef struct{
uint8_t idx = 0; // Index for new data pointer
uint16_t bufStartFrame; // Buffer Start Frame
byte *p; // Pointer declaration for the new received data
byte incomingByte;
byte incomingBytePrev;
long lastValidDataSerial_time;
} SerialRead;
SerialRead SerialcomFront;
SerialRead SerialcomRear;
typedef struct{
uint16_t start;
int16_t speedLeft;
int16_t speedRight;
uint16_t checksum;
} SerialCommand;
SerialCommand CommandFront;
SerialCommand CommandRear;
typedef struct{
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedL_meas;
int16_t speedR_meas;
int16_t batVoltage;
int16_t boardTemp;
int16_t curL_DC; //negative values are current drawn. positive values mean generated current
int16_t curR_DC;
uint16_t cmdLed;
uint16_t checksum;
} SerialFeedback;
SerialFeedback FeedbackFront;
SerialFeedback NewFeedbackFront;
SerialFeedback FeedbackRear;
SerialFeedback NewFeedbackRear;
#define CURRENT_FILTER_SIZE 100 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
#define CURRENT_MEANVALUECOUNT 10 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used
typedef struct{
int16_t curL_DC[CURRENT_FILTER_SIZE] = {0};
int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
uint8_t cur_pos=0;
int16_t cmdL=0;
int16_t cmdR=0;
2021-03-20 20:20:17 +00:00
float filtered_curL=0;
float filtered_curR=0;
unsigned long millis=0; //time when last message received
} MotorParameter;
MotorParameter motorparamsFront;
MotorParameter motorparamsRear;
2021-03-02 22:48:20 +00:00
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);
int sort_desc(const void *cmp1, const void *cmp2);
float filterMedian(int16_t* values);
2021-03-20 20:20:17 +00:00
void writeLogHeader(HardwareSerial &SerialRef);
void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake);
2021-03-02 22:48:20 +00:00
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
{
// Create command
scom.start = (uint16_t)START_FRAME;
scom.speedLeft = (int16_t)uSpeedLeft;
scom.speedRight = (int16_t)uSpeedRight;
scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight);
2021-03-02 22:48:20 +00:00
SerialRef.write((uint8_t *) &scom, sizeof(scom));
}
2021-03-02 22:48:20 +00:00
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef)
{
2021-03-02 22:48:20 +00:00
bool _result=1;
// Check for new data availability in the Serial buffer
2021-03-02 22:48:20 +00:00
if ( SerialRef.available() ) {
sread.incomingByte = SerialRef.read(); // Read the incoming byte
sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame
}
else {
2021-03-02 22:48:20 +00:00
return 0;
}
// If DEBUG_RX is defined print all incoming bytes
#ifdef DEBUG_RX
Serial.print(sread.incomingByte);
#endif
// Copy received data
if (sread.bufStartFrame == START_FRAME) { // Initialize if new data is detected
sread.p = (byte *)&NewFeedback;
*sread.p++ = sread.incomingBytePrev;
*sread.p++ = sread.incomingByte;
sread.idx = 2;
} else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) { // Save the new received data
*sread.p++ = sread.incomingByte;
sread.idx++;
}
// Check if we reached the end of the package
if (sread.idx == sizeof(SerialFeedback)) {
uint16_t checksum;
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed);
// Check validity of the new data
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
// Copy the new data
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
sread.lastValidDataSerial_time = millis();
2021-03-02 22:48:20 +00:00
} else {
_result=0;
}
sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
}
2021-03-02 22:48:20 +00:00
/*
// 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
sread.incomingBytePrev = sread.incomingByte;
return _result; //new data was available
}
// ########################## SETUP ##########################
void setup()
{
2021-03-20 20:20:17 +00:00
Serial.begin(SERIAL_BAUD); //Debug and Program
2021-03-20 20:20:17 +00:00
Serial1.begin(SERIAL_LOG_BAUD); //TX1=1, RX1=0
Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9
Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7
2021-03-20 15:13:16 +00:00
pinMode(PIN_THROTTLE, INPUT);
pinMode(PIN_BRAKE, INPUT);
pinMode(PIN_START, INPUT_PULLUP);
pinMode(PIN_LED_START, OUTPUT);
digitalWrite(PIN_LED_START,HIGH);
2021-03-20 15:13:16 +00:00
pinMode(PIN_MODE_LEDG, OUTPUT);
digitalWrite(PIN_MODE_LEDG,LOW);
2021-03-20 15:13:16 +00:00
pinMode(PIN_MODE_LEDR, OUTPUT);
digitalWrite(PIN_MODE_LEDR,LOW);
2021-03-20 15:13:16 +00:00
pinMode(PIN_LATCH_ENABLE, OUTPUT);
digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
pinMode(PIN_MODE_SWITCH, INPUT_PULLUP);
}
2021-03-02 22:48:20 +00:00
unsigned long loopmillis;
// ########################## LOOP ##########################
void loop() {
loopmillis=millis(); //read millis for this cycle
bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3);
2021-03-20 15:13:16 +00:00
if (loopmillis - last_adcread > ADCREADPERIOD) {
//read teensy adc and filter
last_adcread=loopmillis;
uint16_t throttle_raw = analogRead(PIN_THROTTLE);
throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain
uint16_t brake_raw = analogRead(PIN_BRAKE);
brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
if (brake_pos>0) { //pressed brake disables throttle
throttle_pos=0;
}
//Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
//Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
throttle_pos/=2;
2021-03-20 20:20:17 +00:00
digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
digitalWrite(PIN_MODE_LEDR,HIGH);
}else{ //button not pushed in
2021-03-20 20:20:17 +00:00
digitalWrite(PIN_MODE_LEDG,HIGH);
digitalWrite(PIN_MODE_LEDR,LOW); //Red
}
2021-03-20 15:13:16 +00:00
}
2021-03-20 20:20:17 +00:00
#define FEEDBACKRECEIVETIMEOUT 500
if ( (loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT) | (loopmillis > motorparamsRear.millis+FEEDBACKRECEIVETIMEOUT) ) { //timeout of at least one controller
throttle_pos=0;
brake_pos=0;
controllers_connected=false;
}else if(!controllers_connected) { //not timeouted but was before
controllers_connected=true;
writeLogHeader(Serial1); //connection recovered, write log header
}
2021-03-02 22:48:20 +00:00
if (newData2) {
motorparamsFront.cur_pos++;
motorparamsFront.cur_pos%=CURRENT_FILTER_SIZE;
motorparamsFront.curL_DC[motorparamsFront.cur_pos] = FeedbackFront.curL_DC;
motorparamsFront.curR_DC[motorparamsFront.cur_pos] = FeedbackFront.curR_DC;
2021-03-20 20:20:17 +00:00
motorparamsFront.millis=loopmillis;
log_update=true;
2021-03-02 22:48:20 +00:00
}
if (newData3) {
motorparamsRear.cur_pos++;
motorparamsRear.cur_pos%=CURRENT_FILTER_SIZE;
motorparamsRear.curL_DC[motorparamsRear.cur_pos] = FeedbackRear.curL_DC;
motorparamsRear.curR_DC[motorparamsRear.cur_pos] = FeedbackRear.curR_DC;
2021-03-20 20:20:17 +00:00
motorparamsRear.millis=loopmillis;
log_update=true;
}
if (loopmillis - last_send > SENDPERIOD) {
2021-03-20 15:13:16 +00:00
//Calculate motor stuff and send to motor controllers
last_send=loopmillis;
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle
#define MAXBREAKCURRENT 20
float brakepedal_current_multiplier=MAXBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
float freewheel_current=0.1+brake_pos*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
2021-03-20 20:20:17 +00:00
float freewheel_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
motorparamsFront.filtered_curL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
motorparamsFront.filtered_curR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
motorparamsRear.filtered_curL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
motorparamsRear.filtered_curR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps
2021-03-20 20:20:17 +00:00
float filtered_currentFront=min(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
float filtered_currentRear=min(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);
2021-03-20 20:20:17 +00:00
filtered_currentAll=min(filtered_currentFront,filtered_currentRear);
if (throttle_pos>=last_cmd_send) { //accelerating
cmd_send = throttle_pos; //if throttle higher than apply throttle directly
}else{ //freewheeling or braking
if (filtered_currentAll<freewheel_current) { //drive current too high
cmd_send-= max(0, (-filtered_currentAll+freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
}
cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways
cmd_send=constrain(cmd_send,0,1000);
}
last_cmd_send=cmd_send;
//apply throttle command to all motors
motorparamsFront.cmdL=cmd_send;
motorparamsFront.cmdR=cmd_send;
motorparamsRear.cmdL=cmd_send;
motorparamsRear.cmdR=cmd_send;
2021-03-09 18:45:36 +00:00
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
2021-03-20 20:20:17 +00:00
log_update=true;
//Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000); Serial.println();
}
2021-03-20 20:20:17 +00:00
#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
if (log_update && loopmillis>last_log_send+LOGMININTERVAL) {
last_log_send=loopmillis;
log_update=false;
writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
}
}
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
{
float a = *((float *)cmp1);
float b = *((float *)cmp2);
return a > b ? -1 : (a < b ? 1 : 0);
}
float filterMedian(int16_t* values) {
float copied_values[CURRENT_FILTER_SIZE];
for(int i=0;i<CURRENT_FILTER_SIZE;i++) {
copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
}
float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);
float mean=copied_values[CURRENT_FILTER_SIZE/2];
for (uint8_t i=1; i<=CURRENT_MEANVALUECOUNT;i++) {
mean+=copied_values[CURRENT_FILTER_SIZE/2-i]+copied_values[CURRENT_FILTER_SIZE/2+i]; //add two values around center
}
mean/=(1+CURRENT_MEANVALUECOUNT*2);
return mean;
2021-03-20 20:20:17 +00:00
}
void writeLogHeader(HardwareSerial &SerialRef) {
SerialRef.print("time, cmd_FrontL, cmd_FrontR, cmd_RearL, cmd_RearR, ");
SerialRef.print("current_FrontL, current_FrontR, current_RearL, current_RearR, ");
SerialRef.print("temp_Front, temp_Rear, vbat_Front, vbat_Rear, ");
SerialRef.print("speed_FrontL, speed_FrontR, speed_RearL, speed_RearR, ");
SerialRef.print("currentAll, throttle, brake"); Serial.println();
}
void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
{
SerialRef.print(time/1000.0); SerialRef.print(", "); //time in seconds
SerialRef.print(mpfront.cmdL); SerialRef.print(", ");
SerialRef.print(mpfront.cmdR); SerialRef.print(", ");
SerialRef.print(mprear.cmdL); SerialRef.print(", ");
SerialRef.print(mprear.cmdR); SerialRef.print(", ");
SerialRef.print(mpfront.filtered_curL,3); SerialRef.print(", ");
SerialRef.print(mpfront.filtered_curR,3); SerialRef.print(", ");
SerialRef.print(mprear.filtered_curL,3); SerialRef.print(", ");
SerialRef.print(mprear.filtered_curR,3); SerialRef.print(", ");
SerialRef.print(fbfront.speedL_meas); SerialRef.print(", ");
SerialRef.print(fbfront.speedR_meas); SerialRef.print(", ");
SerialRef.print(fbrear.speedL_meas); SerialRef.print(", ");
SerialRef.print(fbrear.speedR_meas); SerialRef.print(", ");
SerialRef.print(fbfront.boardTemp); SerialRef.print(", ");
SerialRef.print(fbrear.boardTemp); SerialRef.print(", ");
SerialRef.print(fbfront.batVoltage); SerialRef.print(", ");
SerialRef.print(fbrear.batVoltage); SerialRef.print(", ");
SerialRef.print(currentAll,3); SerialRef.print(", ");
SerialRef.print(throttle); SerialRef.print(", ");
SerialRef.print(brake); SerialRef.println();
}