separate files

This commit is contained in:
interfisch 2023-03-02 21:18:48 +01:00
parent f27e0079da
commit 3f78a5b8f2
5 changed files with 391 additions and 338 deletions

View File

@ -0,0 +1,162 @@
#ifndef _COMMS_H_
#define _COMMS_H_
#include "definitions.h"
#include "structs.h"
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis);
void writeLogInfo(HardwareSerial &SerialRef);
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);
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg);
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=false; //return true if new full data frame received
// 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 false; //nothing new
}
// 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();
_result=true;
} else {
_result=false;
}
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
}
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis) {
mp.cur_pos++;
mp.cur_pos%=CURRENT_FILTER_SIZE;
mp.curL_DC[mp.cur_pos] = -fb.curL_DC; //invert so positive current is consumed current. negative then means regenerated
mp.curR_DC[mp.cur_pos] = -fb.curR_DC;
mp.millis=_loopmillis;
log_update=true;
}
void writeLogInfo(HardwareSerial &SerialRef) { //first line of file
SerialRef.print("#TIMESTAMP:");
SerialRef.println(now());
}
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("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
SerialRef.println("currentAll,throttle,brake,speed,trip,currentConsumed");
}
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,3); 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(","); //invert speed, because left wheels are negated
SerialRef.print(-fbfront.speedR_meas); SerialRef.print(",");
SerialRef.print(fbrear.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
SerialRef.print(-fbrear.speedR_meas); SerialRef.print(",");
SerialRef.print(fbfront.boardTemp/10.0,1); SerialRef.print(","); //in degC
SerialRef.print(fbrear.boardTemp/10.0,1); SerialRef.print(","); //in degC
SerialRef.print(fbfront.batVoltage/100.0); SerialRef.print(","); //in V
SerialRef.print(fbrear.batVoltage/100.0); SerialRef.print(","); //in V
SerialRef.print(currentAll,3); SerialRef.print(",");
SerialRef.print(throttle); SerialRef.print(",");
SerialRef.print(brake); SerialRef.print(",");
SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
SerialRef.print(trip); SerialRef.print(","); //in m
SerialRef.print(currentConsumed,3); SerialRef.println(); //in Ah (Amphours)
}
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
{
SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
}
#endif

View File

