247 lines
8.8 KiB
C++
247 lines
8.8 KiB
C++
#include <Arduino.h>
|
|
|
|
// ########################## DEFINES ##########################
|
|
#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)
|
|
#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication
|
|
|
|
|
|
#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
|
|
#define RECEIVEPERIOD 50 //ms
|
|
|
|
#define PIN_THROTTLE A0
|
|
const uint16_t calib_throttle_min = 350;
|
|
const uint16_t calib_throttle_max = 810;
|
|
|
|
|
|
unsigned long last_send = 0;
|
|
unsigned long last_receive = 0;
|
|
|
|
float avg_currentL=0;
|
|
float avg_currentR=0;
|
|
|
|
int16_t 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;
|
|
|
|
|
|
typedef struct{
|
|
uint16_t start;
|
|
int16_t speedLeft;
|
|
int16_t speedRight;
|
|
uint16_t checksum;
|
|
} SerialCommand;
|
|
SerialCommand CommandFront;
|
|
|
|
|
|
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;
|
|
|
|
#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;
|
|
} MotorParameter;
|
|
MotorParameter motorparamsFront;
|
|
|
|
|
|
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);
|
|
|
|
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);
|
|
|
|
SerialRef.write((uint8_t *) &scom, sizeof(scom));
|
|
|
|
}
|
|
|
|
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef)
|
|
{
|
|
bool _result=1;
|
|
// Check for new data availability in the Serial buffer
|
|
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 {
|
|
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();
|
|
} else {
|
|
_result=0;
|
|
}
|
|
sread.idx = 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
|
|
sread.incomingBytePrev = sread.incomingByte;
|
|
|
|
return _result; //new data was available
|
|
}
|
|
|
|
// ########################## SETUP ##########################
|
|
void setup()
|
|
{
|
|
|
|
Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
|
|
|
|
Serial2.begin(SERIAL_CONTROL_BAUD); //control
|
|
Serial3.begin(SERIAL_CONTROL_BAUD); //control
|
|
|
|
pinMode(PIN_THROTTLE, INPUT_PULLUP);
|
|
|
|
}
|
|
|
|
|
|
unsigned long loopmillis;
|
|
// ########################## LOOP ##########################
|
|
void loop() {
|
|
loopmillis=millis(); //read millis for this cycle
|
|
|
|
bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
|
|
|
|
|
|
//Serial.print("fo="); Serial.println(count);
|
|
//count++;
|
|
|
|
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;
|
|
}
|
|
|
|
if (loopmillis - last_send > SENDPERIOD) {
|
|
last_send=loopmillis;
|
|
|
|
uint16_t throttle_raw = analogRead(PIN_THROTTLE);
|
|
int16_t throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain
|
|
|
|
|
|
float freewheel_current=0.1;
|
|
float freewheel_break_factor=1000.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
|
|
float filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0;
|
|
float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0;
|
|
|
|
float filtered_currentAll=(filtered_curFL+filtered_curFR)/2.0; //mean current of all motors
|
|
|
|
if (throttle_pos>=motorparamsFront.cmdR) { //accelerating
|
|
cmd_send = throttle_pos; //if throttle higher than apply throttle directly
|
|
}else{ //freewheeling or braking
|
|
if (-filtered_currentAll>freewheel_current) { //breaking current high enough
|
|
cmd_send-= max(0, (-filtered_currentAll-freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor
|
|
}
|
|
cmd_send-=1; //reduce slowly anyways
|
|
cmd_send=constrain(cmd_send,0,1000);
|
|
}
|
|
|
|
//apply throttle command to all motors
|
|
motorparamsFront.cmdL=cmd_send;
|
|
motorparamsFront.cmdR=cmd_send;
|
|
|
|
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
|
|
|
|
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();
|
|
|
|
}
|
|
}
|
|
|
|
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;
|
|
} |