interfisch
/
hoverbrett
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

make nrf control smoother

This commit is contained in:
interfisch 2022-04-20 20:49:27 +02:00
parent 32d5c8ca3f
commit 6c7015e689
1 changed files with 29 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
hoverbrettctrl/src/main.cpp
View File

@ -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
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
#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("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;