@ -0,0 +1,124 @@
#ifndef _DEFINITIONS_H
#define _DEFINITIONS_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 SERIAL_LOG_BAUD 115200 // baud rate for logging output
bool log_update=true;
unsigned long last_log_send=0;
#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial
#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
#define LOGMAXINTERVAL 10000 //maximum time (ms) after which data is send
#define WRITE_HEADER_TIME 400 //just before FEEDBACKRECEIVETIMEOUT, so header gets written before error comments
bool log_header_written = false;
#define FEEDBACKRECEIVETIMEOUT 500
bool controllerFront_connected=false;
bool controllerRear_connected=false;
bool controllers_connected=false;
#define PIN_THROTTLE A7
//const uint16_t calib_throttle_min = 420; //better a bit too high than too low
//const uint16_t calib_throttle_max = 790;
const uint16_t failsafe_throttle_min = 4900; //if adc value falls below this failsafe is triggered. old 20
const uint16_t failsafe_throttle_max = 14000; //if adc value goes above this failsafe is triggered. old 1000
//const uint16_t throttleCurvePerMM[] = {414,460,490,511,527,539,548,555,561,567,573,578,584,590,599,611,630,657,697,754,789,795}; //adc values for every unit (mm) of linear travel
const uint16_t throttleCurvePerMM[] = {8485,8904,9177,9368,9513,9623,9705,9768,9823,9877,9932,9978,10032,10087,10169,10278,10451,10697,11061,11579,11898,11952}; //adc values for every unit (mm) of linear travel
#define PIN_BRAKE A8
const uint16_t calib_brake_min = 2000;//better a bit too high than too low
const uint16_t calib_brake_max = 11000;
const uint16_t failsafe_brake_min = 700; //if adc value falls below this failsafe is triggered
const uint16_t failsafe_brake_max = 13000; //if adc value goes above this failsafe is triggered
uint16_t ads_throttle_A_raw=0;
uint16_t ads_throttle_B_raw=0;
uint16_t ads_brake_raw=failsafe_brake_min;
uint16_t ads_control_raw=0;
int16_t throttle_pos=0;
int16_t brake_pos=0;
#define ADSREADPERIOD 3 //set slightly higher as actual read time to avoid unnecessary register query
#define ADCREADPERIOD 10
#define BUTTONREADPERIOD 20
unsigned long last_adsread=0; //needed for failcheck
uint16_t throttle_raw=failsafe_throttle_min; //start at min so that failsafe is not triggered
#define THROTTLE_ADC_FILTER 0.15 //higher value = faster response
uint16_t brake_raw=failsafe_brake_min; //start at min so that failsafe is not triggered
#define ADC_OUTOFRANGE_TIME 100
unsigned long throttle_ok_time=0;
unsigned long brake_ok_time=0;
bool error_throttle_outofrange=false;
bool error_brake_outofrange=false;
bool error_ads_max_read_interval=false;
#define REVERSE_ENABLE_TIME 1000 //ms. how long standstill to be able to drive backward
#define REVERSE_SPEED 0.25 //reverse driving speed //0 to 1
#define NORMAL_MAX_ACCELERATION_RATE 10000
#define SLOW_MAX_ACCELERATION_RATE 500
int16_t max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; //maximum cmd send increase per second
float meanSpeedms=0;
float trip=0; //trip distance in meters
float wheelcircumference=0.5278; //wheel diameter in m. 8.4cm radius -> 0.084m*2*Pi
float currentConsumed=0; //Ah
//Driving parameters
int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling)
float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
bool reverse_enabled=false;
unsigned long last_notidle=0; //not rolling to fast, no pedal pressed
#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
unsigned long last_send = 0;
unsigned long last_receive = 0;
float filtered_currentAll=0;
int16_t cmd_send=0;
int16_t last_cmd_send=0;
uint8_t speedmode=0;
#define SPEEDMODE_SLOW 1
#define SPEEDMODE_NORMAL 0
unsigned long button_start_lastchange=0;
bool button_start_state=false;
#define LONG_PRESS_ARMING_TIME 2000
#define DEBOUNCE_TIME 50
bool armed = false; //cmd output values forced to 0 if false
#define CURRENT_FILTER_SIZE 60 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
#define CURRENT_MEANVALUECOUNT 20 //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
#endif

View File

@ -0,0 +1,34 @@
#ifndef _HELPFUNCTIONS_H_
#define _HELPFUNCTIONS_H_
#include "definitions.h"
int sort_desc(const void *cmp1, const void *cmp2);
float filterMedian(int16_t* values);
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;
}
#endif

View File

@ -0,0 +1,60 @@
#ifndef _STRUCTS_H_
#define _STRUCTS_H_
// 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{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedL_meas; //left speed is positive when driving forward
int16_t speedR_meas; //right speed is negatie when driving forward
int16_t batVoltage;
int16_t boardTemp;
int16_t curL_DC; //negative values are current consumed. positive values mean generated current
int16_t curR_DC;
uint16_t cmdLed;
uint16_t checksum;
} SerialFeedback;
SerialFeedback FeedbackFront;
SerialFeedback NewFeedbackFront;
SerialFeedback FeedbackRear;
SerialFeedback NewFeedbackRear;
typedef struct{
int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current
int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
uint8_t cur_pos=0;
int16_t cmdL=0;
int16_t cmdR=0;
float filtered_curL=0;
float filtered_curR=0;
unsigned long millis=0; //time when last message received
} MotorParameter;
MotorParameter motorparamsFront;
MotorParameter motorparamsRear;
#endif

View File

