separate files
This commit is contained in:
parent
f27e0079da
commit
3f78a5b8f2
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue