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

Compare commits

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

No commits in common. "3a04a12713cf352780897b5dfeeb008c4ece6eca" and "9987a1693de3f550b703e394b872450f0b8ff9ee" have entirely different histories.
3a04a12713 ... 9987a1693d

1 changed files with 26 additions and 49 deletions
Show Stats Expand all files Collapse all files

63
hoverbrettctrl/src/main.cpp
View File

@ -63,13 +63,7 @@ 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
@ -79,9 +73,6 @@ 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
@ -166,7 +157,6 @@ 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;
@ -182,6 +172,7 @@ 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;
@ -442,13 +433,11 @@ 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 . Used for safety-off when remote released
motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0
}
}
@ -477,7 +466,8 @@ 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);
@ -503,7 +493,6 @@ 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;
@ -547,52 +536,41 @@ void loop() {
}
//calculate speed l and r from speed and steer
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
last_controlupdate = loopmillis;
#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 (error > 0) { //disarm if error occured
controlmode = MODE_DISARMED; //force disarmed
}
if (controlmode == MODE_DISARMED){ //all disarmed
#define WATCHDOG_TIMEOUT 2000
if (millis()-watchdogtimer>WATCHDOG_TIMEOUT) {
set_speed=0;
esc.setSpeed(0,0);
set_speed=0; //reset filter
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)) {
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);
//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);
}
last_send = loopmillis;
@ -672,7 +650,6 @@ 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:
@ -879,7 +856,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(" max=")); display.println(max_last_nrfreceive_delay);
display.print(F("nrf_delay=")); display.println(last_nrfreceive_delay);
display.print(F("maxdiff=")); display.println(raw_length_maxdiff);