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

improve handling and gametrak safety

This commit is contained in:
interfisch 2022-04-12 16:28:51 +02:00
parent c7b4a7a568
commit 28d74499fd
2 changed files with 32 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
hoverbrettctrl/lib/hoverboard-esc-serial-comm

@ -1 +1 @@
Subproject commit 214fecdce421581dddd71cfd4e6ba0d4d77fc07c Subproject commit c2eba898ac628c6827d17c142a54e601bda5797b

37
hoverbrettctrl/src/main.cpp
View File

@ -371,7 +371,7 @@ void loop() {
//Gametrak Control Code //Gametrak Control Code
motorenabled=true; motorenabled=true;
if (gt_length<=GT_LENGTH_MIN){ //let go if (gt_length<=GT_LENGTH_MIN){ //let go
Serial.println("gametrak released"); //Serial.println("gametrak released");
controlmode=MODE_DISARMED; controlmode=MODE_DISARMED;
motorenabled=false; motorenabled=false;
} }
@ -393,14 +393,31 @@ void loop() {
} }
} }
static float safetymultiplier=1.0; //value to reduce output speed if gametrack is pointing too far down or up (string might be behind vehicle)
#define GT_SAFETY_THRESHOLD_H 120 //above which value (abs) safety slowdown should start
#define GT_SAFETY_THRESHOLD_V 120
#define SAFETYMULTIPLIER_UPDATE_INTERVAL 100
#define SAFETYMULTIPLIER_CHANGE_TIME_DOWN 2.0 //Time how long it should take to go from 100% to 0% when in safety slowdown
#define SAFETYMULTIPLIER_CHANGE_TIME_UP 1.0
static unsigned long last_safetymultiplier_update=0;
if (loopmillis-last_safetymultiplier_update > SAFETYMULTIPLIER_UPDATE_INTERVAL) {
if (abs(gt_vertical)>GT_SAFETY_THRESHOLD_V || abs(gt_horizontal)>GT_SAFETY_THRESHOLD_H) {
safetymultiplier-=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_DOWN;
}else{
safetymultiplier+=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_UP;
}
safetymultiplier=constrain(safetymultiplier,0.0,1.0);
last_safetymultiplier_update=loopmillis;
}
//calculate speed l and r from speed and steer //calculate speed l and r from speed and steer
#define SPEED_COEFFICIENT_GT 1 // higher value == stronger #define SPEED_COEFFICIENT_GT 1 // higher value == stronger
#define STEER_COEFFICIENT_GT 0.5 // higher value == stronger #define STEER_COEFFICIENT_GT 0.5 // higher value == stronger
int16_t _out_speedl,_out_speedr; int16_t _out_speedl,_out_speedr;
_out_speedl = constrain(set_speed * SPEED_COEFFICIENT_GT + set_steer * STEER_COEFFICIENT_GT, -1000, 1000); _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); _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT - set_steer * STEER_COEFFICIENT_GT, -1000, 1000)*safetymultiplier;
esc.setSpeed(_out_speedl,_out_speedr); esc.setSpeed(_out_speedl,_out_speedr);
} }
@ -478,11 +495,19 @@ void updateDisplay(unsigned long loopmillis)
display.println(F("UNDEF")); display.println(F("UNDEF"));
break; break;
} }
display.print(F("nrf_delay=")); display.println(last_nrfreceive_delay);
display.print(F("gt_length=")); display.println(gt_length); /*
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("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.println(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("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.display(); // Show initial text display.display(); // Show initial text
last_updatedisplay=loopmillis; last_updatedisplay=loopmillis;