From 85f200da22cf70277e45104c6cff94fa6bd6f7bf Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 19 Jun 2022 20:07:18 +0200 Subject: [PATCH] add smooth slowdown for safety stop --- hoverbrettctrl/src/main.cpp | 70 ++++++++++++++++++++++++------------- 1 file changed, 45 insertions(+), 25 deletions(-) diff --git a/hoverbrettctrl/src/main.cpp b/hoverbrettctrl/src/main.cpp index 5e694d8..4dd3a46 100644 --- a/hoverbrettctrl/src/main.cpp +++ b/hoverbrettctrl/src/main.cpp @@ -63,7 +63,13 @@ bool button_down_click=false; #define PIN_GAMETRAK_HORIZONTAL A9 //A9=23 +#define SPEED_COEFFICIENT_NRF 1 // higher value == stronger +#define STEER_COEFFICIENT_NRF 0.5 // higher value == stronger +#define SPEED_COEFFICIENT_GT 1 // higher value == stronger +#define STEER_COEFFICIENT_GT 0.5 // higher value == stronger + +unsigned long watchdogtimer=0; //set to current millis everytime new good control input is calculated long last_adcupdated=0; #define ADC_UPDATEPERIOD 10 //in ms @@ -73,6 +79,9 @@ long last_controlupdate = 0; float filter_nrf_set_speed=0.02; //higher value, faster response. depends on CONTROLUPDATEPERIOD float filter_nrf_set_steer=0.06; +float filter_stop_set_speed=0.015; //safety stop +float filter_stop_set_steer=0.06; //safety stop + #define GT_LENGTH_MIN 200 //minimum length for stuff to start happen #define GT_LENGTH_1_OFFSET -22.5 @@ -172,7 +181,6 @@ long last_send = 0; int16_t set_speed = 0; int16_t set_steer = 0; -uint8_t out_checksum = 0; //0= disable motors, 255=reserved, 1<=checksum<255 #define NRFDATA_CENTER 127 //boolean armed = false; @@ -435,9 +443,10 @@ void loop() { last_nrfreceive_delay=loopmillis-last_nrfreceive; //for display purpose last_nrfreceive = loopmillis; + watchdogtimer=loopmillis; //reset watchdog //parse commands - motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0 + motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0 . Used for safety-off when remote released } } @@ -466,8 +475,7 @@ void loop() { set_steer = set_steer*(1.0-filter_nrf_set_steer) + new_set_steer*filter_nrf_set_steer; //calculate speed l and r from speed and steer - #define SPEED_COEFFICIENT_NRF 1 // higher value == stronger - #define STEER_COEFFICIENT_NRF 0.5 // higher value == stronger + int16_t _out_speedl,_out_speedr; _out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF + set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF - set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); @@ -493,6 +501,7 @@ void loop() { }else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active //Gametrak Control Code motorenabled=true; + watchdogtimer=loopmillis; //reset watchdog if (gt_length<=GT_LENGTH_MIN){ //let go //Serial.println("gametrak released"); controlmode=MODE_DISARMED; @@ -536,12 +545,13 @@ void loop() { } //calculate speed l and r from speed and steer - #define SPEED_COEFFICIENT_GT 1 // higher value == stronger - #define STEER_COEFFICIENT_GT 0.5 // higher value == stronger - int16_t _out_speedl,_out_speedr; - _out_speedl = constrain(set_speed * SPEED_COEFFICIENT_GT + set_steer * STEER_COEFFICIENT_GT, -1000, 1000)*safetymultiplier; - _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT - set_steer * STEER_COEFFICIENT_GT, -1000, 1000)*safetymultiplier; - esc.setSpeed(_out_speedl,_out_speedr); + if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { + last_controlupdate = loopmillis; + int16_t _out_speedl,_out_speedr; + _out_speedl = constrain(set_speed * SPEED_COEFFICIENT_GT + set_steer * STEER_COEFFICIENT_GT, -1000, 1000)*safetymultiplier; + _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT - set_steer * STEER_COEFFICIENT_GT, -1000, 1000)*safetymultiplier; + esc.setSpeed(_out_speedl,_out_speedr); + } } @@ -549,28 +559,38 @@ void loop() { controlmode = MODE_DISARMED; //force disarmed } + if (controlmode == MODE_DISARMED){ //all disarmed - esc.setSpeed(0,0); - set_speed=0; //reset filter - set_steer=0; + #define WATCHDOG_TIMEOUT 2000 + if (millis()-watchdogtimer>WATCHDOG_TIMEOUT) { + set_speed=0; + set_steer=0; + } + if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { + last_controlupdate = loopmillis; + set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter + set_steer = set_steer*(1.0-filter_stop_set_steer); + int16_t _out_speedl,_out_speedr; + _out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF + set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); + _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF - set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); + esc.setSpeed(_out_speedl,_out_speedr); + } } if (esc.sendPending(loopmillis)) { - //calculate checksum - out_checksum = ((uint8_t) ((uint8_t)esc.getCmdL()) * ((uint8_t)esc.getCmdR())); //simple checksum - if (out_checksum == 0 || out_checksum == 255) { - out_checksum = 1; //cannot be 0 or 255 (special purpose) - } - - if (!motorenabled) { //disable motors? - out_checksum = 0; //checksum=0 disables motors - } - - if (!motorenabled) {//motors disabled - esc.setSpeed(0,0); + if (!motorenabled) {//motors disabled. Used for safety off when remote released + if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { + last_controlupdate = loopmillis; + set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter + set_steer = set_steer*(1.0-filter_stop_set_steer); + int16_t _out_speedl,_out_speedr; + _out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF + set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); + _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF - set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); + esc.setSpeed(_out_speedl,_out_speedr); + } } last_send = loopmillis;