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

add smooth slowdown for safety stop

This commit is contained in:
interfisch 2022-06-19 20:07:18 +02:00
parent 9987a1693d
commit 85f200da22
1 changed files with 45 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
hoverbrettctrl/src/main.cpp
View File

@ -63,7 +63,13 @@ bool button_down_click=false;
#define PIN_GAMETRAK_HORIZONTAL A9 //A9=23
#define SPEED_COEFFICIENT_NRF 1 // higher value == stronger
#define STEER_COEFFICIENT_NRF 0.5 // higher value == stronger
#define SPEED_COEFFICIENT_GT 1 // higher value == stronger
#define STEER_COEFFICIENT_GT 0.5 // higher value == stronger
unsigned long watchdogtimer=0; //set to current millis everytime new good control input is calculated
long last_adcupdated=0;
#define ADC_UPDATEPERIOD 10 //in ms
@ -73,6 +79,9 @@ long last_controlupdate = 0;
float filter_nrf_set_speed=0.02; //higher value, faster response. depends on CONTROLUPDATEPERIOD
float filter_nrf_set_steer=0.06;
float filter_stop_set_speed=0.015; //safety stop
float filter_stop_set_steer=0.06; //safety stop
#define GT_LENGTH_MIN 200 //minimum length for stuff to start happen
#define GT_LENGTH_1_OFFSET -22.5
@ -172,7 +181,6 @@ long last_send = 0;
int16_t set_speed = 0;
int16_t set_steer = 0;
uint8_t out_checksum = 0; //0= disable motors, 255=reserved, 1<=checksum<255
#define NRFDATA_CENTER 127
//boolean armed = false;
@ -435,9 +443,10 @@ void loop() {
last_nrfreceive_delay=loopmillis-last_nrfreceive; //for display purpose
last_nrfreceive = loopmillis;
watchdogtimer=loopmillis; //reset watchdog
//parse commands
motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0
motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0 . Used for safety-off when remote released
}
}
@ -466,8 +475,7 @@ void loop() {
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
int16_t _out_speedl,_out_speedr;
_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);
@ -493,6 +501,7 @@ void loop() {
}else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
//Gametrak Control Code
motorenabled=true;
watchdogtimer=loopmillis; //reset watchdog
if (gt_length<=GT_LENGTH_MIN){ //let go
//Serial.println("gametrak released");
controlmode=MODE_DISARMED;
@ -536,12 +545,13 @@ void loop() {
}
//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)*safetymultiplier;
_out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT - set_steer * STEER_COEFFICIENT_GT, -1000, 1000)*safetymultiplier;
esc.setSpeed(_out_speedl,_out_speedr);
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
last_controlupdate = loopmillis;
int16_t _out_speedl,_out_speedr;
_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);
}
}
@ -549,28 +559,38 @@ void loop() {
controlmode = MODE_DISARMED; //force disarmed
}
if (controlmode == MODE_DISARMED){ //all disarmed
esc.setSpeed(0,0);
set_speed=0; //reset filter
set_steer=0;
#define WATCHDOG_TIMEOUT 2000
if (millis()-watchdogtimer>WATCHDOG_TIMEOUT) {
set_speed=0;
set_steer=0;
}
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
last_controlupdate = loopmillis;
set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
set_steer = set_steer*(1.0-filter_stop_set_steer);
int16_t _out_speedl,_out_speedr;
_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 (esc.sendPending(loopmillis)) {
//calculate checksum
out_checksum = ((uint8_t) ((uint8_t)esc.getCmdL()) * ((uint8_t)esc.getCmdR())); //simple checksum
if (out_checksum == 0 || out_checksum == 255) {
out_checksum = 1; //cannot be 0 or 255 (special purpose)
}
if (!motorenabled) { //disable motors?
out_checksum = 0; //checksum=0 disables motors
}
if (!motorenabled) {//motors disabled
esc.setSpeed(0,0);
if (!motorenabled) {//motors disabled. Used for safety off when remote released
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
last_controlupdate = loopmillis;
set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
set_steer = set_steer*(1.0-filter_stop_set_steer);
int16_t _out_speedl,_out_speedr;
_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);
}
}
last_send = loopmillis;