From 955696d42864b91834f38df5182192e738f05844 Mon Sep 17 00:00:00 2001 From: Fisch Date: Thu, 14 Jul 2022 21:41:29 +0200 Subject: [PATCH] fix safety off --- hoverbrettctrl/src/main.cpp | 129 +++++++++++++++++++----------------- 1 file changed, 70 insertions(+), 59 deletions(-) diff --git a/hoverbrettctrl/src/main.cpp b/hoverbrettctrl/src/main.cpp index 73e1a0e..fc7e50d 100644 --- a/hoverbrettctrl/src/main.cpp +++ b/hoverbrettctrl/src/main.cpp @@ -4,6 +4,8 @@ unsigned long loopmillis; unsigned long last_loopmillis; +//for testing debug +uint8_t debug_count_disarmedbecauseofnrfdelay=0; #include //for teensy rtc time_t getTeensy3Time(); @@ -64,8 +66,8 @@ bool button_down_click=false; #define PIN_GAMETRAK_VERTICAL A8 //A8=22 #define PIN_GAMETRAK_HORIZONTAL A9 //A9=23 - -float speed_coefficient_nrf=1.0; // higher value == stronger + +float speed_coefficient_nrf=0.4; // higher value == stronger float steer_coefficient_nrf=0.5; // higher value == stronger float speed_coefficient_gt=1.0; // higher value == stronger @@ -77,8 +79,7 @@ long last_adcupdated=0; #define ADC_UPDATEPERIOD 10 //in ms #define CONTROLUPDATEPERIOD 10 -long last_controlupdate = 0; -float filter_nrf_set_speed=0.02; //higher value, faster response. depends on CONTROLUPDATEPERIOD +float filter_nrf_set_speed=0.06; //higher value, faster response. depends on CONTROLUPDATEPERIOD float filter_nrf_set_steer=0.06; float filter_stop_set_speed=0.015; //safety stop @@ -185,7 +186,7 @@ boolean setup_directon_press_right=false; -long last_send = 0; + int16_t set_speed = 0; int16_t set_steer = 0; @@ -462,11 +463,10 @@ void loop() { }else if ((lastnrfdata.commands & (1 << 2))>>0) { //up setup_directon_press_up = true; Serial.println("RF Button Up"); - speed_coefficient_nrf=1.0; }else if ((lastnrfdata.commands & (1 << 3))>>0) { //down setup_directon_press_down = true; Serial.println("RF Button Down"); - speed_coefficient_nrf=0.5; + }else if ((lastnrfdata.commands & (1 << 4))>>0) { //right setup_directon_press_right = true; Serial.println("RF Button Right"); @@ -477,17 +477,26 @@ void loop() { if (abs(set_speed)<10 && abs(set_steer)<10) { //standstill - if (setup_directon_press_left) { - + if (setup_directon_press_left || setup_directon_press_right) { + //Fast riding mode + speed_coefficient_nrf=1.0; + steer_coefficient_nrf=0.3; + filter_nrf_set_speed=0.015; + filter_nrf_set_steer=0.08; } if (setup_directon_press_up) { - + //Remote controlled Aggro + speed_coefficient_nrf=1.0; + steer_coefficient_nrf=0.5; + filter_nrf_set_speed=0.08; + filter_nrf_set_steer=0.08; } if (setup_directon_press_down) { - - } - if (setup_directon_press_right) { - + //Remote controlled Walking + speed_coefficient_nrf=0.4; + steer_coefficient_nrf=0.5; + filter_nrf_set_speed=0.06; + filter_nrf_set_steer=0.06; } } setup_directon_press_left=false; @@ -499,6 +508,7 @@ void loop() { if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive controlmode = MODE_DISARMED; + debug_count_disarmedbecauseofnrfdelay++; #ifdef DEBUG Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!"); #endif @@ -507,24 +517,35 @@ void loop() { if (lastpacketOK) { //if lastnrfdata is valid - if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { - last_controlupdate = loopmillis; + static unsigned long last_controlupdate_validnrf=0; + if (loopmillis - last_controlupdate_validnrf > CONTROLUPDATEPERIOD) { + last_controlupdate_validnrf = loopmillis; //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); int16_t new_set_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000 int16_t new_set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ) * -1; + if (!motorenabled) { //when safety off, reason: remote relased + if (abs(new_set_speed) < abs(set_speed)) { //only allow going slower + set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter + } + if (abs(new_set_steer) < abs(set_steer)) { //only allow turning slower + set_steer = set_steer*(1.0-filter_nrf_set_steer) + new_set_steer*filter_nrf_set_steer; + } + + }else{ + set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter + set_steer = set_steer*(1.0-filter_nrf_set_steer) + new_set_steer*filter_nrf_set_steer; - set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter - 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 - - int16_t _out_speedl,_out_speedr; - _out_speedl = constrain(set_speed * speed_coefficient_nrf + set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); - _out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); - esc.setSpeed(_out_speedl,_out_speedr); + //calculate speed l and r from speed and steer + + int16_t _out_speedl,_out_speedr; + _out_speedl = constrain(set_speed * speed_coefficient_nrf + set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); + _out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); + esc.setSpeed(_out_speedl,_out_speedr); + } } }//if pastpacket not ok, keep last out_steer and speed values until disarmed @@ -590,8 +611,9 @@ void loop() { } //calculate speed l and r from speed and steer - if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { - last_controlupdate = loopmillis; + static unsigned long last_controlupdate_gt=0; + if (loopmillis - last_controlupdate_gt > CONTROLUPDATEPERIOD) { + last_controlupdate_gt = loopmillis; int16_t _out_speedl,_out_speedr; _out_speedl = constrain(set_speed * speed_coefficient_gt + set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier; _out_speedr = constrain(set_speed * speed_coefficient_gt - set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier; @@ -611,8 +633,9 @@ void loop() { set_speed=0; set_steer=0; } - if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { - last_controlupdate = loopmillis; + static unsigned long last_controlupdate_disarmed=0; + if (loopmillis - last_controlupdate_disarmed > CONTROLUPDATEPERIOD) { + last_controlupdate_disarmed = 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; @@ -624,36 +647,17 @@ void loop() { - if (esc.sendPending(loopmillis)) { - - 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 * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); - _out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); - esc.setSpeed(_out_speedl,_out_speedr); - } + if (!motorenabled) {//motors disabled. Used for safety off when remote released + static unsigned long last_controlupdate_motordisabled=0; + if (loopmillis - last_controlupdate_motordisabled > CONTROLUPDATEPERIOD) { + last_controlupdate_motordisabled = 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 * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); + _out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); + esc.setSpeed(_out_speedl,_out_speedr); } - last_send = loopmillis; - - - #ifdef DEBUG - Serial.print(" out_speedl="); - Serial.print(out_speedl); - Serial.print(" out_speedr="); - Serial.print(out_speedr); - Serial.print(" checksum="); - Serial.print(out_checksum); - - Serial.print(" controlmode="); - Serial.print(controlmode); - - Serial.println(); - - #endif } @@ -674,7 +678,8 @@ void loop() { dataFile.print("current_L,current_R,"); dataFile.print("rpm_L,rpm_R,"); dataFile.print("temp,vbat,"); - dataFile.println("trip,currentConsumed"); + dataFile.println("trip,currentConsumed,motorenabled,disarmedByDelay"); + dataFile.print("#TIMESTAMP:"); dataFile.println(now()); logging_headerWritten=true; } dataFile.print(String(loopmillis)); dataFile.print(";"); @@ -687,7 +692,9 @@ void loop() { dataFile.print(esc.getFeedback_boardTemp()); dataFile.print(";"); dataFile.print(esc.getFeedback_batVoltage()); dataFile.print(";"); dataFile.print(esc.getTrip()); dataFile.print(";"); - dataFile.print(esc.getCurrentConsumed(),3); + dataFile.print(esc.getCurrentConsumed(),3); dataFile.print(";"); + dataFile.print(motorenabled); dataFile.print(";"); + dataFile.print(debug_count_disarmedbecauseofnrfdelay); dataFile.println(""); dataFile.close(); } @@ -1009,6 +1016,10 @@ void display_show_stats3() { display.print(F("nrfdata x,y=")); display.print(lastnrfdata.steer); display.print(F(", ")); display.println(lastnrfdata.speed); display.print(F("nrfdata command=")); display.println(lastnrfdata.commands); + display.print(F("disarmed delay#=")); display.println(debug_count_disarmedbecauseofnrfdelay); + + + display.display(); // Show initial text }