diff --git a/hoverbrettctrl/src/main.cpp b/hoverbrettctrl/src/main.cpp index 77359e5..9483e31 100644 --- a/hoverbrettctrl/src/main.cpp +++ b/hoverbrettctrl/src/main.cpp @@ -22,7 +22,7 @@ uint8_t displaymode=0; #define DISPLAY_STATS3 2 #define DISPLAY_MENU 3 uint8_t menu_entrypos=0; -#define MENU_ENTRIES 3 // max id is MENU_ENTRIES-1 +#define MENU_ENTRIES 7 // max id is MENU_ENTRIES-1 uint8_t error = 0; #define IMU_NO_CHANGE 2 //IMU values did not change for too long @@ -120,9 +120,9 @@ uint16_t gt_length_set=1000; //set length to keep [mm] float gt_speed_p=0.7; //value to multipy difference [mm] with -> out_speed float gt_speedbackward_p=0.7; float gt_steer_p=2.0; -#define GT_SPEED_LIMIT 400 //maximum out_speed value + -#define GT_SPEEDBACKWARD_LIMIT 100//maximum out_speed value (for backward driving) - -#define GT_STEER_LIMIT 300 //maximum out_steer value +- +int16_t gt_speed_limit=400; //maximum out_speed value + +int16_t gt_backward_speed_limit=100; //maximum out_speed value (for backward driving) - +int16_t gt_steer_limit=300; //maximum out_steer value +- #define GT_LENGTH_MAXIMUMDIFFBACKWARD -250 //[mm]. if gt_length_set=1000 and GT_LENGTH_MAXIMUMDIFFBACKWARD=-200 then only drives backward if lenght is greater 800 @@ -433,13 +433,13 @@ void loop() { _gt_length_diff=0; //threshold } - set_steer=constrain((int16_t)(-gt_horizontal*gt_steer_p),-GT_STEER_LIMIT,GT_STEER_LIMIT); //steer positive is left //gt_horizontal left is negative + set_steer=constrain((int16_t)(-gt_horizontal*gt_steer_p),-gt_steer_limit,gt_steer_limit); //steer positive is left //gt_horizontal left is negative if (_gt_length_diff>0) { //needs to drive forward - set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),0,GT_SPEED_LIMIT); + set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),0,gt_speed_limit); }else{ //drive backward if (_gt_length_diff > GT_LENGTH_MAXIMUMDIFFBACKWARD){ //only drive if not pulled back too much - set_speed = constrain((int16_t)(_gt_length_diff*gt_speedbackward_p),-GT_SPEEDBACKWARD_LIMIT,0); + set_speed = constrain((int16_t)(_gt_length_diff*gt_speedbackward_p),-gt_backward_speed_limit,0); }else{ set_speed = 0; //stop set_steer = 0; @@ -582,6 +582,50 @@ void updateInputs(unsigned long loopmillis) { filter_nrf_set_steer = constrain(filter_nrf_set_steer, 0.01, 1.0); } break; + case 3: + if (button_inc_click) { + gt_speed_p+=0.05; + gt_speed_p = constrain(gt_speed_p, 0.05, 5.0); + } + if (button_dec_click) { + gt_speed_p-=0.05; + gt_speed_p = constrain(gt_speed_p, 0.05, 5.0); + } + break; + case 4: + if (button_inc_click) { + gt_steer_p+=0.05; + gt_steer_p = constrain(gt_steer_p, 0.05, 5.0); + } + if (button_dec_click) { + gt_steer_p-=0.05; + gt_steer_p = constrain(gt_steer_p, 0.05, 5.0); + } + break; + case 5: + if (button_inc_click) { + gt_speed_limit+=50; + gt_speed_limit = constrain(gt_speed_limit, 50, 1000); + } + if (button_dec_click) { + gt_speed_limit-=50; + gt_speed_limit = constrain(gt_speed_limit, 50, 1000); + } + break; + case 6: + if (button_inc_click) { + gt_steer_limit+=50; + gt_steer_limit = constrain(gt_steer_limit, 50, 1000); + } + if (button_dec_click) { + gt_steer_limit-=50; + gt_steer_limit = constrain(gt_steer_limit, 50, 1000); + } + break; + + + + } break; @@ -711,7 +755,20 @@ void display_show_menu() { case 2: display.print(F("Fltr str=")); display.println(filter_nrf_set_steer); break; + case 3: + display.print(F("gt_speed_p=")); display.println(gt_speed_p); + break; + case 4: + display.print(F("gt_steer_p=")); display.println(gt_steer_p); + break; + case 5: + display.print(F("gt_speed_limit=")); display.println(gt_speed_limit); + break; + case 6: + display.print(F("gt_steer_limit=")); display.println(gt_steer_limit); + break; } + } display.display(); // Show initial text