reverse steer internally

This commit is contained in:
interfisch 2022-07-14 19:39:34 +02:00
parent af4aa0946c
commit e7f7ffa8a1
2 changed files with 233 additions and 111 deletions

View File

@ -30,6 +30,8 @@ uint8_t displaymode=0;
#define DISPLAY_MENU 3 #define DISPLAY_MENU 3
uint8_t menu_entrypos=0; uint8_t menu_entrypos=0;
#define MENU_ENTRIES 8 // max id is MENU_ENTRIES-1 #define MENU_ENTRIES 8 // max id is MENU_ENTRIES-1
uint8_t menu_pagepos=0;
#define MENU_PAGES 3
uint8_t error = 0; uint8_t error = 0;
#define IMU_NO_CHANGE 2 //IMU values did not change for too long #define IMU_NO_CHANGE 2 //IMU values did not change for too long
@ -63,8 +65,8 @@ bool button_down_click=false;
#define PIN_GAMETRAK_HORIZONTAL A9 //A9=23 #define PIN_GAMETRAK_HORIZONTAL A9 //A9=23
#define SPEED_COEFFICIENT_NRF 1 // higher value == stronger float speed_coefficient_nrf=1.0; // higher value == stronger
#define STEER_COEFFICIENT_NRF 0.5 // higher value == stronger float steer_coefficient_nrf=0.5; // higher value == stronger
#define SPEED_COEFFICIENT_GT 1 // higher value == stronger #define SPEED_COEFFICIENT_GT 1 // higher value == stronger
#define STEER_COEFFICIENT_GT 0.5 // higher value == stronger #define STEER_COEFFICIENT_GT 0.5 // higher value == stronger
@ -509,7 +511,7 @@ void loop() {
//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 ); int16_t new_set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ) * -1;
set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter
@ -518,8 +520,8 @@ void loop() {
//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 * STEER_COEFFICIENT_NRF, -1500, 1500); _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); _out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * 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
@ -612,8 +614,8 @@ void loop() {
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 * STEER_COEFFICIENT_NRF, -1500, 1500); _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); _out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * steer_coefficient_nrf, -1500, 1500);
esc.setSpeed(_out_speedl,_out_speedr); esc.setSpeed(_out_speedl,_out_speedr);
} }
} }
@ -628,8 +630,8 @@ void loop() {
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 * STEER_COEFFICIENT_NRF, -1500, 1500); _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); _out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * steer_coefficient_nrf, -1500, 1500);
esc.setSpeed(_out_speedl,_out_speedr); esc.setSpeed(_out_speedl,_out_speedr);
} }
} }
@ -718,6 +720,8 @@ void updateInputs(unsigned long loopmillis) {
if (button_up_click) { if (button_up_click) {
if (menu_entrypos>0) { if (menu_entrypos>0) {
menu_entrypos--; menu_entrypos--;
}else{
displaymode=DISPLAY_STATS; //when at top entry click up again to go back to exit menu
} }
} }
if (button_down_click) { if (button_down_click) {
@ -727,10 +731,18 @@ void updateInputs(unsigned long loopmillis) {
} }
if (menu_pagepos==0) {
switch(menu_entrypos) { switch(menu_entrypos) {
case 0: //Exit case 0: //Change pages
if (button_inc_click || button_dec_click) { if (button_inc_click) {
displaymode=DISPLAY_STATS; if (menu_pagepos<MENU_PAGES-1) {
menu_pagepos++;
}
}
if (button_dec_click) {
if (menu_pagepos>0) {
menu_pagepos--;
}
} }
break; break;
case 1: //Filter NRF Speed case 1: //Filter NRF Speed
@ -798,9 +810,60 @@ void updateInputs(unsigned long loopmillis) {
esc.resetStatistics(); esc.resetStatistics();
} }
break; break;
}
}else if(menu_pagepos==1) {
switch(menu_entrypos) {
case 0: //Change Pages
if (button_inc_click) {
if (menu_pagepos<MENU_PAGES-1) {
menu_pagepos++;
}
}
if (button_dec_click) {
if (menu_pagepos>0) {
menu_pagepos--;
}
}
break;
case 1: // speed_coefficient_nrf
if (button_inc_click) {
speed_coefficient_nrf+=0.05;
speed_coefficient_nrf = constrain(speed_coefficient_nrf, 0.05, 1.0);
}
if (button_dec_click) {
speed_coefficient_nrf-=0.05;
speed_coefficient_nrf = constrain(speed_coefficient_nrf, 0.05, 1.0);
}
break;
case 2: // steer_coefficient_nrf
if (button_inc_click) {
steer_coefficient_nrf+=0.05;
steer_coefficient_nrf = constrain(steer_coefficient_nrf, 0.05, 1.0);
}
if (button_dec_click) {
steer_coefficient_nrf-=0.05;
steer_coefficient_nrf = constrain(steer_coefficient_nrf, 0.05, 1.0);
}
break;
}
}else if(menu_pagepos==2) {
switch(menu_entrypos) {
case 0: //Change Pages
if (button_inc_click) {
if (menu_pagepos<MENU_PAGES-1) {
menu_pagepos++;
}
}
if (button_dec_click) {
if (menu_pagepos>0) {
menu_pagepos--;
}
}
break;
}
} }
break; break;
@ -940,9 +1003,11 @@ void display_show_menu() {
}else{ }else{
display.print(F(" ")); display.print(F(" "));
} }
if(menu_pagepos==0) {
switch(i) { switch(i) {
case 0: case 0:
display.print(F("EXIT")); display.println(); //display.print(F("EXIT")); display.println();
display.print(F("# Page 0 / ")); display.print(MENU_PAGES-1); display.print(F(" #")); display.println();
break; break;
case 1: case 1:
display.print(F("Fltr spd=")); display.println(filter_nrf_set_speed,3); display.print(F("Fltr spd=")); display.println(filter_nrf_set_speed,3);
@ -970,6 +1035,63 @@ void display_show_menu() {
} }
break; break;
} }
}else if(menu_pagepos==1) {
switch(i) {
case 0:
//display.print(F("EXIT")); display.println();
display.print(F("# Page 1 / ")); display.print(MENU_PAGES-1); display.print(F(" NRF #")); display.println();
break;
case 1:
display.print(F("speed_coeff_nrf=")); display.println(speed_coefficient_nrf,2);
break;
case 2:
display.print(F("steer_coeff_nrf=")); display.println(steer_coefficient_nrf,2);
break;
case 3:
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
break;
}
}else if(menu_pagepos==2) {
switch(i) {
case 0:
//display.print(F("EXIT")); display.println();
display.print(F("# Page 2 / ")); display.print(MENU_PAGES-1); display.print(F(" GT #")); display.println();
break;
case 1:
display.print(F("speed_coeff_gt=")); display.println(SPEED_COEFFICIENT_GT,2);
break;
case 2:
display.print(F("steer_coeff_gt=")); display.println(STEER_COEFFICIENT_GT,2);
break;
case 3:
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
break;
}
}
} }

@ -1 +1 @@
Subproject commit a1da44f7c960a7f4bd84c686a5dc0d0f9191618c Subproject commit 75f87c284dd5e86e82faead13bd6022adf4bf5c8