793 lines
27 KiB
C++
793 lines
27 KiB
C++
//TODO:
|
|
/*
|
|
- reset Trip to 0 by button press or something. function: resetTrip()
|
|
*/
|
|
|
|
|
|
#include <Arduino.h>
|
|
|
|
|
|
#include "definitions.h"
|
|
//#include "structs.h"
|
|
|
|
#include <TimeLib.h> //for teensy rtc
|
|
#include "helpfunctions.h"
|
|
#include "hoverboard-esc-serial-comm.h"
|
|
#include "led.h"
|
|
#include "temperature.h"
|
|
|
|
|
|
String getLogFilename();
|
|
bool getDatalogging();
|
|
#include "display.h"
|
|
#include "logging.h"
|
|
|
|
#include "ADS1X15.h"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ESCSerialComm escFront(Serial7);
|
|
ESCSerialComm escRear(Serial2);
|
|
|
|
|
|
ADS1115 ADS(0x48, &Wire); //Pins: https://www.pjrc.com/teensy/td_libs_Wire.html
|
|
|
|
/*
|
|
Serial Hoverboard Colors
|
|
RX (Green) connect to TX on Teensy
|
|
TX (Blue) connect to RX on Teensy
|
|
*/
|
|
|
|
|
|
|
|
void readADS();
|
|
void readADC();
|
|
void failChecks();
|
|
//void sendCMD();
|
|
void calculateSetSpeed(unsigned long timediff);
|
|
|
|
|
|
void leds();
|
|
|
|
void readButtons();
|
|
void readADSButtons();
|
|
|
|
uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending);
|
|
|
|
|
|
time_t getTeensy3Time();
|
|
|
|
|
|
// ########################## SETUP ##########################
|
|
void setup()
|
|
{
|
|
|
|
Serial.begin(SERIAL_BAUD); //Debug and Program
|
|
|
|
Serial1.begin(SERIAL_LOG_BAUD); //TX1=1, RX1=0
|
|
|
|
|
|
pinMode(PIN_PWRBUTTON, INPUT_PULLUP); //Pressed=High
|
|
|
|
datalogging=false; //disable logging when connected via usb to not clutter up sd card
|
|
Serial.print("PWRBUTTON ");
|
|
for (uint8_t i=0;i<10;i++) { //check a few times
|
|
if (!digitalRead(PIN_PWRBUTTON)) { //button is not pressed during startup means teensy is powered externally (usb)
|
|
Serial.print(".");
|
|
delay(100); //give more time to disable logging
|
|
}else{
|
|
datalogging=true; //button presse at least once recognized. enable logging
|
|
Serial.print("X");
|
|
}
|
|
}
|
|
if (!datalogging){
|
|
Serial.println(" not pressed. Logging disabled!");
|
|
}else{
|
|
Serial.println(" Pressed. Logging enabled");
|
|
}
|
|
|
|
pinMode(PIN_LED_START, OUTPUT); //Active High
|
|
|
|
pinMode(PIN_FAN,OUTPUT);
|
|
digitalWrite(PIN_FAN,HIGH); //Turn fan on during startup for debugging purposes
|
|
|
|
|
|
|
|
pinMode(PIN_LATCH_ENABLE, OUTPUT);
|
|
digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
|
|
|
|
|
|
init_led();
|
|
led_testLEDSBlocking();
|
|
|
|
|
|
delay(100);
|
|
Serial.println("Init Functions");
|
|
led_simpeProgress(0,1);
|
|
|
|
bool initResult=false;
|
|
|
|
initResult=display_init();
|
|
if (!initResult) {
|
|
writeLogComment((unsigned long)millis(), "SSD1306 allocation failed");
|
|
}
|
|
led_simpeProgress(1,initResult);
|
|
|
|
|
|
initResult=initLogging();
|
|
led_simpeProgress(2,(initResult ? (datalogging ? 1:2):0)); //0=sd card fail, 1=sd ok and logging, 2(warn)=sd ok and logging off
|
|
|
|
|
|
|
|
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);
|
|
for (uint8_t i=0;i<4;i++){ //read all channels once to have adc readings ready in first loop (to prevent premature failsafe)
|
|
readADS();
|
|
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");
|
|
|
|
writeLogComment((unsigned long)millis(), "Unable to sync with the RTC");
|
|
led_simpeProgress(7,false);
|
|
} else {
|
|
Serial.println("RTC has set the system time");
|
|
led_simpeProgress(7,true);
|
|
}
|
|
|
|
|
|
if (sdcard_available) { //sd init was successful
|
|
initResult=loadTripSD();
|
|
}else{
|
|
initResult=false;
|
|
}
|
|
led_simpeProgress(8,initResult);
|
|
|
|
|
|
initTemperature();
|
|
led_simpeProgress(9,true);
|
|
|
|
|
|
|
|
writeLogComment(millis(), "Setup Finished");
|
|
|
|
led_simpleProgressWait(); //wait longer if any errors were displayed with led_simpeProgress()
|
|
|
|
Serial.println("Ready");
|
|
}
|
|
|
|
|
|
|
|
// ########################## LOOP ##########################
|
|
void loop() {
|
|
//Serial.print("Loopduration="); Serial.println(); //loopduration is at max 11ms
|
|
|
|
loopmillis=millis(); //read millis for this cycle
|
|
|
|
|
|
|
|
|
|
if (ADS.isConnected() && (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();
|
|
readADSButtons();
|
|
}
|
|
|
|
|
|
failChecks();
|
|
|
|
static unsigned long last_calculateSetSpeed=0;
|
|
if (loopmillis - last_calculateSetSpeed > SENDPERIOD) {
|
|
unsigned long _timediff=loopmillis-last_calculateSetSpeed;
|
|
last_calculateSetSpeed=loopmillis;
|
|
calculateSetSpeed(_timediff);
|
|
|
|
//Update Statistics
|
|
|
|
max_filtered_currentAll=max(max_filtered_currentAll,filtered_currentAll);
|
|
min_filtered_currentAll=min(min_filtered_currentAll,filtered_currentAll);
|
|
max_filtered_wattAll=max(max_filtered_wattAll,filtered_currentAll*(escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0);
|
|
min_filtered_wattAll=min(min_filtered_wattAll,filtered_currentAll*(escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0);
|
|
max_meanSpeed=max(max_meanSpeed,(escFront.getMeanSpeed()+escRear.getMeanSpeed())/2);
|
|
if (!armed) { //reset statistics if disarmed
|
|
max_filtered_currentAll=0;
|
|
min_filtered_currentAll=0;
|
|
max_filtered_wattAll=0;
|
|
min_filtered_wattAll=0;
|
|
max_meanSpeed=0;
|
|
}
|
|
}
|
|
|
|
escFront.update(loopmillis);
|
|
escRear.update(loopmillis);
|
|
|
|
static unsigned long last_statsupdate=0;
|
|
#define STATSUPDATEINTERVAL 100
|
|
if (loopmillis-last_statsupdate>STATSUPDATEINTERVAL) {
|
|
|
|
minSpeedms=min(escFront.getWheelspeed_L(),min(escFront.getWheelspeed_R(),min(escRear.getWheelspeed_L(),escRear.getWheelspeed_R()))); //take speed of slowest wheel
|
|
float _tripincrease=abs(minSpeedms) * ((loopmillis-last_statsupdate)/1000.0);
|
|
trip+=_tripincrease;
|
|
overallTrip+=_tripincrease;
|
|
|
|
float _currentIncrease=(escFront.getFiltered_curL()+escFront.getFiltered_curR()+escRear.getFiltered_curL()+escRear.getFiltered_curR())* ((loopmillis-last_statsupdate)/1000.0)/3600.0; //amp hours
|
|
float _watthoursIncrease=((escFront.getFiltered_curL()+escFront.getFiltered_curR())*escFront.getFeedback_batVoltage()+(escRear.getFiltered_curL()+escRear.getFiltered_curR())*escRear.getFeedback_batVoltage())* ((loopmillis-last_statsupdate)/1000.0)/3600.0; //amp hours
|
|
currentConsumed += _currentIncrease;
|
|
overallCurrentConsumed += _currentIncrease;
|
|
|
|
watthoursConsumed += _watthoursIncrease;
|
|
overallWatthoursConsumed += _watthoursIncrease;
|
|
|
|
last_statsupdate=loopmillis;
|
|
}
|
|
|
|
|
|
|
|
loggingLoop(loopmillis,escFront,escRear);
|
|
if (!armed && !statswritten) { //write stats only once when disarmed
|
|
statswritten=true;
|
|
writeTrip(loopmillis,escFront,escRear);
|
|
}
|
|
if (statswritten && armed) {
|
|
statswritten=false;
|
|
}
|
|
|
|
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(escFront,escRear);
|
|
}
|
|
|
|
|
|
//Temperature
|
|
if (!temperatureLoop(loopmillis)){
|
|
writeLogComment(loopmillis, "Request Temperatures Timeout!");
|
|
}
|
|
|
|
|
|
//Fan
|
|
static unsigned long last_fan_update=0;
|
|
#define FANUPDATEPERIOD 5000
|
|
float fan_turn_on_temp=45;
|
|
float fan_turn_off_temp=32;
|
|
if (loopmillis - last_fan_update > FANUPDATEPERIOD) {
|
|
last_fan_update=loopmillis;
|
|
boolean fanstatus=digitalRead(PIN_FAN);
|
|
//float temp=max(escFront.getFeedback_boardTemp(),escRear.getFeedback_boardTemp());
|
|
float temp=max(temp_Front,temp_Rear);
|
|
if (temp_Front==DEVICE_DISCONNECTED_C || temp_Rear==DEVICE_DISCONNECTED_C ) { //temperature error
|
|
digitalWrite(PIN_FAN,HIGH); //force fan on
|
|
|
|
}else{ //normal temperature control_currentIncrease
|
|
|
|
if (!fanstatus) { //fan is off
|
|
if (temp>=fan_turn_on_temp){
|
|
digitalWrite(PIN_FAN,HIGH);
|
|
}
|
|
}else{ //fan is on
|
|
if (temp<=fan_turn_off_temp){
|
|
digitalWrite(PIN_FAN,LOW);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
serialCommandLoop(loopmillis,escFront,escRear);
|
|
|
|
|
|
looptime_duration_min=min(looptime_duration_min,millis()-loopmillis);
|
|
looptime_duration_max=max(looptime_duration_max,millis()-loopmillis);
|
|
|
|
}
|
|
|
|
|
|
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
|
|
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+ads_throttle_B_raw)/2.0*THROTTLE_ADC_FILTER + throttle_raw*(1-THROTTLE_ADC_FILTER); //apply filter
|
|
|
|
throttle_rawA=ads_throttle_A_raw;
|
|
throttle_rawB=ads_throttle_B_raw;
|
|
|
|
|
|
//maps throttle curve to be linear
|
|
//throttle_pos=max(0,min(1000,linearizeThrottle(throttle_raw))); //map and constrain
|
|
throttle_posA=max(0,min(1000, linearizeThrottle(ads_throttle_A_raw, throttleCurvePerMM_A, sizeof(throttleCurvePerMM_A)/sizeof(throttleCurvePerMM_A[0]), throttleCurvePerMM_A_Descending ) )); //map and constrain
|
|
throttle_posB=max(0,min(1000, linearizeThrottle(ads_throttle_B_raw, throttleCurvePerMM_B, sizeof(throttleCurvePerMM_B)/sizeof(throttleCurvePerMM_B[0]), throttleCurvePerMM_B_Descending ) )); //map and constrain
|
|
|
|
//Serial.print(throttle_posA); Serial.print('\t');
|
|
//Serial.print(throttle_posB); Serial.print('\t');
|
|
|
|
int16_t throttle_posMean = (throttle_posA+throttle_posB)/2.0;
|
|
|
|
|
|
throttle_pos = throttle_posMean*THROTTLE_ADC_FILTER + throttle_pos*(1-THROTTLE_ADC_FILTER); //apply filter
|
|
|
|
|
|
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 || ((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0) >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
|
|
|
|
|
|
|
|
}
|
|
|
|
void failChecks() {
|
|
static bool laststate_Front_getControllerConnected;
|
|
if ( !escFront.getControllerConnected() && laststate_Front_getControllerConnected) { //controller got disconnected and was connected before
|
|
laststate_Front_getControllerConnected=false;
|
|
writeLogComment(loopmillis, "Controller Front feedback timeout");
|
|
}else if( escFront.getControllerConnected() && !laststate_Front_getControllerConnected) { //controller was disconnected and is now connected
|
|
laststate_Front_getControllerConnected=true;
|
|
writeLogComment(loopmillis, "Controller Front connected");
|
|
}
|
|
|
|
static bool laststate_Rear_getControllerConnected;
|
|
if ( !escRear.getControllerConnected() && laststate_Rear_getControllerConnected) { //controller got disconnected and was connected before
|
|
laststate_Rear_getControllerConnected=false;
|
|
writeLogComment(loopmillis, "Controller Rear feedback timeout");
|
|
}else if( escRear.getControllerConnected() && !laststate_Rear_getControllerConnected) { //controller was disconnected and is now connected
|
|
laststate_Rear_getControllerConnected=true;
|
|
writeLogComment(loopmillis, "Controller Rear connected");
|
|
}
|
|
|
|
|
|
controllers_connected=escFront.getControllerConnected() & escRear.getControllerConnected();
|
|
|
|
//ADC Range Check
|
|
static unsigned long throttle_ok_time=0;
|
|
if ((ads_throttle_A_raw >= failsafe_throttle_min_A) & (ads_throttle_A_raw <= failsafe_throttle_max_A) & (ads_throttle_B_raw >= failsafe_throttle_min_B) & (ads_throttle_B_raw <= failsafe_throttle_max_B)) { //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. A="+(String)ads_throttle_A_raw+" B="+(String)ads_throttle_B_raw);
|
|
}
|
|
//Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
|
|
}
|
|
|
|
static unsigned long throttlediff_ok_time=0;
|
|
if (abs(throttle_posA-throttle_posB) <= failsafe_throttle_maxDiff) { //inside safe range (to check if wire got disconnected)
|
|
throttlediff_ok_time=loopmillis;
|
|
}
|
|
if (loopmillis>throttlediff_ok_time+ADC_DIFFHIGH_TIME) { //not ok for too long
|
|
if (!error_throttle_difftoohigh) {
|
|
error_throttle_difftoohigh=true;
|
|
writeLogComment(loopmillis, "Error Throttle Diff too High. A="+(String)ads_throttle_A_raw+" B="+(String)ads_throttle_B_raw);
|
|
}
|
|
//Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
|
|
}
|
|
|
|
|
|
static unsigned long brake_ok_time=0;
|
|
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. ADC="+(String)brake_raw);
|
|
}
|
|
//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);
|
|
}
|
|
|
|
boolean logged_error_sdfile_unavailable=false;
|
|
if (error_sdfile_unavailable && !logged_error_sdfile_unavailable) {
|
|
logged_error_sdfile_unavailable=true;
|
|
writeLogComment(loopmillis, "Error SDFile Unavailable");
|
|
}
|
|
|
|
if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange || error_throttle_difftoohigh || error_ads_max_read_interval) { //any errors?
|
|
armed=false; //disarm
|
|
throttle_pos=0;
|
|
brake_pos=0;
|
|
}
|
|
}
|
|
|
|
void calculateSetSpeed(unsigned long timediff){
|
|
|
|
int16_t adjusted_throttle_pos=constrain(throttle_pos*(throttle_max/1000.0),0,throttle_max);
|
|
|
|
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*timediff/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 filtered_currentFront=max(escFront.getFiltered_curL(),escFront.getFiltered_curR());
|
|
float filtered_currentRear=max(escRear.getFiltered_curL(),escRear.getFiltered_curR());
|
|
|
|
filtered_currentAll=filtered_currentFront+filtered_currentRear; //positive value is current Drawn from battery. negative value is braking current
|
|
|
|
|
|
if(adjusted_throttle_pos<last_cmd_send){ //freewheeling or braking
|
|
if (filtered_currentAll>freewheel_current) { //drive current too high
|
|
cmd_send-= max(0, (filtered_currentAll-freewheel_current)*freewheel_break_factor*(timediff/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
|
|
|
|
}
|
|
|
|
// ## Acceleration
|
|
cmd_send += constrain(adjusted_throttle_pos-cmd_send,0,(int16_t)(max_acceleration_rate*(timediff/1000.0)) ); //if throttle higher than last applied value, apply throttle directly with rate limit
|
|
|
|
cmd_send=constrain(cmd_send,0,throttle_max);
|
|
|
|
last_cmd_send=cmd_send;
|
|
|
|
int16_t cmd_send_toMotor_FL=0;
|
|
int16_t cmd_send_toMotor_FR=0;
|
|
int16_t cmd_send_toMotor_RL=0;
|
|
int16_t cmd_send_toMotor_RR=0;
|
|
int16_t _cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos*0.5/1000.0) ) ,0,throttle_max); //brake "ducking"
|
|
|
|
|
|
cmd_send_toMotor_FL=_cmd_send_toMotor;
|
|
cmd_send_toMotor_FR=_cmd_send_toMotor;
|
|
cmd_send_toMotor_RL=_cmd_send_toMotor;
|
|
cmd_send_toMotor_RR=_cmd_send_toMotor;
|
|
|
|
|
|
|
|
|
|
// ## Experimental Tank steering
|
|
static float tanksteering_differential=0; //to ramp up slowly. value between -1.0 and 1.0
|
|
//Parameters:
|
|
int16_t tanksteering_max_speed=200;
|
|
float tanksteering_rate_increase=2.0; //increase units per second
|
|
float tanksteering_rate_decrease=3.0; //decrease units per second
|
|
|
|
|
|
|
|
if (control_buttonA && !control_buttonB && !reverse_enabled && throttle_pos>0) { //Right button (A) only. and throttle touched
|
|
tanksteering_differential+=tanksteering_rate_increase*(timediff/1000.0);
|
|
tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1
|
|
}else if(control_buttonB && !control_buttonA && !reverse_enabled && throttle_pos>0) { //Left button (B) only. and throttle touched
|
|
tanksteering_differential-=tanksteering_rate_increase*(timediff/1000.0);
|
|
tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1
|
|
}else{ //buttons released
|
|
if(tanksteering_differential>0) {
|
|
tanksteering_differential-=tanksteering_rate_decrease*(timediff/1000.0);
|
|
}else if(tanksteering_differential<0){
|
|
tanksteering_differential+=tanksteering_rate_decrease*(timediff/1000.0);
|
|
}
|
|
tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1
|
|
}
|
|
|
|
cmd_send_toMotor_FL+=tanksteering_differential*tanksteering_max_speed;
|
|
cmd_send_toMotor_FR-=tanksteering_differential*tanksteering_max_speed;
|
|
cmd_send_toMotor_RL+=tanksteering_differential*tanksteering_max_speed;
|
|
cmd_send_toMotor_RR-=tanksteering_differential*tanksteering_max_speed;
|
|
|
|
|
|
|
|
// ## Braking, Reversing and Standstill movements below here ##
|
|
/*
|
|
if (reverse_enabled) { //backwards driving not prohibited
|
|
_cmd_send_toMotor-=brake_pos*reverse_speed;
|
|
|
|
cmd_send_toMotor_FL=_cmd_send_toMotor;
|
|
cmd_send_toMotor_FR=_cmd_send_toMotor;
|
|
cmd_send_toMotor_RL=_cmd_send_toMotor;
|
|
cmd_send_toMotor_RR=_cmd_send_toMotor;
|
|
|
|
}
|
|
*/
|
|
|
|
|
|
float steeringdifferential_speed=0.3; //Speed for turning the steering wheel by differtially driving the front wheels
|
|
|
|
static bool reverse_enabled2=true; //pohibit instant reverse driving when releasing steering wheel buttons when brake is still pressed. wait for throttle and brake to release once
|
|
if (!reverse_enabled2 && throttle_pos<=0 && brake_pos<=0){
|
|
reverse_enabled2=true;
|
|
}
|
|
|
|
if (reverse_enabled) { //backwards driving not prohibited
|
|
if (control_buttonA && !control_buttonB) { //Right button (A) only
|
|
cmd_send_toMotor_FL+=brake_pos*steeringdifferential_speed;
|
|
cmd_send_toMotor_FR-=brake_pos*steeringdifferential_speed;
|
|
cmd_send_toMotor_RL+=brake_pos*steeringdifferential_speed;
|
|
cmd_send_toMotor_RR-=brake_pos*steeringdifferential_speed;
|
|
reverse_enabled2=false;
|
|
|
|
}else if(control_buttonB && !control_buttonA) { //Left button (B) only
|
|
cmd_send_toMotor_FL-=brake_pos*steeringdifferential_speed;
|
|
cmd_send_toMotor_FR+=brake_pos*steeringdifferential_speed;
|
|
cmd_send_toMotor_RL-=brake_pos*steeringdifferential_speed;
|
|
cmd_send_toMotor_RR+=brake_pos*steeringdifferential_speed;
|
|
reverse_enabled2=false;
|
|
|
|
}else if(reverse_enabled2){ //no button on steering wheel pressed, drive backwards
|
|
_cmd_send_toMotor-=brake_pos*reverse_speed;
|
|
|
|
cmd_send_toMotor_FL=_cmd_send_toMotor;
|
|
cmd_send_toMotor_FR=_cmd_send_toMotor;
|
|
cmd_send_toMotor_RL=_cmd_send_toMotor;
|
|
cmd_send_toMotor_RR=_cmd_send_toMotor;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
if (!controllers_connected || !armed) { //controllers not connected or not armed
|
|
cmd_send=0;
|
|
cmd_send_toMotor_FL=0; //safety off
|
|
cmd_send_toMotor_FR=0; //safety off
|
|
cmd_send_toMotor_RL=0; //safety off
|
|
cmd_send_toMotor_RR=0; //safety off
|
|
}
|
|
|
|
escFront.setSpeed(cmd_send_toMotor_FL,cmd_send_toMotor_FR);
|
|
escRear.setSpeed(cmd_send_toMotor_RL,cmd_send_toMotor_RR);
|
|
|
|
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
|
|
}
|
|
|
|
|
|
}
|
|
|
|
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_PWRBUTTON) && !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_PWRBUTTON) && 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_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_state) { //release wait flag at end if button released
|
|
button_start_wait_release_flag=false;
|
|
}
|
|
|
|
|
|
if (button_start_shortpress_flag) {
|
|
armed=false; //disarm
|
|
writeLogComment(loopmillis, "Disarmed by button");
|
|
}
|
|
if (button_start_longpress_flag) {
|
|
if (escFront.getControllerConnected() && escRear.getControllerConnected()) {
|
|
armed=true; //arm if button pressed long enough
|
|
writeLogComment(loopmillis, "Armed by button");
|
|
if (control_buttonA) { //button A is held down during start button press
|
|
throttle_max=1000;
|
|
reverse_speed=0.25;
|
|
max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE;
|
|
}else if (control_buttonB) { //button B is held down during start button press
|
|
throttle_max=750;
|
|
reverse_speed=0.25;
|
|
max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE;
|
|
}else { //no control button pressed during start
|
|
throttle_max=250;
|
|
reverse_speed=0.15;
|
|
max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE;
|
|
}
|
|
|
|
}else{
|
|
writeLogComment(loopmillis, "Unable to arm");
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void readADSButtons() {
|
|
bool last_control_buttonA=control_buttonA;
|
|
bool last_control_buttonB=control_buttonB;
|
|
if ( (ads_control_raw > (calib_control_buttonA-calib_control_treshold)) && (ads_control_raw < (calib_control_buttonA+calib_control_treshold) ) ) {
|
|
control_buttonA=true;
|
|
control_buttonB=false;
|
|
}else if ( (ads_control_raw > (calib_control_buttonB-calib_control_treshold)) && (ads_control_raw < (calib_control_buttonB+calib_control_treshold) ) ) {
|
|
control_buttonA=false;
|
|
control_buttonB=true;
|
|
}else if ( (ads_control_raw > (calib_control_buttonAB-calib_control_treshold)) && (ads_control_raw < (calib_control_buttonAB+calib_control_treshold) ) ) {
|
|
control_buttonA=true;
|
|
control_buttonB=true;
|
|
}else if ( ads_control_raw > calib_control_max){
|
|
control_buttonA=false;
|
|
control_buttonB=false;
|
|
}
|
|
|
|
if (control_buttonA && !last_control_buttonA) { //button A was just pressed
|
|
writeLogComment(loopmillis, "Button A Pressed");
|
|
|
|
if (!armed) { //standing disarmed display is showing
|
|
if (standingDisplayScreen<NUM_STANDINGDISPLAYSCREEN-1){
|
|
standingDisplayScreen++;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (control_buttonB && !last_control_buttonB) { //button B was just pressed
|
|
writeLogComment(loopmillis, "Button B Pressed");
|
|
|
|
if (!armed) { //standing disarmed display is showing
|
|
if (standingDisplayScreen>0){
|
|
standingDisplayScreen--;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending) {
|
|
//input is raw adc value from hall sensor
|
|
//uses pthrottleCurvePerMM array to find linear approximation of actual throttle travel
|
|
//array has to be sorted ! if sorteddescending=false then sorted ascending, if true then array should be sorted descending
|
|
uint8_t _searchpos=0;
|
|
//uint8_t arraysize = sizeof(pthrottleCurvePerMM)/sizeof(pthrottleCurvePerMM[0]);
|
|
while (_searchpos < arraysize && v>pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)]) { //find arraypos with value above input value
|
|
_searchpos++; //try next value
|
|
}
|
|
|
|
if (_searchpos <=0) { //lower limit
|
|
return (sorteddescending?1000:0);
|
|
}
|
|
if (_searchpos >= arraysize) { //upper limit
|
|
return (sorteddescending?0:1000);
|
|
}
|
|
|
|
uint16_t nextLower=pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)-(sorteddescending?0:1)];
|
|
uint16_t nextHigher=pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)-(sorteddescending?1:0)];
|
|
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
|
|
if (sorteddescending){
|
|
_linearThrottle=1000-_linearThrottle; //invert result
|
|
}
|
|
return (uint16_t)_linearThrottle;
|
|
} |