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,80 +731,139 @@ void updateInputs(unsigned long loopmillis) {
} }
switch(menu_entrypos) { if (menu_pagepos==0) {
case 0: //Exit switch(menu_entrypos) {
if (button_inc_click || button_dec_click) { case 0: //Change pages
displaymode=DISPLAY_STATS; if (button_inc_click) {
} if (menu_pagepos<MENU_PAGES-1) {
break; menu_pagepos++;
case 1: //Filter NRF Speed }
if (button_inc_click) { }
filter_nrf_set_speed+=0.005; if (button_dec_click) {
filter_nrf_set_speed = constrain(filter_nrf_set_speed, 0.01, 1.0); if (menu_pagepos>0) {
} menu_pagepos--;
if (button_dec_click) { }
filter_nrf_set_speed-=0.005; }
filter_nrf_set_speed = constrain(filter_nrf_set_speed, 0.01, 1.0); break;
} case 1: //Filter NRF Speed
break; if (button_inc_click) {
case 2: //Filter NRF Steer filter_nrf_set_speed+=0.005;
if (button_inc_click) { filter_nrf_set_speed = constrain(filter_nrf_set_speed, 0.01, 1.0);
filter_nrf_set_steer+=0.01; }
filter_nrf_set_steer = constrain(filter_nrf_set_steer, 0.01, 1.0); if (button_dec_click) {
} filter_nrf_set_speed-=0.005;
if (button_dec_click) { filter_nrf_set_speed = constrain(filter_nrf_set_speed, 0.01, 1.0);
filter_nrf_set_steer-=0.01; }
filter_nrf_set_steer = constrain(filter_nrf_set_steer, 0.01, 1.0); break;
} case 2: //Filter NRF Steer
break; if (button_inc_click) {
case 3: filter_nrf_set_steer+=0.01;
if (button_inc_click) { filter_nrf_set_steer = constrain(filter_nrf_set_steer, 0.01, 1.0);
gt_speed_p+=0.05; }
gt_speed_p = constrain(gt_speed_p, 0.05, 5.0); if (button_dec_click) {
} filter_nrf_set_steer-=0.01;
if (button_dec_click) { filter_nrf_set_steer = constrain(filter_nrf_set_steer, 0.01, 1.0);
gt_speed_p-=0.05; }
gt_speed_p = constrain(gt_speed_p, 0.05, 5.0); break;
} case 3:
break; if (button_inc_click) {
case 4: gt_speed_p+=0.05;
if (button_inc_click) { gt_speed_p = constrain(gt_speed_p, 0.05, 5.0);
gt_steer_p+=0.05; }
gt_steer_p = constrain(gt_steer_p, 0.05, 5.0); if (button_dec_click) {
} gt_speed_p-=0.05;
if (button_dec_click) { gt_speed_p = constrain(gt_speed_p, 0.05, 5.0);
gt_steer_p-=0.05; }
gt_steer_p = constrain(gt_steer_p, 0.05, 5.0); break;
} case 4:
break; if (button_inc_click) {
case 5: gt_steer_p+=0.05;
if (button_inc_click) { gt_steer_p = constrain(gt_steer_p, 0.05, 5.0);
gt_speed_limit+=50; }
gt_speed_limit = constrain(gt_speed_limit, 50, 1000); if (button_dec_click) {
} gt_steer_p-=0.05;
if (button_dec_click) { gt_steer_p = constrain(gt_steer_p, 0.05, 5.0);
gt_speed_limit-=50; }
gt_speed_limit = constrain(gt_speed_limit, 50, 1000); break;
} case 5:
break; if (button_inc_click) {
case 6: gt_speed_limit+=50;
if (button_inc_click) { gt_speed_limit = constrain(gt_speed_limit, 50, 1000);
gt_steer_limit+=50; }
gt_steer_limit = constrain(gt_steer_limit, 50, 1000); if (button_dec_click) {
} gt_speed_limit-=50;
if (button_dec_click) { gt_speed_limit = constrain(gt_speed_limit, 50, 1000);
gt_steer_limit-=50; }
gt_steer_limit = constrain(gt_steer_limit, 50, 1000); break;
} case 6:
break; if (button_inc_click) {
case 7: gt_steer_limit+=50;
if (button_inc_click) { gt_steer_limit = constrain(gt_steer_limit, 50, 1000);
esc.resetStatistics(); }
} if (button_dec_click) {
break; gt_steer_limit-=50;
gt_steer_limit = constrain(gt_steer_limit, 50, 1000);
}
break;
case 7:
if (button_inc_click) {
esc.resetStatistics();
}
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,35 +1003,94 @@ void display_show_menu() {
}else{ }else{
display.print(F(" ")); display.print(F(" "));
} }
switch(i) { if(menu_pagepos==0) {
case 0: switch(i) {
display.print(F("EXIT")); display.println(); case 0:
break; //display.print(F("EXIT")); display.println();
case 1: display.print(F("# Page 0 / ")); display.print(MENU_PAGES-1); display.print(F(" #")); display.println();
display.print(F("Fltr spd=")); display.println(filter_nrf_set_speed,3); break;
break; case 1:
case 2: display.print(F("Fltr spd=")); display.println(filter_nrf_set_speed,3);
display.print(F("Fltr str=")); display.println(filter_nrf_set_steer,3); break;
break; case 2:
case 3: display.print(F("Fltr str=")); display.println(filter_nrf_set_steer,3);
display.print(F("gt_speed_p=")); display.println(gt_speed_p); break;
break; case 3:
case 4: display.print(F("gt_speed_p=")); display.println(gt_speed_p);
display.print(F("gt_steer_p=")); display.println(gt_steer_p); break;
break; case 4:
case 5: display.print(F("gt_steer_p=")); display.println(gt_steer_p);
display.print(F("gt_speed_limit=")); display.println(gt_speed_limit); break;
break; case 5:
case 6: display.print(F("gt_speed_limit=")); display.println(gt_speed_limit);
display.print(F("gt_steer_limit=")); display.println(gt_steer_limit); break;
break; case 6:
case 7: display.print(F("gt_steer_limit=")); display.println(gt_steer_limit);
if ((loopmillis/1000)%2==0) { break;
display.print(F("Inc to reset stats")); case 7:
}else{ if ((loopmillis/1000)%2==0) {
display.print(F("t=")); display.println(esc.getTripTime(loopmillis)/1000); display.println(F("s")); display.print(F("Inc to reset stats"));
} }else{
break; display.print(F("t=")); display.println(esc.getTripTime(loopmillis)/1000); display.println(F("s"));
}
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