From 6c7015e689e1add52d3d1921fdc4443471262548 Mon Sep 17 00:00:00 2001 From: Fisch Date: Wed, 20 Apr 2022 20:49:27 +0200 Subject: [PATCH] make nrf control smoother --- hoverbrettctrl/src/main.cpp | 40 +++++++++++++++++++++++++++---------- 1 file changed, 29 insertions(+), 11 deletions(-) diff --git a/hoverbrettctrl/src/main.cpp b/hoverbrettctrl/src/main.cpp index 236c3ae..b1f1d17 100644 --- a/hoverbrettctrl/src/main.cpp +++ b/hoverbrettctrl/src/main.cpp @@ -35,6 +35,8 @@ long last_adcupdated=0; #define CONTROLUPDATEPERIOD 10 long last_controlupdate = 0; +#define FILTER_NRF_SET_SPEED 0.1 //higher value, faster response. depends on CONTROLUPDATEPERIOD +#define FILTER_NRF_SET_STEER 0.1 #define GT_LENGTH_MIN 200 //minimum length for stuff to start happen @@ -90,10 +92,10 @@ uint16_t gt_length_set=1000; //set length to keep [mm] float gt_speed_p=0.7; //value to multipy difference [mm] with -> out_speed float gt_speedbackward_p=0.7; float gt_steer_p=2.0; -#define GT_SPEED_LIMIT 300 //maximum out_speed value + +#define GT_SPEED_LIMIT 400 //maximum out_speed value + #define GT_SPEEDBACKWARD_LIMIT 100//maximum out_speed value (for backward driving) - #define GT_STEER_LIMIT 300 //maximum out_steer value +- -#define GT_LENGTH_MAXIMUMDIFFBACKWARD -200 //[mm]. if gt_length_set=1000 and GT_LENGTH_MAXIMUMDIFFBACKWARD=-200 then only drives backward if lenght is greater 800 +#define GT_LENGTH_MAXIMUMDIFFBACKWARD -250 //[mm]. if gt_length_set=1000 and GT_LENGTH_MAXIMUMDIFFBACKWARD=-200 then only drives backward if lenght is greater 800 #include @@ -337,9 +339,12 @@ void loop() { //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); - set_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000 - set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ); + 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 ); + + 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 #define SPEED_COEFFICIENT_NRF 1 // higher value == stronger @@ -348,7 +353,6 @@ void loop() { _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 pastpacket not ok, keep last out_steer and speed values until disarmed @@ -428,6 +432,8 @@ void loop() { if (controlmode == MODE_DISARMED){ //all disarmed esc.setSpeed(0,0); + set_speed=0; //reset filter + set_steer=0; } @@ -496,18 +502,30 @@ void updateDisplay(unsigned long loopmillis) break; } - /* + static float maxcurL=0; + static float maxcurR=0; + maxcurL=max(maxcurL,esc.getFiltered_curL()); + maxcurR=max(maxcurR,esc.getFiltered_curR()); + + static float mincurL=0; + static float mincurR=0; + mincurL=min(mincurL,esc.getFiltered_curL()); + mincurR=min(mincurR,esc.getFiltered_curR()); + + static float maxspdL=0; + display.print(F("Bat=")); display.print(esc.getFeedback_batVoltage()); display.print(F(" Temp=")); display.println(esc.getFeedback_boardTemp()); display.print(F("nrf_delay=")); display.print(last_nrfreceive_delay); display.print(F(" L=")); display.println(gt_length); display.print(F("maxdiff=")); display.println(raw_length_maxdiff); - display.print(F("CMD=")); display.print(esc.getCmdL()); display.print(F(", ")); display.println(esc.getCmdR()); + display.print(F("CMD=")); display.print(esc.getCmdL()); display.print(F(", ")); display.print(esc.getCmdR()); //display.print(F("FBC=")); display.print(esc.getFeedback_cmd1()); display.print(F(", ")); display.print(esc.getFeedback_cmd2()); - display.print(F(" sped=")); display.print(esc.getFeedback_speedL_meas()); display.print(F(", ")); display.println(esc.getFeedback_speedR_meas()); - display.print(F("curDC=")); display.print(esc.getFiltered_curL()); display.print(F(", ")); display.println(esc.getFiltered_curL()); + display.print(F(" spd=")); display.print(esc.getFeedback_speedL_meas()); display.print(F(", ")); display.println(esc.getFeedback_speedR_meas()); + display.print(F("DC max=")); display.print(maxcurL,1); display.print(F("/")); display.println(maxcurR,1); // display.print(F(" min=")); display.print(mincurL,1); display.print(F("/")); display.println(mincurR,1); display.print(F("trip=")); display.print(esc.getTrip(),0); display.print(F(",")); display.print(esc.getCurrentConsumed(),3); display.println(F("Ah")); - */ + + /* display.print(F("H=")); display.print(gt_horizontal); display.print(F(" V=")); display.println(gt_vertical); - display.print(F("CMD=")); display.print(esc.getCmdL()); display.print(F(", ")); display.println(esc.getCmdR()); + display.print(F("CMD=")); display.print(esc.getCmdL()); display.print(F(", ")); display.println(esc.getCmdR());*/ display.display(); // Show initial text last_updatedisplay=loopmillis;