@ -1,6 +1,12 @@
#include <Arduino.h>
#include "definitions.h"
#include "structs.h"
#include "helpfunctions.h"
#include <TimeLib.h> //for teensy rtc
#include "comms.h"
#include "ADS1X15.h"
ADS1115 ADS(0x48);
@ -14,280 +20,29 @@ Tennsy Pin, Pin Name, Connected to
7, Rx3, Hoverboard TX(Blue)
*/
// ########################## 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 SERIAL_LOG_BAUD 115200 // baud rate for logging output
bool log_update=true;
unsigned long last_log_send=0;
#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial
#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
#define LOGMAXINTERVAL 10000 //maximum time (ms) after which data is send
#define WRITE_HEADER_TIME 400 //just before FEEDBACKRECEIVETIMEOUT, so header gets written before error comments
bool log_header_written = false;
#define FEEDBACKRECEIVETIMEOUT 500
bool controllerFront_connected=false;
bool controllerRear_connected=false;
bool controllers_connected=false;
#define PIN_THROTTLE A7
//const uint16_t calib_throttle_min = 420; //better a bit too high than too low
//const uint16_t calib_throttle_max = 790;
const uint16_t failsafe_throttle_min = 4900; //if adc value falls below this failsafe is triggered. old 20
const uint16_t failsafe_throttle_max = 14000; //if adc value goes above this failsafe is triggered. old 1000
//const uint16_t throttleCurvePerMM[] = {414,460,490,511,527,539,548,555,561,567,573,578,584,590,599,611,630,657,697,754,789,795}; //adc values for every unit (mm) of linear travel
const uint16_t throttleCurvePerMM[] = {8485,8904,9177,9368,9513,9623,9705,9768,9823,9877,9932,9978,10032,10087,10169,10278,10451,10697,11061,11579,11898,11952}; //adc values for every unit (mm) of linear travel
#define PIN_BRAKE A8
const uint16_t calib_brake_min = 2000;//better a bit too high than too low
const uint16_t calib_brake_max = 11000;
const uint16_t failsafe_brake_min = 700; //if adc value falls below this failsafe is triggered
const uint16_t failsafe_brake_max = 13000; //if adc value goes above this failsafe is triggered
uint16_t ads_throttle_A_raw=0;
uint16_t ads_throttle_B_raw=0;
uint16_t ads_brake_raw=failsafe_brake_min;
uint16_t ads_control_raw=0;
int16_t throttle_pos=0;
int16_t brake_pos=0;
#define ADSREADPERIOD 3 //set slightly higher as actual read time to avoid unnecessary register query
#define ADCREADPERIOD 10
#define BUTTONREADPERIOD 20
unsigned long last_adsread=0; //needed for failcheck
uint16_t throttle_raw=failsafe_throttle_min; //start at min so that failsafe is not triggered
#define THROTTLE_ADC_FILTER 0.15 //higher value = faster response
uint16_t brake_raw=failsafe_brake_min; //start at min so that failsafe is not triggered
#define ADC_OUTOFRANGE_TIME 100
unsigned long throttle_ok_time=0;
unsigned long brake_ok_time=0;
bool error_throttle_outofrange=false;
bool error_brake_outofrange=false;
bool error_ads_max_read_interval=false;
#define REVERSE_ENABLE_TIME 1000 //ms. how long standstill to be able to drive backward
#define REVERSE_SPEED 0.25 //reverse driving speed //0 to 1
#define NORMAL_MAX_ACCELERATION_RATE 10000
#define SLOW_MAX_ACCELERATION_RATE 500
int16_t max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; //maximum cmd send increase per second
float meanSpeedms=0;
float trip=0; //trip distance in meters
float wheelcircumference=0.5278; //wheel diameter in m. 8.4cm radius -> 0.084m*2*Pi
float currentConsumed=0; //Ah
//Driving parameters
int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling)
float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
bool reverse_enabled=false;
unsigned long last_notidle=0; //not rolling to fast, no pedal pressed
#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
unsigned long last_send = 0;
unsigned long last_receive = 0;
float filtered_currentAll=0;
int16_t cmd_send=0;
int16_t last_cmd_send=0;
uint8_t speedmode=0;
#define SPEEDMODE_SLOW 1
#define SPEEDMODE_NORMAL 0
unsigned long button_start_lastchange=0;
bool button_start_state=false;
#define LONG_PRESS_ARMING_TIME 2000
#define DEBOUNCE_TIME 50
bool armed = false; //cmd output values forced to 0 if false
// 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{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedL_meas; //left speed is positive when driving forward
int16_t speedR_meas; //right speed is negatie when driving forward
int16_t batVoltage;
int16_t boardTemp;
int16_t curL_DC; //negative values are current consumed. 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 60 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
#define CURRENT_MEANVALUECOUNT 20 //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}; //current will be inverted for this so positive value means consumed current
int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
uint8_t cur_pos=0;
int16_t cmdL=0;
int16_t cmdR=0;
float filtered_curL=0;
float filtered_curR=0;
unsigned long millis=0; //time when last message received
} MotorParameter;
MotorParameter motorparamsFront;
MotorParameter motorparamsRear;
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 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);
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg);
void readADS();
void readADC();
void failChecks();
void sendCMD();
void checkLog();
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb);
void leds();
void readButtons();
uint16_t linearizeThrottle(uint16_t v);
#include <TimeLib.h> //for teensy rtc
time_t getTeensy3Time();
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=false; //return true if new full data frame received
// 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 false; //nothing new
}
// 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();
_result=true;
} else {
_result=false;
}
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()
@ -376,10 +131,10 @@ void loop() {
//Max (40) or 22 available/pending bytes
if (newData2) {
updateMotorparams(motorparamsFront,FeedbackFront);
updateMotorparams(motorparamsFront,FeedbackFront,loopmillis);
}
if (newData3) {
updateMotorparams(motorparamsRear,FeedbackRear);
updateMotorparams(motorparamsRear,FeedbackRear,loopmillis);
}
@ -433,88 +188,13 @@ void loop() {
}
// ##### HELPFUNCTIONS
time_t getTeensy3Time()
{
return Teensy3Clock.get();
}
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;
}
void writeLogInfo(HardwareSerial &SerialRef) { //first line of file
SerialRef.print("#TIMESTAMP:");
SerialRef.println(now());
}
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("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
SerialRef.println("currentAll,throttle,brake,speed,trip,currentConsumed");
}
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,3); 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(","); //invert speed, because left wheels are negated
SerialRef.print(-fbfront.speedR_meas); SerialRef.print(",");
SerialRef.print(fbrear.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
SerialRef.print(-fbrear.speedR_meas); SerialRef.print(",");
SerialRef.print(fbfront.boardTemp/10.0,1); SerialRef.print(","); //in degC
SerialRef.print(fbrear.boardTemp/10.0,1); SerialRef.print(","); //in degC
SerialRef.print(fbfront.batVoltage/100.0); SerialRef.print(","); //in V
SerialRef.print(fbrear.batVoltage/100.0); SerialRef.print(","); //in V
SerialRef.print(currentAll,3); SerialRef.print(",");
SerialRef.print(throttle); SerialRef.print(",");
SerialRef.print(brake); SerialRef.print(",");
SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
SerialRef.print(trip); SerialRef.print(","); //in m
SerialRef.print(currentConsumed,3); SerialRef.println(); //in Ah (Amphours)
}
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
{
SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
}
void readADS() { //sequentially read ads and write to variable
@ -760,14 +440,7 @@ void checkLog() {
}
}
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb) {
mp.cur_pos++;
mp.cur_pos%=CURRENT_FILTER_SIZE;
mp.curL_DC[mp.cur_pos] = -fb.curL_DC; //invert so positive current is consumed current. negative then means regenerated
mp.curR_DC[mp.cur_pos] = -fb.curR_DC;
mp.millis=loopmillis;
log_update=true;
}
void leds() {
//Start LED