make nrf control smoother

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 <SPI.h>
@@ -337,10 +339,13 @@ 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
+        int16_t new_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_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
         #define STEER_COEFFICIENT_NRF   0.5  // 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(" spd=")); 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("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;