582 lines
22 KiB
C++
582 lines
22 KiB
C++
#include <Arduino.h>
|
|
|
|
|
|
/*
|
|
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 ##########################
|
|
#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 1000
|
|
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 = 400; //better a bit too high than too low
|
|
const uint16_t calib_throttle_max = 790;
|
|
const uint16_t failsafe_throttle_min = 20; //if adc value falls below this failsafe is triggered
|
|
const uint16_t failsafe_throttle_max = 1000; //if adc value goes above this failsafe is triggered
|
|
#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;
|
|
const uint16_t failsafe_brake_min = 20; //if adc value falls below this failsafe is triggered
|
|
const uint16_t failsafe_brake_max = 1000; //if adc value goes above this failsafe is triggered
|
|
|
|
int16_t throttle_pos=0;
|
|
int16_t brake_pos=0;
|
|
|
|
unsigned long last_adcread=0;
|
|
#define ADCREADPERIOD 10
|
|
uint16_t throttle_raw=0;
|
|
uint16_t brake_raw=0;
|
|
#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;
|
|
|
|
#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{
|
|
uint16_t start;
|
|
int16_t cmd1;
|
|
int16_t cmd2;
|
|
int16_t speedL_meas; //left speed is negative when driving forward
|
|
int16_t speedR_meas; //right speed is positive 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 20 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
|
|
#define CURRENT_MEANVALUECOUNT 4 //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, String msg);
|
|
|
|
void readADC();
|
|
void failChecks();
|
|
void sendCMD();
|
|
void checkLog();
|
|
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb);
|
|
void leds();
|
|
|
|
void readButtons();
|
|
|
|
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
|
|
|
|
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
|
|
|
|
pinMode(PIN_THROTTLE, INPUT);
|
|
pinMode(PIN_BRAKE, INPUT);
|
|
pinMode(PIN_START, INPUT_PULLUP); //Pressed=High
|
|
|
|
pinMode(PIN_LED_START, OUTPUT); //Active High
|
|
|
|
pinMode(PIN_MODE_LEDG, OUTPUT); //Active Low
|
|
digitalWrite(PIN_MODE_LEDG,LOW);
|
|
pinMode(PIN_MODE_LEDR, OUTPUT); //Active Low
|
|
digitalWrite(PIN_MODE_LEDR,LOW);
|
|
|
|
pinMode(PIN_LATCH_ENABLE, OUTPUT);
|
|
digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
|
|
pinMode(PIN_MODE_SWITCH, INPUT_PULLUP);
|
|
}
|
|
|
|
|
|
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);
|
|
|
|
if (newData2) {
|
|
updateMotorparams(motorparamsFront,FeedbackFront);
|
|
}
|
|
if (newData3) {
|
|
updateMotorparams(motorparamsRear,FeedbackRear);
|
|
}
|
|
|
|
|
|
if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
|
|
last_adcread=loopmillis;
|
|
readADC();
|
|
|
|
readButtons();
|
|
}
|
|
|
|
|
|
failChecks();
|
|
|
|
if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers
|
|
last_send=loopmillis;
|
|
sendCMD();
|
|
}
|
|
|
|
//If needed write log to serial port
|
|
checkLog();
|
|
|
|
leds();
|
|
|
|
}
|
|
|
|
|
|
// ##### HELPFUNCTIONS
|
|
|
|
|
|
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 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("speed_FrontL,speed_FrontR,speed_RearL,speed_RearR,");
|
|
SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
|
|
SerialRef.println("currentAll,throttle,brake");
|
|
}
|
|
|
|
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(","); //invert. positive current is drive current
|
|
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); SerialRef.print(","); //in degC
|
|
SerialRef.print(fbrear.boardTemp/10.0); 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(","); //invert. positive current is drive current
|
|
SerialRef.print(throttle); SerialRef.print(",");
|
|
SerialRef.print(brake); SerialRef.println();
|
|
}
|
|
|
|
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
|
|
{
|
|
SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
|
|
}
|
|
|
|
|
|
|
|
// #### LOOPFUNCTIONS
|
|
|
|
|
|
void readADC() {
|
|
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
|
|
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
|
|
|
|
int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake
|
|
throttle_pos=constrain(throttlebreak_pos,0,1000);
|
|
brake_pos=constrain(-throttlebreak_pos/2,0,1000); //rescale brake value from throttlebreak_pos
|
|
|
|
//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
|
|
speedmode=SPEEDMODE_SLOW;
|
|
}else{ //button not pushed in
|
|
speedmode=SPEEDMODE_NORMAL;
|
|
}
|
|
|
|
|
|
if (speedmode==SPEEDMODE_SLOW) {
|
|
throttle_pos/=2;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void failChecks() {
|
|
if ( loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected
|
|
if (controllerFront_connected) { //just got disconnected
|
|
controllerFront_connected=false;
|
|
writeLogComment(Serial1,loopmillis, "Controller Front feedback timeout");
|
|
}
|
|
}else if(!controllerFront_connected) { //not timeouted but was before
|
|
controllerFront_connected=true;
|
|
writeLogComment(Serial1,loopmillis, "Controller Front connected");
|
|
}
|
|
|
|
if ( loopmillis > motorparamsRear.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected
|
|
if (controllerRear_connected) { //just got disconnected
|
|
controllerRear_connected=false;
|
|
writeLogComment(Serial1,loopmillis, "Controller Rear feedback timeout");
|
|
}
|
|
}else if(!controllerRear_connected) { //not timeouted but was before
|
|
controllerRear_connected=true;
|
|
writeLogComment(Serial1,loopmillis, "Controller Rear connected");
|
|
}
|
|
|
|
controllers_connected=controllerFront_connected & controllerRear_connected;
|
|
|
|
//ADC Range Check
|
|
if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //inside safe range (to check if wire got disconnected)
|
|
throttle_ok_time=loopmillis;
|
|
}
|
|
if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
|
|
if (!error_throttle_outofrange) {
|
|
error_throttle_outofrange=true;
|
|
writeLogComment(Serial1,loopmillis, "Error Throttle ADC Out of Range");
|
|
}
|
|
}
|
|
if ((brake_raw >= failsafe_brake_min) & (brake_raw <= failsafe_brake_max)) { //outside safe range. maybe wire got disconnected
|
|
brake_ok_time=loopmillis;
|
|
}
|
|
if (loopmillis>brake_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
|
|
if(!error_brake_outofrange) {
|
|
error_brake_outofrange=true;
|
|
writeLogComment(Serial1,loopmillis, "Error Brake ADC Out of Range");
|
|
}
|
|
}
|
|
|
|
|
|
|
|
if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange) { //any errors?
|
|
throttle_pos=0;
|
|
brake_pos=0;
|
|
}
|
|
}
|
|
|
|
void sendCMD() {
|
|
#define MINIMUM_CONSTANT_CMD_REDUCE 1 //reduce cmd every loop by this constant amount when freewheeling/braking
|
|
#define BREAK_CMDREDUCE_PROPORTIONAL 500 //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=BREAK_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)
|
|
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,(int16_t)(BREAK_CMDREDUCE_PROPORTIONAL*SENDPERIOD/1000)); //reduce cmd value every cycle
|
|
#define STARTBREAKCURRENT 5 //Ampere. at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
|
|
#define STARTBREAKCURRENT_OFFSET 0.1 //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
|
|
float brakepedal_current_multiplier=STARTBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
|
|
|
|
float freewheel_current=STARTBREAKCURRENT_OFFSET-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
|
|
float freewheel_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (on 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
|
|
|
|
float filtered_currentFront=max(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
|
|
float filtered_currentRear=max(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);
|
|
|
|
filtered_currentAll=max(filtered_currentFront,filtered_currentRear); //positive value is current Drawn from battery. negative value is braking current
|
|
|
|
if (throttle_pos>=last_cmd_send) { //accelerating
|
|
cmd_send = throttle_pos; //if throttle higher than last applied value, 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(MINIMUM_CONSTANT_CMD_REDUCE,cmdreduce_constant); //reduce slowly anyways
|
|
cmd_send=constrain(cmd_send,0,1000);
|
|
}
|
|
|
|
if (!controllers_connected || !armed) { //controllers not connected or not armed
|
|
cmd_send=0; //safety off
|
|
}
|
|
|
|
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;
|
|
|
|
if (controllers_connected) {
|
|
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
|
|
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
|
|
|
|
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()
|
|
}
|
|
}
|
|
|
|
|
|
void checkLog() {
|
|
if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){ //write header for log file after logger booted up
|
|
writeLogHeader(Serial1);
|
|
log_header_written=true;
|
|
}
|
|
|
|
if (log_header_written && ( (log_update && loopmillis>last_log_send+LOGMININTERVAL) || loopmillis>last_log_send+LOGMAXINTERVAL) ) {
|
|
last_log_send=loopmillis;
|
|
log_update=false;
|
|
writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
|
|
}
|
|
}
|
|
|
|
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
|
|
if (!armed) { //disarmed
|
|
digitalWrite(PIN_LED_START,((loopmillis/1000)%2 == 0)); //high is on for LED_START. blink every second. loopmillis 0 - 1000 led is on.
|
|
}else{ //armed
|
|
digitalWrite(PIN_LED_START,HIGH); //LED On
|
|
}
|
|
|
|
|
|
if (speedmode==SPEEDMODE_SLOW) {
|
|
digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
|
|
digitalWrite(PIN_MODE_LEDR,HIGH);
|
|
}else if (speedmode==SPEEDMODE_NORMAL) {
|
|
digitalWrite(PIN_MODE_LEDG,HIGH);
|
|
digitalWrite(PIN_MODE_LEDR,LOW); //Red
|
|
}
|
|
}
|
|
|
|
void readButtons() {
|
|
|
|
if (loopmillis > button_start_lastchange+DEBOUNCE_TIME) { //wait some time after last change
|
|
if (digitalRead(PIN_START) && !button_start_state) { //start engine button pressed and was not pressed before
|
|
button_start_state=true; //pressed
|
|
button_start_lastchange=loopmillis; //save time for debouncing
|
|
}else if (!digitalRead(PIN_START) && button_start_state) { //released an was pressed before
|
|
button_start_state=false; // not pressed
|
|
button_start_lastchange=loopmillis; //save time for debouncing
|
|
}
|
|
}
|
|
|
|
if (button_start_state) { //pressed
|
|
if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
|
|
if (throttle_pos<=0 && brake_pos<=0 && !armed) { //brake or thottle not pressed
|
|
armed=true; //arm if button pressed long enough
|
|
writeLogComment(Serial1,loopmillis, "Armed by button");
|
|
}
|
|
}else if (armed){ //not pressed long enough and is armed
|
|
armed=false; //disarm
|
|
writeLogComment(Serial1,loopmillis, "Disarmed by button");
|
|
}
|
|
}
|
|
|
|
} |