diff --git a/hoverbrettctrl/lib/hoverboard-esc-serial-comm b/hoverbrettctrl/lib/hoverboard-esc-serial-comm index 214fecd..c2eba89 160000 --- a/hoverbrettctrl/lib/hoverboard-esc-serial-comm +++ b/hoverbrettctrl/lib/hoverboard-esc-serial-comm @@ -1 +1 @@ -Subproject commit 214fecdce421581dddd71cfd4e6ba0d4d77fc07c +Subproject commit c2eba898ac628c6827d17c142a54e601bda5797b diff --git a/hoverbrettctrl/src/main.cpp b/hoverbrettctrl/src/main.cpp index 07b1423..236c3ae 100644 --- a/hoverbrettctrl/src/main.cpp +++ b/hoverbrettctrl/src/main.cpp @@ -371,7 +371,7 @@ void loop() { //Gametrak Control Code motorenabled=true; if (gt_length<=GT_LENGTH_MIN){ //let go - Serial.println("gametrak released"); + //Serial.println("gametrak released"); controlmode=MODE_DISARMED; 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 #define SPEED_COEFFICIENT_GT 1 // higher value == stronger #define STEER_COEFFICIENT_GT 0.5 // higher value == stronger int16_t _out_speedl,_out_speedr; - _out_speedl = 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); + _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)*safetymultiplier; esc.setSpeed(_out_speedl,_out_speedr); } @@ -478,11 +495,19 @@ void updateDisplay(unsigned long loopmillis) display.println(F("UNDEF")); 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("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 last_updatedisplay=loopmillis;