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

fix safety off

This commit is contained in:
interfisch 2022-07-14 21:41:29 +02:00
parent aea62ec868
commit 955696d428
1 changed files with 70 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
hoverbrettctrl/src/main.cpp
View File

@ -4,6 +4,8 @@
unsigned long loopmillis; unsigned long loopmillis;
unsigned long last_loopmillis; unsigned long last_loopmillis;
//for testing debug
uint8_t debug_count_disarmedbecauseofnrfdelay=0;
#include <TimeLib.h> //for teensy rtc #include <TimeLib.h> //for teensy rtc
time_t getTeensy3Time(); time_t getTeensy3Time();
@ -64,8 +66,8 @@ bool button_down_click=false;
#define PIN_GAMETRAK_VERTICAL A8 //A8=22 #define PIN_GAMETRAK_VERTICAL A8 //A8=22
#define PIN_GAMETRAK_HORIZONTAL A9 //A9=23 #define PIN_GAMETRAK_HORIZONTAL A9 //A9=23
float speed_coefficient_nrf=1.0; // higher value == stronger float speed_coefficient_nrf=0.4; // higher value == stronger
float steer_coefficient_nrf=0.5; // higher value == stronger float steer_coefficient_nrf=0.5; // higher value == stronger
float speed_coefficient_gt=1.0; // higher value == stronger float speed_coefficient_gt=1.0; // higher value == stronger
@ -77,8 +79,7 @@ long last_adcupdated=0;
#define ADC_UPDATEPERIOD 10 //in ms #define ADC_UPDATEPERIOD 10 //in ms
#define CONTROLUPDATEPERIOD 10 #define CONTROLUPDATEPERIOD 10
long last_controlupdate = 0; float filter_nrf_set_speed=0.06; //higher value, faster response. depends on CONTROLUPDATEPERIOD
float filter_nrf_set_speed=0.02; //higher value, faster response. depends on CONTROLUPDATEPERIOD
float filter_nrf_set_steer=0.06; float filter_nrf_set_steer=0.06;
float filter_stop_set_speed=0.015; //safety stop float filter_stop_set_speed=0.015; //safety stop
@ -185,7 +186,7 @@ boolean setup_directon_press_right=false;
long last_send = 0;
int16_t set_speed = 0; int16_t set_speed = 0;
int16_t set_steer = 0; int16_t set_steer = 0;
@ -462,11 +463,10 @@ void loop() {
}else if ((lastnrfdata.commands & (1 << 2))>>0) { //up }else if ((lastnrfdata.commands & (1 << 2))>>0) { //up
setup_directon_press_up = true; setup_directon_press_up = true;
Serial.println("RF Button Up"); Serial.println("RF Button Up");
speed_coefficient_nrf=1.0;
}else if ((lastnrfdata.commands & (1 << 3))>>0) { //down }else if ((lastnrfdata.commands & (1 << 3))>>0) { //down
setup_directon_press_down = true; setup_directon_press_down = true;
Serial.println("RF Button Down"); Serial.println("RF Button Down");
speed_coefficient_nrf=0.5;
}else if ((lastnrfdata.commands & (1 << 4))>>0) { //right }else if ((lastnrfdata.commands & (1 << 4))>>0) { //right
setup_directon_press_right = true; setup_directon_press_right = true;
Serial.println("RF Button Right"); Serial.println("RF Button Right");
@ -477,17 +477,26 @@ void loop() {
if (abs(set_speed)<10 && abs(set_steer)<10) { //standstill if (abs(set_speed)<10 && abs(set_steer)<10) { //standstill
if (setup_directon_press_left) { if (setup_directon_press_left || setup_directon_press_right) {
//Fast riding mode
speed_coefficient_nrf=1.0;
steer_coefficient_nrf=0.3;
filter_nrf_set_speed=0.015;
filter_nrf_set_steer=0.08;
} }
if (setup_directon_press_up) { if (setup_directon_press_up) {
//Remote controlled Aggro
speed_coefficient_nrf=1.0;
steer_coefficient_nrf=0.5;
filter_nrf_set_speed=0.08;
filter_nrf_set_steer=0.08;
} }
if (setup_directon_press_down) { if (setup_directon_press_down) {
//Remote controlled Walking
} speed_coefficient_nrf=0.4;
if (setup_directon_press_right) { steer_coefficient_nrf=0.5;
filter_nrf_set_speed=0.06;
filter_nrf_set_steer=0.06;
} }
} }
setup_directon_press_left=false; setup_directon_press_left=false;
@ -499,6 +508,7 @@ void loop() {
if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
controlmode = MODE_DISARMED; controlmode = MODE_DISARMED;
debug_count_disarmedbecauseofnrfdelay++;
#ifdef DEBUG #ifdef DEBUG
Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!"); Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
#endif #endif
@ -507,24 +517,35 @@ void loop() {
if (lastpacketOK) { //if lastnrfdata is valid if (lastpacketOK) { //if lastnrfdata is valid
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { static unsigned long last_controlupdate_validnrf=0;
last_controlupdate = loopmillis; if (loopmillis - last_controlupdate_validnrf > CONTROLUPDATEPERIOD) {
last_controlupdate_validnrf = loopmillis;
//out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
//out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
int16_t new_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
int16_t new_set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ) * -1; int16_t new_set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ) * -1;
if (!motorenabled) { //when safety off, reason: remote relased
if (abs(new_set_speed) < abs(set_speed)) { //only allow going slower
set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter
}
if (abs(new_set_steer) < abs(set_steer)) { //only allow turning slower
set_steer = set_steer*(1.0-filter_nrf_set_steer) + new_set_steer*filter_nrf_set_steer;
}
}else{
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;
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 //calculate speed l and r from speed and steer
int16_t _out_speedl,_out_speedr; int16_t _out_speedl,_out_speedr;
_out_speedl = constrain(set_speed * speed_coefficient_nrf + set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); _out_speedl = constrain(set_speed * speed_coefficient_nrf + set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
_out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); _out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
esc.setSpeed(_out_speedl,_out_speedr); esc.setSpeed(_out_speedl,_out_speedr);
}
} }
}//if pastpacket not ok, keep last out_steer and speed values until disarmed }//if pastpacket not ok, keep last out_steer and speed values until disarmed
@ -590,8 +611,9 @@ void loop() {
} }
//calculate speed l and r from speed and steer //calculate speed l and r from speed and steer
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { static unsigned long last_controlupdate_gt=0;
last_controlupdate = loopmillis; if (loopmillis - last_controlupdate_gt > CONTROLUPDATEPERIOD) {
last_controlupdate_gt = loopmillis;
int16_t _out_speedl,_out_speedr; int16_t _out_speedl,_out_speedr;
_out_speedl = constrain(set_speed * speed_coefficient_gt + set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier; _out_speedl = constrain(set_speed * speed_coefficient_gt + set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier;
_out_speedr = constrain(set_speed * speed_coefficient_gt - set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier; _out_speedr = constrain(set_speed * speed_coefficient_gt - set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier;
@ -611,8 +633,9 @@ void loop() {
set_speed=0; set_speed=0;
set_steer=0; set_steer=0;
} }
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { static unsigned long last_controlupdate_disarmed=0;
last_controlupdate = loopmillis; if (loopmillis - last_controlupdate_disarmed > CONTROLUPDATEPERIOD) {
last_controlupdate_disarmed = loopmillis;
set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
set_steer = set_steer*(1.0-filter_stop_set_steer); set_steer = set_steer*(1.0-filter_stop_set_steer);
int16_t _out_speedl,_out_speedr; int16_t _out_speedl,_out_speedr;
@ -624,36 +647,17 @@ void loop() {
if (esc.sendPending(loopmillis)) { if (!motorenabled) {//motors disabled. Used for safety off when remote released
static unsigned long last_controlupdate_motordisabled=0;
if (!motorenabled) {//motors disabled. Used for safety off when remote released if (loopmillis - last_controlupdate_motordisabled > CONTROLUPDATEPERIOD) {
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { last_controlupdate_motordisabled = loopmillis;
last_controlupdate = loopmillis; set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter set_steer = set_steer*(1.0-filter_stop_set_steer);
set_steer = set_steer*(1.0-filter_stop_set_steer); int16_t _out_speedl,_out_speedr;
int16_t _out_speedl,_out_speedr; _out_speedl = constrain(set_speed * speed_coefficient_nrf + set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
_out_speedl = constrain(set_speed * speed_coefficient_nrf + set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); _out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
_out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500); esc.setSpeed(_out_speedl,_out_speedr);
esc.setSpeed(_out_speedl,_out_speedr);
}
} }
last_send = loopmillis;
#ifdef DEBUG
Serial.print(" out_speedl=");
Serial.print(out_speedl);
Serial.print(" out_speedr=");
Serial.print(out_speedr);
Serial.print(" checksum=");
Serial.print(out_checksum);
Serial.print(" controlmode=");
Serial.print(controlmode);
Serial.println();
#endif
} }
@ -674,7 +678,8 @@ void loop() {
dataFile.print("current_L,current_R,"); dataFile.print("current_L,current_R,");
dataFile.print("rpm_L,rpm_R,"); dataFile.print("rpm_L,rpm_R,");
dataFile.print("temp,vbat,"); dataFile.print("temp,vbat,");
dataFile.println("trip,currentConsumed"); dataFile.println("trip,currentConsumed,motorenabled,disarmedByDelay");
dataFile.print("#TIMESTAMP:"); dataFile.println(now());
logging_headerWritten=true; logging_headerWritten=true;
} }
dataFile.print(String(loopmillis)); dataFile.print(";"); dataFile.print(String(loopmillis)); dataFile.print(";");
@ -687,7 +692,9 @@ void loop() {
dataFile.print(esc.getFeedback_boardTemp()); dataFile.print(";"); dataFile.print(esc.getFeedback_boardTemp()); dataFile.print(";");
dataFile.print(esc.getFeedback_batVoltage()); dataFile.print(";"); dataFile.print(esc.getFeedback_batVoltage()); dataFile.print(";");
dataFile.print(esc.getTrip()); dataFile.print(";"); dataFile.print(esc.getTrip()); dataFile.print(";");
dataFile.print(esc.getCurrentConsumed(),3); dataFile.print(esc.getCurrentConsumed(),3); dataFile.print(";");
dataFile.print(motorenabled); dataFile.print(";");
dataFile.print(debug_count_disarmedbecauseofnrfdelay);
dataFile.println(""); dataFile.println("");
dataFile.close(); dataFile.close();
} }
@ -1009,6 +1016,10 @@ void display_show_stats3() {
display.print(F("nrfdata x,y=")); display.print(lastnrfdata.steer); display.print(F(", ")); display.println(lastnrfdata.speed); display.print(F("nrfdata x,y=")); display.print(lastnrfdata.steer); display.print(F(", ")); display.println(lastnrfdata.speed);
display.print(F("nrfdata command=")); display.println(lastnrfdata.commands); display.print(F("nrfdata command=")); display.println(lastnrfdata.commands);
display.print(F("disarmed delay#=")); display.println(debug_count_disarmedbecauseofnrfdelay);
display.display(); // Show initial text display.display(); // Show initial text
} }