666 lines
22 KiB
C++
666 lines
22 KiB
C++
#include <Arduino.h>
|
|
|
|
#include "led.h"
|
|
|
|
#include "definitions.h"
|
|
//#include "structs.h"
|
|
#include "helpfunctions.h"
|
|
#include <TimeLib.h> //for teensy rtc
|
|
#include "hoverboard-esc-serial-comm.h"
|
|
//#include "comms.h"
|
|
#include "display.h"
|
|
#include "logging.h"
|
|
#include "ADS1X15.h"
|
|
|
|
|
|
|
|
|
|
|
|
ESCSerialComm escFront(Serial2);
|
|
ESCSerialComm escRear(Serial3);
|
|
//Serial1 = TX1=1, RX1=0
|
|
//Serial2 = TX2=10, RX2=9
|
|
//Serial3 = TX3=8, RX3=7
|
|
|
|
ADS1115 ADS(0x48);
|
|
|
|
/*
|
|
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)
|
|
*/
|
|
|
|
|
|
|
|
void readADS();
|
|
void readADC();
|
|
void failChecks();
|
|
//void sendCMD();
|
|
void calculateSetSpeed();
|
|
void checkLog();
|
|
|
|
void leds();
|
|
|
|
void readButtons();
|
|
|
|
uint16_t linearizeThrottle(uint16_t v);
|
|
|
|
|
|
time_t getTeensy3Time();
|
|
|
|
|
|
// ########################## 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
|
|
|
|
|
|
//TODO: remove mode button things
|
|
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
|
|
|
|
|
|
init_led();
|
|
led_testLEDSBlocking();
|
|
|
|
|
|
delay(2000);
|
|
Serial.println("Init Functions");
|
|
led_simpeProgress(0,1);
|
|
|
|
bool initResult=false;
|
|
|
|
initResult=display_init();
|
|
led_simpeProgress(1,initResult);
|
|
|
|
|
|
|
|
initResult=initLogging();
|
|
led_simpeProgress(2,initResult);
|
|
|
|
escFront.init();
|
|
led_simpeProgress(3,true);
|
|
escRear.init();
|
|
led_simpeProgress(4,true);
|
|
|
|
delay(2000);
|
|
Serial.println("Wait finished. Booting..");
|
|
led_simpeProgress(5,true);
|
|
|
|
//init ADS1115
|
|
if (!ADS.begin()) {
|
|
Serial.println("Error:"); delay(2000); Serial.println("ADS1115 Init Error!");
|
|
led_simpeProgress(6,false);
|
|
writeLogComment((unsigned long)millis(), "Error ADS1115 Init");
|
|
}else{
|
|
ADS.setGain(0);
|
|
ADS.setDataRate(7);// Read Interval: 7-> 2ms, 6-> 3-4ms , 5-> 5-6ms, 4-> 9ms, 0-> 124ms
|
|
// also set ADSREADPERIOD to at least the read interval
|
|
ADS.requestADC(0); //Start requesting a channel
|
|
led_simpeProgress(6,true);
|
|
}
|
|
delay(10);
|
|
|
|
|
|
setSyncProvider(getTeensy3Time); //See https://www.pjrc.com/teensy/td_libs_Time.html#teensy3
|
|
if (timeStatus()!= timeSet) {
|
|
Serial.println("Unable to sync with the RTC");
|
|
led_simpeProgress(7,false);
|
|
} else {
|
|
Serial.println("RTC has set the system time");
|
|
led_simpeProgress(7,true);
|
|
}
|
|
|
|
|
|
|
|
writeLogComment(millis(), "Setup Finished");
|
|
led_simpeProgress(15,true);
|
|
|
|
led_simpleProgressWait(); //wait longer if any errors were displayed with led_simpeProgress()
|
|
}
|
|
|
|
unsigned long loopmillis;
|
|
|
|
// ########################## LOOP ##########################
|
|
void loop() {
|
|
//Serial.print("Loopduration="); Serial.println(); //loopduration is at max 11ms
|
|
|
|
loopmillis=millis(); //read millis for this cycle
|
|
|
|
|
|
/*
|
|
// ____ Debugging pending serial byted for feedback
|
|
static int s2availmax=0;
|
|
int _a2=Serial2.available();
|
|
if (_a2>s2availmax) {
|
|
s2availmax=_a2;
|
|
//Serial.print("new s2availmax"); Serial.println(s2availmax);
|
|
String _text="Serial2 Bytes Available Max=";
|
|
_text+=s2availmax;
|
|
writeLogComment(Serial1,loopmillis, _text);
|
|
}
|
|
|
|
static int s3availmax=0;
|
|
int _a3=Serial3.available();
|
|
if (_a3>s3availmax) {
|
|
s3availmax=_a3;
|
|
//Serial.print("new s3availmax"); Serial.println(s3availmax);
|
|
String _text="Serial3 Bytes Available Max=";
|
|
_text+=s3availmax;
|
|
writeLogComment(Serial1,loopmillis, _text);
|
|
}
|
|
// ----- End of debug
|
|
|
|
|
|
|
|
bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
|
|
bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3);
|
|
//Max (40) or 22 available/pending bytes
|
|
|
|
if (newData2) {
|
|
updateMotorparams(motorparamsFront,FeedbackFront,loopmillis);
|
|
}
|
|
if (newData3) {
|
|
updateMotorparams(motorparamsRear,FeedbackRear,loopmillis);
|
|
}
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
if (loopmillis - last_adsread > ADSREADPERIOD) { //read teensy adc and filter
|
|
last_adsread=loopmillis;
|
|
if (ADS.isBusy() == false) //reads a register on ads
|
|
{
|
|
readADS();
|
|
}else{
|
|
Serial.println("Unnecessary ADS poll. Increase ADSREADPERIOD");
|
|
}
|
|
}
|
|
|
|
static unsigned long last_adcread=0;
|
|
if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
|
|
last_adcread=loopmillis;
|
|
|
|
readADC();
|
|
}
|
|
|
|
static unsigned long last_buttonread=0;
|
|
if (loopmillis - last_buttonread > BUTTONREADPERIOD) { //read digital input states
|
|
last_buttonread=loopmillis;
|
|
readButtons();
|
|
}
|
|
|
|
|
|
failChecks();
|
|
|
|
static unsigned long last_calculateSetSpeed=0;
|
|
if (loopmillis - last_calculateSetSpeed > SENDPERIOD) {
|
|
calculateSetSpeed();
|
|
}
|
|
|
|
escFront.update(loopmillis);
|
|
escRear.update(loopmillis);
|
|
|
|
/* TODO: remove this if, because everything contained in esc.update()
|
|
if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers
|
|
last_send=loopmillis;
|
|
//sendCMD();
|
|
|
|
|
|
//Update speed and trip
|
|
float _meanRPM=(FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas)/4.0;
|
|
meanSpeedms=_meanRPM*wheelcircumference/60.0; // Units: 1/min * m / 60s
|
|
trip+=abs(meanSpeedms)* (SENDPERIOD/1000.0);
|
|
|
|
//mah consumed
|
|
currentConsumed += (motorparamsFront.filtered_curL+motorparamsFront.filtered_curR+motorparamsRear.filtered_curL+motorparamsRear.filtered_curR)* (SENDPERIOD/1000.0)/3600.0; //amp hours
|
|
}
|
|
*/
|
|
|
|
//If needed write log to serial port
|
|
//checkLog(); //TODO remove
|
|
loggingLoop(loopmillis,escFront,escRear);
|
|
|
|
leds();
|
|
led_update(loopmillis,escFront,escRear); //ws2812 led ring
|
|
|
|
static unsigned long last_display_update=0;
|
|
if (loopmillis - last_display_update > DISPLAYUPDATEPERIOD) {
|
|
last_display_update=loopmillis;
|
|
display_update();
|
|
}
|
|
}
|
|
|
|
|
|
time_t getTeensy3Time()
|
|
{
|
|
return Teensy3Clock.get();
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void readADS() { //sequentially read ads and write to variable
|
|
/*static unsigned long _lastReadADS=0;
|
|
Serial.print("readADS Interval="); Serial.println(millis()-_lastReadADS);
|
|
_lastReadADS=millis();*/
|
|
static uint8_t ads_input_switch=0;
|
|
|
|
int16_t ads_val = ADS.getValue(); //get value from last selected channel
|
|
|
|
|
|
switch (ads_input_switch) {
|
|
case 0: //Throttle Sensor A
|
|
ads_throttle_A_raw=ads_val;
|
|
break;
|
|
case 1: //Throttle Sensor B
|
|
ads_throttle_B_raw=ads_val;
|
|
break;
|
|
case 2: //Brake
|
|
ads_brake_raw=ads_val;
|
|
break;
|
|
case 3: //Buttons TODO
|
|
ads_control_raw=ads_val;
|
|
break;
|
|
}
|
|
|
|
ads_input_switch++;
|
|
ads_input_switch%=4; //max 4 channels
|
|
ADS.requestADC(ads_input_switch); // request a new one
|
|
}
|
|
|
|
|
|
// #### LOOPFUNCTIONS
|
|
|
|
|
|
void readADC() {
|
|
/*Serial.print(ads_throttle_A_raw); Serial.print('\t');
|
|
Serial.print(ads_throttle_B_raw); Serial.print('\t');
|
|
Serial.print(ads_brake_raw); Serial.print('\t');
|
|
Serial.print(ads_control_raw); Serial.println();*/
|
|
throttle_raw = ads_throttle_A_raw*THROTTLE_ADC_FILTER + throttle_raw*(1-THROTTLE_ADC_FILTER); //apply filter
|
|
|
|
//maps throttle curve to be linear
|
|
throttle_pos=max(0,min(1000,linearizeThrottle(throttle_raw))); //map and constrain
|
|
|
|
brake_raw=ads_brake_raw;
|
|
brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
|
|
//brake_pos = (int16_t)(pow((brake_pos/1000.0),2)*1000);
|
|
|
|
|
|
|
|
if (throttle_pos>0 || meanSpeedms>0.5 || (!reverse_enabled && brake_pos>0)) { //reset idle time on these conditions (disables reverse driving)
|
|
last_notidle=loopmillis;
|
|
reverse_enabled=false;
|
|
}
|
|
|
|
if (loopmillis-last_notidle > REVERSE_ENABLE_TIME) {
|
|
reverse_enabled=true;
|
|
}
|
|
|
|
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
|
|
if (speedmode!=SPEEDMODE_SLOW) {
|
|
speedmode=SPEEDMODE_SLOW;
|
|
max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE;
|
|
if (loopmillis>WRITE_HEADER_TIME) {
|
|
//writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_SLOW");
|
|
}
|
|
}
|
|
}else{ //button not pushed in
|
|
if (speedmode!=SPEEDMODE_NORMAL) {
|
|
speedmode=SPEEDMODE_NORMAL;
|
|
max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE;
|
|
if (loopmillis>WRITE_HEADER_TIME) {
|
|
//writeLogComment(Serial1,loopmillis, "Mode switched to 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");
|
|
//Serial.println("Controller Front feedback timeout");
|
|
}
|
|
}else if(!controllerFront_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //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");
|
|
//Serial.println("Controller Rear feedback timeout");
|
|
}
|
|
}else if(!controllerRear_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
|
|
controllerRear_connected=true;
|
|
writeLogComment(Serial1,loopmillis, "Controller Rear connected");
|
|
}*/
|
|
|
|
controllers_connected=escFront.getControllerConnected() & escRear.getControllerConnected();
|
|
|
|
//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(loopmillis, "Error Throttle ADC Out of Range");
|
|
|
|
}
|
|
//Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
|
|
}
|
|
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(loopmillis, "Error Brake ADC Out of Range");
|
|
}
|
|
//Serial.print("Error Brake ADC Out of Range="); Serial.println(brake_raw);
|
|
}
|
|
#define ADS_MAX_READ_INTERVAL 100
|
|
if (loopmillis-last_adsread > ADS_MAX_READ_INTERVAL) {
|
|
if (!error_ads_max_read_interval) {
|
|
error_ads_max_read_interval=true;
|
|
writeLogComment(loopmillis, "Error ADS Max read interval");
|
|
}
|
|
//Serial.print("Error ADS Max read interval=");Serial.println(loopmillis-last_adsread);
|
|
}
|
|
|
|
if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange || error_ads_max_read_interval) { //any errors?
|
|
throttle_pos=0;
|
|
brake_pos=0;
|
|
}
|
|
}
|
|
|
|
void calculateSetSpeed(){
|
|
|
|
int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000);
|
|
|
|
float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
|
|
|
|
int16_t cmdreduce_constant=map(brake_pos_expo,0,1000,0,(int16_t)(brake_cmdreduce_proportional*SENDPERIOD/1000)); //reduce cmd value every cycle
|
|
|
|
float freewheel_current=startbrakecurrent_offset-brake_pos_expo*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)
|
|
|
|
|
|
float filtered_currentFront=max(escFront.getFiltered_curL(),escFront.getFiltered_curR());
|
|
float filtered_currentRear=max(escRear.getFiltered_curL(),escRear.getFiltered_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 += constrain(throttle_pos-cmd_send,0,max_acceleration_rate*SENDPERIOD/1000); //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);
|
|
|
|
last_cmd_send=cmd_send;
|
|
|
|
int16_t cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos*0.5/1000.0) ) ,0,1000); //brake "ducking"
|
|
|
|
|
|
if (reverse_enabled) {
|
|
cmd_send_toMotor-=brake_pos*REVERSE_SPEED;
|
|
}
|
|
|
|
if (!controllers_connected || !armed) { //controllers not connected or not armed
|
|
cmd_send=0;
|
|
cmd_send_toMotor=0; //safety off
|
|
}
|
|
|
|
escFront.setSpeed(cmd_send_toMotor,cmd_send_toMotor);
|
|
escRear.setSpeed(cmd_send_toMotor,cmd_send_toMotor);
|
|
|
|
log_update=true;
|
|
|
|
}
|
|
|
|
/*
|
|
void sendCMD() { //TODO: remove complete function because replaced by calculateSetSpeed()
|
|
// ## FOR REFERENCE:
|
|
//int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
|
|
//int16_t brake_cmdreduce_proportional=100; //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
|
|
|
|
int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000);
|
|
|
|
float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
|
|
|
|
int16_t cmdreduce_constant=map(brake_pos_expo,0,1000,0,(int16_t)(brake_cmdreduce_proportional*SENDPERIOD/1000)); //reduce cmd value every cycle
|
|
|
|
float freewheel_current=startbrakecurrent_offset-brake_pos_expo*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 += constrain(throttle_pos-cmd_send,0,max_acceleration_rate*SENDPERIOD/1000); //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);
|
|
|
|
last_cmd_send=cmd_send;
|
|
|
|
int16_t cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos*0.5/1000.0) ) ,0,1000); //brake "ducking"
|
|
|
|
|
|
if (reverse_enabled) {
|
|
cmd_send_toMotor-=brake_pos*REVERSE_SPEED;
|
|
}
|
|
|
|
if (!controllers_connected || !armed) { //controllers not connected or not armed
|
|
cmd_send=0;
|
|
cmd_send_toMotor=0; //safety off
|
|
}
|
|
|
|
//apply throttle command to all motors
|
|
|
|
motorparamsFront.cmdL=cmd_send_toMotor;
|
|
motorparamsFront.cmdR=cmd_send_toMotor;
|
|
motorparamsRear.cmdL=cmd_send_toMotor;
|
|
motorparamsRear.cmdR=cmd_send_toMotor;
|
|
|
|
|
|
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()
|
|
|
|
}//else if(loopmillis>last_log_send+LOGMININTERVAL){
|
|
// //Serial.print(throttle_raw); Serial.println();
|
|
// Serial.print(linearizeThrottle(throttle_raw)); Serial.println();
|
|
// last_log_send=loopmillis;
|
|
//}
|
|
}*/
|
|
|
|
|
|
/*
|
|
void checkLog() { //TODO: remove
|
|
if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){ //write header for log file after logger booted up
|
|
writeLogInfo(Serial1);
|
|
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 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() {
|
|
bool button_start_longpress_flag=false;
|
|
bool button_start_shortpress_flag=false;
|
|
|
|
static bool button_start_wait_release_flag=false;
|
|
bool last_button_start_state=button_start_state;
|
|
|
|
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_wait_release_flag=false;
|
|
button_start_lastchange=loopmillis; //save time for debouncing
|
|
}
|
|
}
|
|
|
|
if (!button_start_wait_release_flag) { //action not prohibited currently
|
|
if (button_start_state) { //button is pressed
|
|
if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
|
|
button_start_longpress_flag=true;
|
|
button_start_wait_release_flag=true; //do not trigger again until button released
|
|
}
|
|
}else if(!button_start_state && last_button_start_state) { //just released
|
|
button_start_shortpress_flag=true;
|
|
}
|
|
}
|
|
|
|
if (button_start_shortpress_flag) {
|
|
armed=false; //disarm
|
|
writeLogComment(loopmillis, "Disarmed by button");
|
|
}
|
|
if (button_start_longpress_flag) {
|
|
armed=true; //arm if button pressed long enough
|
|
writeLogComment(loopmillis, "Armed by button");
|
|
}
|
|
|
|
/* TODO: if works, remove this
|
|
if (button_start_state) { //pressed
|
|
if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
|
|
if (throttle_pos<=0 && brake_pos<=0 && controllers_connected && !armed) { //brake or thottle not pressed, controllers connected
|
|
armed=true; //arm if button pressed long enough
|
|
writeLogComment(loopmillis, "Armed by button");
|
|
}
|
|
}else if (armed){ //not pressed long enough and is armed
|
|
armed=false; //disarm
|
|
writeLogComment(loopmillis, "Disarmed by button");
|
|
}
|
|
}
|
|
*/
|
|
|
|
}
|
|
|
|
uint16_t linearizeThrottle(uint16_t v) {
|
|
//input is raw adc value from hall sensor
|
|
//uses throttleCurvePerMM array to find linear approximation of actual throttle travel
|
|
//array has to be sorted !
|
|
uint8_t _searchpos=0;
|
|
uint8_t arraysize = sizeof(throttleCurvePerMM)/sizeof(throttleCurvePerMM[0]);
|
|
while (_searchpos < arraysize && v>throttleCurvePerMM[_searchpos]) { //find arraypos with value above input value
|
|
_searchpos++; //try next value
|
|
}
|
|
|
|
if (_searchpos <=0) { //lower limit
|
|
return 0;
|
|
}
|
|
if (_searchpos >= arraysize) { //upper limit
|
|
return 1000;
|
|
}
|
|
|
|
uint16_t nextLower=throttleCurvePerMM[_searchpos-1];
|
|
uint16_t nextHigher=throttleCurvePerMM[_searchpos];
|
|
float _linearThrottle = _searchpos+map(v*1.0,nextLower,nextHigher,0.0,1.0);
|
|
_linearThrottle/=arraysize; //scale to 0-1
|
|
_linearThrottle*=1000; //scale to 0-1000
|
|
return (uint16_t)_linearThrottle;
|
|
} |