James O'Keeffe
/
BeaconDemo_RobotCodeNew
ft. button press reset
Fork of BeaconDemo_RobotCode by
Diff: PsiSwarm/demo.cpp
- Revision:
- 3:cd048f6e544e
- Parent:
- 2:a6214fd156ff
- Child:
- 5:598298aa4900
diff -r a6214fd156ff -r cd048f6e544e PsiSwarm/demo.cpp --- a/PsiSwarm/demo.cpp Mon Oct 05 14:42:16 2015 +0000 +++ b/PsiSwarm/demo.cpp Mon Oct 05 20:42:37 2015 +0000 @@ -17,6 +17,11 @@ #include "psiswarm.h" +// PID terms +#define LF_P_TERM 0.2 +#define LF_I_TERM 0 +#define LF_D_TERM 4 + char top_menu = 0; char sub_menu = 0; char level = 0; @@ -48,6 +53,15 @@ char stress_step = 0; +float lf_right; +float lf_left; +float lf_current_pos_of_line = 0.0; +float lf_previous_pos_of_line = 0.0; +float lf_derivative,lf_proportional,lf_integral = 0; +float lf_power; +float lf_speed = 0.4; + + void demo_mode() { @@ -180,7 +194,7 @@ if(side_ir_index == 0) side_ir_index = 7; else side_ir_index --; } - if(sub_menu == 10) level = 0; + if(sub_menu == 11) level = 0; break; case 2: // Motor Menu if(sub_menu == 0) { @@ -228,6 +242,15 @@ if(sub_menu == 6) level = 0; break; case 5: // Demo Menu + if(sub_menu == 0) { + if(demo_running == 0) { + start_line_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + } + } if(sub_menu == 1) { if(demo_running == 0) { start_obstacle_demo(); @@ -332,7 +355,7 @@ if(side_ir_index == 7) side_ir_index = 0; else side_ir_index ++; } - if(sub_menu == 10) level = 0; + if(sub_menu == 11) level = 0; break; case 2: // Motor Menu if(sub_menu == 0) { @@ -380,6 +403,15 @@ if(sub_menu == 6) level = 0; break; case 5: // Demo Menu + if(sub_menu == 0) { + if(demo_running == 0) { + start_line_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + } + } if(sub_menu == 1) { if(demo_running == 0) { start_obstacle_demo(); @@ -427,7 +459,7 @@ else sub_menu --; break; case 1: //Sensors Menu - if(sub_menu == 0) sub_menu = 10; + if(sub_menu == 0) sub_menu = 11; else sub_menu --; break; @@ -461,7 +493,7 @@ else sub_menu ++; break; case 1: //Sensors Menu - if(sub_menu == 10) sub_menu = 0; + if(sub_menu == 11) sub_menu = 0; else sub_menu ++; break; case 2: //Motor Menu @@ -503,7 +535,7 @@ if(sub_menu == 13) level = 0; break; case 1: // Sensors Menu - if(sub_menu == 10) level = 0; + if(sub_menu == 11) level = 0; break; case 2: //Motor Menu if(sub_menu == 2) { @@ -676,6 +708,11 @@ update_ultrasonic_measure(); break; case 10: + store_line_position(); + if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position); + else sprintf(bottomline,"LINE:---------"); + break; + case 11: sprintf(bottomline,"EXIT"); break; } @@ -788,6 +825,17 @@ } } +void start_line_demo() +{ + display.set_backlight_brightness(0); + time_out = 0.25f; + demo_timer.start(); + state = 0; + speed = 0; + led_step = 0; + demo_running = 1; + demo_timeout.attach_us(&line_demo_cycle,1000); +} void start_obstacle_demo() { @@ -830,6 +878,87 @@ demo_timeout.attach_us(&spinning_demo_cycle,1000); } +void line_demo_cycle() +{ + if(demo_timer.read() > time_out) { + store_line_position(); + if(line_found) { + time_out = 0.01f; + state = 0; + // Get the position of the line. + lf_current_pos_of_line = line_position; + lf_proportional = lf_current_pos_of_line; + + // Compute the derivative + lf_derivative = lf_current_pos_of_line - lf_previous_pos_of_line; + + // Compute the integral + lf_integral += lf_proportional; + + // Remember the last position. + lf_previous_pos_of_line = lf_current_pos_of_line; + + // Compute the power + lf_power = (lf_proportional * (LF_P_TERM) ) + (lf_integral*(LF_I_TERM)) + (lf_derivative*(LF_D_TERM)) ; + + // Compute new speeds + lf_right = lf_speed-lf_power; + lf_left = lf_speed+lf_power; + + // limit checks + if (lf_right < 0) + lf_right = 0; + else if (lf_right > 1.0f) + lf_right = 1.0f; + + if (lf_left < 0) + lf_left = 0; + else if (lf_left > 1.0f) + lf_left = 1.0f; + }else{ + //Cannot see line: hunt for it + if(lf_left > lf_right){ + //Currently turning left, keep turning left + state ++; + float d_step = state * 0.04; + lf_left = 0.2 + d_step; + lf_right = -0.2 - d_step; + if(state > 20){ + state = 0; + lf_right = 0.2; + lf_left = -0.2; + time_out += 0.01f; + if(time_out > 0.1f) demo_running = 0; + } + }else{ + //Currently turning right, keep turning right + state ++; + float d_step = state * 0.04; + lf_left = -0.2 - d_step; + lf_right = 0.2 + d_step; + if(state > 20){ + state = 0; + lf_right = -0.2; + lf_left = 0.2; + time_out += 0.01f; + if(time_out > 0.1f) demo_running = 0; + } + } + } + // set speed + set_left_motor_speed(lf_left); + set_right_motor_speed(lf_right); + + + demo_timer.reset(); + } + if(demo_running == 1)demo_timeout.attach_us(&line_demo_cycle,500); + else { + stop(); + display.set_backlight_brightness(bl_brightness * 0.01f); + } +} + void stress_demo_cycle() { if(demo_timer.read() > time_out) {