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

Compare commits

base: interfisch:9987a1693de3f550b703e394b872450f0b8ff9ee
Branches Tags
interfisch:master
...
compare: interfisch:3a04a12713cf352780897b5dfeeb008c4ece6eca
Branches Tags
interfisch:master

2 Commits
9987a1693d ... 3a04a12713

Author SHA1 Message Date
interfisch 3a04a12713 add max nrfreceivedelay to display 2022-06-19 20:10:31 +02:00
interfisch 85f200da22 add smooth slowdown for safety stop 2022-06-19 20:07:18 +02:00
1 changed files with 49 additions and 26 deletions
Show Stats Expand all files Collapse all files

75
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
@ -157,6 +166,7 @@ nrfdata lastnrfdata;
long last_nrfreceive = 0; //last time values were received and checksum ok
unsigned long nrf_delay = 0;
unsigned long last_nrfreceive_delay=0;
unsigned long max_last_nrfreceive_delay=0; //maximum value (used for display)
#define MAX_NRFDELAY 100 //ms. maximum time delay at which vehicle will disarm
boolean radiosendOk=false;
@ -172,7 +182,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;
@ -433,11 +442,13 @@ void loop() {
if (lastnrfdata.checksum == calcchecksum) { //checksum ok?
lastpacketOK = true;
last_nrfreceive_delay=loopmillis-last_nrfreceive; //for display purpose
max_last_nrfreceive_delay=max(last_nrfreceive_delay,max_last_nrfreceive_delay);
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 +477,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 +503,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 +547,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 +561,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;
@ -650,6 +672,7 @@ void updateInputs(unsigned long loopmillis) {
}
if (button_down_click) {
displaymode=DISPLAY_STATS3;
max_last_nrfreceive_delay=0; //reset when switching to this display
}
break;
case DISPLAY_MENU:
@ -856,7 +879,7 @@ void display_show_stats3() {
display.print(F("looptime=")); display.println(loopmillis-last_loopmillis);
display.print(F("feedback=")); display.println(esc.getFeedbackInterval());
display.print(F("nrf_delay=")); display.println(last_nrfreceive_delay);
display.print(F("nrf_delay=")); display.println(last_nrfreceive_delay); display.print(F(" max=")); display.println(max_last_nrfreceive_delay);
display.print(F("maxdiff=")); display.println(raw_length_maxdiff);