Science Memeseum
/
BeaconDemo_RobotCode
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of
PsiSwarm-BeaconDemo_Bluetooth
by 
James Wilson
Embed:
(wiki syntax)
Show/hide line numbers
demo.cpp
00001 /* University of York Robotics Laboratory PsiSwarm Library: Demo Mode Source File 00002 * 00003 * File: demo.cpp 00004 * 00005 * (C) Dept. Electronics & Computer Science, University of York 00006 * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis 00007 * 00008 * PsiSwarm Library Version: 0.3 00009 * 00010 * Version 0.2 : Remove most the functionality from center-button push to allow all operations to be accessable from 00011 * four directions alone. 00012 * Added extra sensor information, added various testing demos 00013 * 00014 * October 2015 00015 * 00016 */ 00017 00018 00019 #include "psiswarm.h" 00020 00021 // PID terms 00022 #define LF_P_TERM 0.2 00023 #define LF_I_TERM 0 00024 #define LF_D_TERM 4 00025 00026 char top_menu = 0; 00027 char sub_menu = 0; 00028 char level = 0; 00029 char started = 0; 00030 char topline[17]; 00031 char bottomline[17]; 00032 char led_state[9]; 00033 char all_led_state = 0; 00034 char base_led_state = 0; 00035 char brightness = 20; 00036 char bl_brightness = 100; 00037 char base_ir_index = 0; 00038 char side_ir_index = 0; 00039 signed short left_speed = 0; 00040 signed short right_speed = 0; 00041 char both_motor_mode = 0; 00042 char last_switch_pressed; 00043 Timeout demo_event; 00044 char handling_event = 0; 00045 00046 Timeout demo_timeout; 00047 char demo_running = 0; 00048 Timer demo_timer; 00049 float time_out; 00050 float speed; 00051 char state; 00052 char led_step = 0; 00053 char spin_step = 0; 00054 char stress_step = 0; 00055 00056 00057 float lf_right; 00058 float lf_left; 00059 float lf_current_pos_of_line = 0.0; 00060 float lf_previous_pos_of_line = 0.0; 00061 float lf_derivative,lf_proportional,lf_integral = 0; 00062 float lf_power; 00063 float lf_speed = 0.4; 00064 00065 00066 00067 void demo_mode() 00068 { 00069 debug("- Starting Demo Mode\n"); 00070 demo_on = 1; 00071 display.set_backlight_brightness(bl_brightness * 0.01f); 00072 display.clear_display(); 00073 display.write_string("PSI SWARM SYSTEM"); 00074 display.set_position(1,0); 00075 display.write_string(" DEMO MODE"); 00076 wait(0.5); 00077 display.clear_display(); 00078 display.write_string("Use cursor to"); 00079 display.set_position(1,0); 00080 display.write_string("navigate menus"); 00081 char step = 0; 00082 while(demo_on) { 00083 if(demo_running == 0) { 00084 switch(step) { 00085 case 0: 00086 mbed_led1 = 1; 00087 mbed_led2 = 0; 00088 break; 00089 case 1: 00090 mbed_led2 = 1; 00091 mbed_led1 = 0; 00092 break; 00093 } 00094 step++; 00095 if(step==2)step=0; 00096 } else { 00097 mbed_led1 = 0; 00098 mbed_led2 = 0; 00099 mbed_led3 = 0; 00100 mbed_led4 = 0; 00101 } 00102 wait(0.5); 00103 } 00104 debug("- Demo mode ended\n"); 00105 } 00106 00107 void demo_handle_switch_event(char switch_pressed) 00108 { 00109 if(!handling_event) { 00110 handling_event = 1; 00111 last_switch_pressed = switch_pressed; 00112 demo_event.attach_us(&demo_event_thread, 1000); 00113 } 00114 } 00115 00116 void demo_event_thread() 00117 { 00118 handling_event = 0; 00119 if(started == 1) { 00120 switch(last_switch_pressed) { 00121 case 1: //Up pressed 00122 switch(level) { 00123 case 0: 00124 if(top_menu < 6) { 00125 level++; 00126 sub_menu = 0; 00127 } else { 00128 demo_on = 0; 00129 user_code_running = user_code_restore_mode; 00130 } 00131 break; 00132 case 1: 00133 switch(top_menu) { 00134 case 0: //LED Menu 00135 if(sub_menu < 9) { 00136 if(led_state[sub_menu] == 0) led_state[sub_menu] = 3; 00137 else led_state[sub_menu]--; 00138 demo_update_leds(); 00139 } 00140 if(sub_menu == 9) { 00141 if(all_led_state == 0) all_led_state = 3; 00142 else all_led_state--; 00143 for(int i=0; i<9; i++) { 00144 led_state[i]=all_led_state; 00145 } 00146 demo_update_leds(); 00147 } 00148 if(sub_menu == 10) { 00149 base_led_state = 1 - base_led_state; 00150 demo_update_leds(); 00151 } 00152 if(sub_menu == 11) { 00153 switch(brightness) { 00154 case 100: 00155 brightness = 50; 00156 break; 00157 case 2: 00158 brightness = 1; 00159 break; 00160 case 5: 00161 brightness = 2; 00162 break; 00163 case 10: 00164 brightness = 5; 00165 break; 00166 case 20: 00167 brightness = 10; 00168 break; 00169 case 50: 00170 brightness = 20; 00171 break; 00172 } 00173 demo_update_leds(); 00174 } 00175 if(sub_menu == 12) { 00176 if(bl_brightness > 0) bl_brightness-=10; 00177 display.set_backlight_brightness(bl_brightness * 0.01f); 00178 } 00179 if(sub_menu == 13) level = 0; 00180 break; 00181 case 1: // Sensors Menu 00182 if(sub_menu == 4 || sub_menu == 5) { 00183 if(base_ir_index == 0) base_ir_index = 4; 00184 else base_ir_index --; 00185 } 00186 if(sub_menu > 5 && sub_menu < 9) { 00187 if(side_ir_index == 0) side_ir_index = 7; 00188 else side_ir_index --; 00189 } 00190 if(sub_menu == 11) level = 0; 00191 break; 00192 case 2: // Motor Menu 00193 if(sub_menu == 0) { 00194 left_speed += 5; 00195 if(left_speed > 100) left_speed = 100; 00196 set_left_motor_speed(left_speed / 100.0f); 00197 } 00198 if(sub_menu == 1) { 00199 right_speed += 5; 00200 if(right_speed > 100) right_speed = 100; 00201 set_right_motor_speed(right_speed / 100.0f); 00202 } 00203 if(sub_menu == 2) { 00204 if(both_motor_mode == 0) both_motor_mode=5; 00205 else both_motor_mode--; 00206 switch(both_motor_mode) { 00207 case 0: 00208 stop(); 00209 break; 00210 case 1: 00211 brake(); 00212 break; 00213 case 2: 00214 forward(0.5); 00215 break; 00216 case 3: 00217 forward(1); 00218 break; 00219 case 4: 00220 backward(0.5); 00221 break; 00222 case 5: 00223 backward(1.0); 00224 break; 00225 } 00226 } 00227 if(sub_menu == 3) { 00228 level = 0; 00229 } 00230 break; 00231 case 3: // Radio Menu 00232 if(sub_menu == 0) level = 0; 00233 break; 00234 case 4: // Info Menu 00235 if(sub_menu == 6) level = 0; 00236 break; 00237 case 5: // Demo Menu 00238 if(sub_menu == 0) { 00239 if(demo_running == 0) { 00240 start_line_demo(); 00241 } else { 00242 demo_running = 0; 00243 display.set_backlight_brightness(bl_brightness * 0.01f); 00244 stop(); 00245 } 00246 } 00247 if(sub_menu == 1) { 00248 if(demo_running == 0) { 00249 start_obstacle_demo(); 00250 } else { 00251 demo_running = 0; 00252 display.set_backlight_brightness(bl_brightness * 0.01f); 00253 stop(); 00254 } 00255 } 00256 if(sub_menu == 2) { 00257 if(demo_running == 0) { 00258 start_spinning_demo(); 00259 } else { 00260 demo_running = 0; 00261 display.set_backlight_brightness(bl_brightness * 0.01f); 00262 stop(); 00263 } 00264 } 00265 if(sub_menu == 3) { 00266 if(demo_running == 0) { 00267 start_stress_demo(); 00268 } else { 00269 demo_running = 0; 00270 display.set_backlight_brightness(bl_brightness * 0.01f); 00271 stop(); 00272 } 00273 } 00274 if(sub_menu == 4) level = 0; 00275 break; 00276 } 00277 break; 00278 } 00279 break; 00280 case 2: //Down pressed 00281 switch(level) { 00282 case 0: 00283 if(top_menu < 6) { 00284 level++; 00285 sub_menu = 0; 00286 } else { 00287 demo_on = 0; 00288 user_code_running = user_code_restore_mode; 00289 } 00290 break; 00291 case 1: 00292 switch(top_menu) { 00293 case 0: //LED Menu 00294 if(sub_menu < 9) { 00295 led_state[sub_menu]++; 00296 if(led_state[sub_menu] == 4) led_state[sub_menu] = 0; 00297 demo_update_leds(); 00298 } 00299 if(sub_menu == 9) { 00300 all_led_state++; 00301 if(all_led_state == 4) all_led_state = 0; 00302 for(int i=0; i<9; i++) { 00303 led_state[i]=all_led_state; 00304 } 00305 demo_update_leds(); 00306 } 00307 if(sub_menu == 10) { 00308 base_led_state = 1 - base_led_state; 00309 demo_update_leds(); 00310 } 00311 if(sub_menu == 11) { 00312 switch(brightness) { 00313 case 1: 00314 brightness = 2; 00315 break; 00316 case 2: 00317 brightness = 5; 00318 break; 00319 case 5: 00320 brightness = 10; 00321 break; 00322 case 10: 00323 brightness = 20; 00324 break; 00325 case 20: 00326 brightness = 50; 00327 break; 00328 case 50: 00329 brightness = 100; 00330 break; 00331 } 00332 demo_update_leds(); 00333 } 00334 00335 if(sub_menu == 12) { 00336 if(bl_brightness < 100) bl_brightness+=10; 00337 display.set_backlight_brightness(bl_brightness * 0.01f); 00338 } 00339 if(sub_menu == 13) level = 0; 00340 00341 break; 00342 case 1: // Sensors Menu 00343 if(sub_menu == 4 || sub_menu == 5) { 00344 if(base_ir_index == 4) base_ir_index = 0; 00345 else base_ir_index ++; 00346 } 00347 if(sub_menu > 5 && sub_menu < 9) { 00348 if(side_ir_index == 7) side_ir_index = 0; 00349 else side_ir_index ++; 00350 } 00351 if(sub_menu == 11) level = 0; 00352 break; 00353 case 2: // Motor Menu 00354 if(sub_menu == 0) { 00355 left_speed -= 5; 00356 if(left_speed < -100) left_speed = -100; 00357 set_left_motor_speed(left_speed / 100.0f); 00358 } 00359 if(sub_menu == 1) { 00360 right_speed -= 5; 00361 if(right_speed < -100) right_speed = -100; 00362 set_right_motor_speed(right_speed / 100.0f); 00363 } 00364 if(sub_menu == 2) { 00365 both_motor_mode++; 00366 if(both_motor_mode == 6) both_motor_mode=0; 00367 switch(both_motor_mode) { 00368 case 0: 00369 stop(); 00370 break; 00371 case 1: 00372 brake(); 00373 break; 00374 case 2: 00375 forward(0.5); 00376 break; 00377 case 3: 00378 forward(1); 00379 break; 00380 case 4: 00381 backward(0.5); 00382 break; 00383 case 5: 00384 backward(1.0); 00385 break; 00386 } 00387 } 00388 if(sub_menu == 3) { 00389 level = 0; 00390 } 00391 break; 00392 case 3: // Radio Menu 00393 if(sub_menu == 0) level = 0; 00394 break; 00395 case 4: // Info Menu 00396 if(sub_menu == 6) level = 0; 00397 break; 00398 case 5: // Demo Menu 00399 if(sub_menu == 0) { 00400 if(demo_running == 0) { 00401 start_line_demo(); 00402 } else { 00403 demo_running = 0; 00404 display.set_backlight_brightness(bl_brightness * 0.01f); 00405 stop(); 00406 } 00407 } 00408 if(sub_menu == 1) { 00409 if(demo_running == 0) { 00410 start_obstacle_demo(); 00411 } else { 00412 demo_running = 0; 00413 display.set_backlight_brightness(bl_brightness * 0.01f); 00414 stop(); 00415 00416 } 00417 } 00418 if(sub_menu == 2) { 00419 if(demo_running == 0) { 00420 start_spinning_demo(); 00421 } else { 00422 demo_running = 0; 00423 display.set_backlight_brightness(bl_brightness * 0.01f); 00424 stop(); 00425 } 00426 } 00427 if(sub_menu == 3) { 00428 if(demo_running == 0) { 00429 start_stress_demo(); 00430 } else { 00431 demo_running = 0; 00432 display.set_backlight_brightness(bl_brightness * 0.01f); 00433 stop(); 00434 } 00435 } 00436 if(sub_menu == 4) level = 0; 00437 break; 00438 } 00439 break; 00440 } 00441 break; 00442 case 4: //Left pressed 00443 switch(level) { 00444 case 0: 00445 if(top_menu == 0) top_menu = 6; 00446 else top_menu --; 00447 break; 00448 case 1: 00449 switch(top_menu) { 00450 case 0: //LED Menu 00451 if(sub_menu == 0) sub_menu = 13; 00452 else sub_menu --; 00453 break; 00454 case 1: //Sensors Menu 00455 if(sub_menu == 0) sub_menu = 11; 00456 else sub_menu --; 00457 break; 00458 00459 case 2: //Motor Menu 00460 if(sub_menu == 0) sub_menu = 3; 00461 else sub_menu --; 00462 break; 00463 case 4: //Info Menu 00464 if(sub_menu == 0) sub_menu = 6; 00465 else sub_menu --; 00466 break; 00467 case 5: //Demo Menu 00468 if(sub_menu == 0) sub_menu = 4; 00469 else sub_menu --; 00470 break; 00471 } 00472 break; 00473 00474 } 00475 break; 00476 case 8: //Right pressed 00477 switch(level) { 00478 case 0: 00479 top_menu ++; 00480 if(top_menu > 6) top_menu = 0; 00481 break; 00482 case 1: 00483 switch(top_menu) { 00484 case 0: //LED Menu 00485 if(sub_menu == 13) sub_menu = 0; 00486 else sub_menu ++; 00487 break; 00488 case 1: //Sensors Menu 00489 if(sub_menu == 11) sub_menu = 0; 00490 else sub_menu ++; 00491 break; 00492 case 2: //Motor Menu 00493 if(sub_menu == 3) sub_menu = 0; 00494 else sub_menu ++; 00495 break; 00496 case 4: //Info Menu 00497 if(sub_menu == 6) sub_menu = 0; 00498 else sub_menu ++; 00499 break; 00500 case 5: //Demo Menu 00501 if(sub_menu == 4) sub_menu = 0; 00502 else sub_menu ++; 00503 break; 00504 } 00505 break; 00506 } 00507 break; 00508 case 16: //Select pressed 00509 switch(level) { 00510 case 0: 00511 if(top_menu < 6) { 00512 level++; 00513 sub_menu = 0; 00514 } else { 00515 demo_on = 0; 00516 user_code_running = user_code_restore_mode; 00517 } 00518 break; 00519 case 1: 00520 switch(top_menu) { 00521 case 0: //LED Menu 00522 if(sub_menu == 9) { 00523 for(int i=0; i<9; i++) { 00524 led_state[i]=all_led_state; 00525 } 00526 demo_update_leds(); 00527 } 00528 if(sub_menu == 13) level = 0; 00529 break; 00530 case 1: // Sensors Menu 00531 if(sub_menu == 11) level = 0; 00532 break; 00533 case 2: //Motor Menu 00534 if(sub_menu == 2) { 00535 switch(both_motor_mode) { 00536 case 0: 00537 stop(); 00538 break; 00539 case 1: 00540 brake(); 00541 break; 00542 case 2: 00543 forward(0.5); 00544 break; 00545 case 3: 00546 forward(1); 00547 break; 00548 case 4: 00549 backward(0.5); 00550 break; 00551 case 5: 00552 backward(1.0); 00553 break; 00554 } 00555 } 00556 if(sub_menu == 3) { 00557 level = 0; 00558 } 00559 break; 00560 case 3: // Radio Menu 00561 if(sub_menu == 0) level = 0; 00562 break; 00563 case 4: // Info Menu 00564 if(sub_menu == 6) level = 0; 00565 break; 00566 case 5: // Demo Menu 00567 if(sub_menu == 4) level = 0; 00568 break; 00569 } 00570 break; 00571 } 00572 break; 00573 } 00574 } else started = 1; 00575 display.clear_display(); 00576 switch(level) { 00577 case 0: 00578 //Top level menu 00579 switch(top_menu) { 00580 case 0: 00581 strcpy(topline,"---TEST LEDS----"); 00582 break; 00583 case 1: 00584 strcpy(topline,"--TEST SENSORS--"); 00585 break; 00586 case 2: 00587 strcpy(topline,"--TEST MOTORS---"); 00588 break; 00589 case 3: 00590 strcpy(topline,"---TEST RADIO---"); 00591 break; 00592 case 4: 00593 strcpy(topline,"------INFO------"); 00594 break; 00595 case 5: 00596 strcpy(topline,"---CODE DEMOS---"); 00597 break; 00598 case 6: 00599 strcpy(topline,"------EXIT------"); 00600 break; 00601 } 00602 strcpy(bottomline,""); 00603 break; 00604 case 1: 00605 //Sub level menu 00606 switch(top_menu) { 00607 case 0: 00608 strcpy(topline,"----LED MENU----"); 00609 char led_status[7]; 00610 if(sub_menu<9) { 00611 switch(led_state[sub_menu]) { 00612 case 0: 00613 strcpy(led_status,"OFF"); 00614 break; 00615 case 1: 00616 strcpy(led_status,"RED"); 00617 break; 00618 case 2: 00619 strcpy(led_status,"GREEN"); 00620 break; 00621 case 3: 00622 strcpy(led_status,"YELLOW"); 00623 break; 00624 } 00625 } 00626 if(sub_menu < 8) sprintf(bottomline,"OUTER %d: %s",(sub_menu + 1),led_status); 00627 if(sub_menu == 8) sprintf(bottomline,"CENTER: %s",led_status); 00628 if(sub_menu == 9) { 00629 switch(all_led_state) { 00630 case 0: 00631 strcpy(led_status,"OFF"); 00632 break; 00633 case 1: 00634 strcpy(led_status,"RED"); 00635 break; 00636 case 2: 00637 strcpy(led_status,"GREEN"); 00638 break; 00639 case 3: 00640 strcpy(led_status,"YELLOW"); 00641 break; 00642 } 00643 sprintf(bottomline,"SET ALL %s", led_status); 00644 } 00645 if(sub_menu == 10) { 00646 if(base_led_state == 0) strcpy(led_status,"OFF"); 00647 else strcpy(led_status,"ON"); 00648 sprintf(bottomline,"BASE: %s",led_status); 00649 } 00650 if(sub_menu == 11) sprintf(bottomline,"CLED BNESS %d%%", brightness); 00651 if(sub_menu == 12) sprintf(bottomline,"DISP BNESS %d%%", bl_brightness); 00652 if(sub_menu == 13) strcpy(bottomline,"EXIT"); 00653 break; 00654 00655 case 1: 00656 strcpy(topline,"--SENSORS MENU--"); 00657 switch(sub_menu) { 00658 case 0: { 00659 float battery = get_battery_voltage (); 00660 sprintf(bottomline,"BATTERY: %1.3fV",battery); 00661 break; 00662 } 00663 case 1: { 00664 float dc = get_dc_voltage (); 00665 sprintf(bottomline,"DC: %1.3fV",dc); 00666 break; 00667 } 00668 case 2: { 00669 float current = get_current (); 00670 sprintf(bottomline,"CURRENT: %1.3fA",current); 00671 break; 00672 } 00673 case 3: { 00674 float temperature = get_temperature(); 00675 sprintf(bottomline,"TEMP: %3.2fC", temperature); 00676 break; 00677 } 00678 case 4: 00679 store_background_base_ir_values(); 00680 sprintf(bottomline,"BIR%dB:%d",base_ir_index+1,get_background_base_ir_value(base_ir_index)); 00681 break; 00682 case 5: 00683 store_illuminated_base_ir_values(); 00684 sprintf(bottomline,"BIR%dR:%d",base_ir_index+1,get_illuminated_base_ir_value(base_ir_index)); 00685 break; 00686 case 6: 00687 store_background_raw_ir_values(); 00688 sprintf(bottomline,"SIR%dB:%d",side_ir_index+1,get_background_raw_ir_value(side_ir_index)); 00689 break; 00690 case 7: 00691 store_illuminated_raw_ir_values(); 00692 sprintf(bottomline,"SIR%dR:%d",side_ir_index+1,get_illuminated_raw_ir_value(side_ir_index)); 00693 break; 00694 case 8: 00695 sprintf(bottomline,"SIR%dD:%3.1f",side_ir_index+1,0.0); 00696 break; 00697 case 9: 00698 if(ultrasonic_distance_updated == 1) { 00699 sprintf(bottomline,"USONIC:%3dcm",ultrasonic_distance); 00700 } else sprintf(bottomline,"USONIC:---------"); 00701 update_ultrasonic_measure(); 00702 break; 00703 case 10: 00704 store_line_position(); 00705 if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position); 00706 else sprintf(bottomline,"LINE:---------"); 00707 break; 00708 case 11: 00709 sprintf(bottomline,"EXIT"); 00710 break; 00711 } 00712 break; 00713 case 2: 00714 strcpy(topline,"--MOTORS MENU---"); 00715 switch(sub_menu) { 00716 case 0: 00717 sprintf(bottomline,"LEFT: %d%%", left_speed); 00718 break; 00719 case 1: 00720 sprintf(bottomline,"RIGHT: %d%%", right_speed); 00721 break; 00722 case 2: 00723 char both_mode_string[16]; 00724 switch(both_motor_mode) { 00725 case 0: 00726 strcpy(both_mode_string,"OFF"); 00727 break; 00728 case 1: 00729 strcpy(both_mode_string,"BRAKE"); 00730 break; 00731 case 2: 00732 strcpy(both_mode_string,"+50%"); 00733 break; 00734 case 3: 00735 strcpy(both_mode_string,"+100%"); 00736 break; 00737 case 4: 00738 strcpy(both_mode_string,"-50%"); 00739 break; 00740 case 5: 00741 strcpy(both_mode_string,"-100%"); 00742 break; 00743 } 00744 sprintf(bottomline,"BOTH TO %s",both_mode_string); 00745 break; 00746 case 3: 00747 sprintf(bottomline,"EXIT"); 00748 break; 00749 } 00750 break; 00751 case 3: 00752 strcpy(topline,"---RADIO MENU---"); 00753 switch(sub_menu) { 00754 00755 case 0: 00756 sprintf(bottomline,"EXIT"); 00757 break; 00758 } 00759 break; 00760 case 4: 00761 strcpy(topline,"---INFO MENU----"); 00762 switch(sub_menu) { 00763 case 0: 00764 sprintf(bottomline,"ROBOT ID: %d",robot_id); 00765 break; 00766 case 1: 00767 sprintf(bottomline,"SOFTWARE: %1.2f",SOFTWARE_VERSION_CODE); 00768 break; 00769 case 2: 00770 if(firmware_version > 0) sprintf(bottomline,"FIRMWARE: %1.2f",firmware_version); 00771 else sprintf(bottomline,"FIRMWARE: ?????"); 00772 break; 00773 case 3: 00774 sprintf(bottomline,"PROG:%s",program_name); 00775 break; 00776 case 4: 00777 sprintf(bottomline,"AUTH:%s",author_name); 00778 break; 00779 case 5: 00780 sprintf(bottomline,"VER:%s",version_name); 00781 break; 00782 case 6: 00783 sprintf(bottomline,"EXIT"); 00784 break; 00785 } 00786 break; 00787 case 5: 00788 strcpy(topline,"---CODE DEMOS---"); 00789 switch(sub_menu) { 00790 case 0: 00791 sprintf(bottomline,"LINE FOLLOW"); 00792 break; 00793 case 1: 00794 sprintf(bottomline,"OBST. AVOID"); 00795 break; 00796 case 2: 00797 sprintf(bottomline,"COLOUR SPIN"); 00798 break; 00799 case 3: 00800 sprintf(bottomline,"STRESS TEST"); 00801 break; 00802 case 4: 00803 sprintf(bottomline,"EXIT"); 00804 break; 00805 } 00806 break; 00807 case 6: 00808 strcpy(topline,""); 00809 break; 00810 } 00811 break; 00812 } 00813 display.write_string(topline); 00814 display.set_position(1,0); 00815 display.write_string(bottomline); 00816 if(top_menu == 1 && level == 1) { 00817 demo_event.attach(&demo_event_thread, 0.25); 00818 } 00819 } 00820 00821 void start_line_demo() 00822 { 00823 display.set_backlight_brightness(0); 00824 time_out = 0.25f; 00825 demo_timer.start(); 00826 state = 0; 00827 speed = 0; 00828 led_step = 0; 00829 demo_running = 1; 00830 demo_timeout.attach_us(&line_demo_cycle,1000); 00831 } 00832 00833 void start_obstacle_demo() 00834 { 00835 display.set_backlight_brightness(0); 00836 time_out = 0.25f; 00837 demo_timer.start(); 00838 state = 0; 00839 speed = 0; 00840 led_step = 0; 00841 demo_running = 1; 00842 demo_timeout.attach_us(&obstacle_demo_cycle,1000); 00843 } 00844 00845 void start_stress_demo() 00846 { 00847 display.set_backlight_brightness(0.25); 00848 display.write_string("STRESS TEST"); 00849 display.set_position(1,0); 00850 display.write_string("----25%----"); 00851 time_out = 0.04f; 00852 demo_timer.start(); 00853 state = 0; 00854 speed = 0; 00855 stress_step = 0; 00856 spin_step = 0; 00857 demo_running = 1; 00858 demo_timeout.attach_us(&stress_demo_cycle,1000); 00859 } 00860 00861 void start_spinning_demo() 00862 { 00863 display.set_backlight_brightness(0); 00864 time_out = 0.0f; 00865 demo_timer.start(); 00866 state = 0; 00867 speed = 0; 00868 led_step = 0; 00869 spin_step = 0; 00870 demo_running = 1; 00871 demo_timeout.attach_us(&spinning_demo_cycle,1000); 00872 } 00873 00874 void line_demo_cycle() 00875 { 00876 if(demo_timer.read() > time_out) { 00877 store_line_position(); 00878 if(line_found) { 00879 time_out = 0.01f; 00880 state = 0; 00881 // Get the position of the line. 00882 lf_current_pos_of_line = line_position; 00883 lf_proportional = lf_current_pos_of_line; 00884 00885 // Compute the derivative 00886 lf_derivative = lf_current_pos_of_line - lf_previous_pos_of_line; 00887 00888 // Compute the integral 00889 lf_integral += lf_proportional; 00890 00891 // Remember the last position. 00892 lf_previous_pos_of_line = lf_current_pos_of_line; 00893 00894 // Compute the power 00895 lf_power = (lf_proportional * (LF_P_TERM) ) + (lf_integral*(LF_I_TERM)) + (lf_derivative*(LF_D_TERM)) ; 00896 00897 // Compute new speeds 00898 lf_right = lf_speed-lf_power; 00899 lf_left = lf_speed+lf_power; 00900 00901 // limit checks 00902 if (lf_right < 0) 00903 lf_right = 0; 00904 else if (lf_right > 1.0f) 00905 lf_right = 1.0f; 00906 00907 if (lf_left < 0) 00908 lf_left = 0; 00909 else if (lf_left > 1.0f) 00910 lf_left = 1.0f; 00911 }else{ 00912 //Cannot see line: hunt for it 00913 if(lf_left > lf_right){ 00914 //Currently turning left, keep turning left 00915 state ++; 00916 float d_step = state * 0.04; 00917 lf_left = 0.2 + d_step; 00918 lf_right = -0.2 - d_step; 00919 if(state > 20){ 00920 state = 0; 00921 lf_right = 0.2; 00922 lf_left = -0.2; 00923 time_out += 0.01f; 00924 if(time_out > 0.1f) demo_running = 0; 00925 } 00926 }else{ 00927 //Currently turning right, keep turning right 00928 state ++; 00929 float d_step = state * 0.04; 00930 lf_left = -0.2 - d_step; 00931 lf_right = 0.2 + d_step; 00932 if(state > 20){ 00933 state = 0; 00934 lf_right = -0.2; 00935 lf_left = 0.2; 00936 time_out += 0.01f; 00937 if(time_out > 0.1f) demo_running = 0; 00938 } 00939 } 00940 } 00941 // set speed 00942 set_left_motor_speed(lf_left); 00943 set_right_motor_speed(lf_right); 00944 00945 00946 demo_timer.reset(); 00947 } 00948 if(demo_running == 1)demo_timeout.attach_us(&line_demo_cycle,500); 00949 else { 00950 stop(); 00951 display.set_backlight_brightness(bl_brightness * 0.01f); 00952 } 00953 } 00954 00955 void stress_demo_cycle() 00956 { 00957 if(demo_timer.read() > time_out) { 00958 float pct = 0.25 + (0.25 * stress_step); 00959 switch(state) { 00960 case 0: 00961 if(spin_step % 2 == 0) { 00962 forward(pct); 00963 set_leds(0xFF,0xFF); 00964 } else { 00965 backward(pct); 00966 set_leds(0,0xFF); 00967 } 00968 spin_step ++; 00969 if(spin_step > 199) { 00970 state ++; 00971 spin_step = 0; 00972 } 00973 break; 00974 case 1: 00975 if(stress_step < 3) stress_step ++; 00976 float pct = 0.25 + (0.25 * stress_step); 00977 display.set_backlight_brightness(pct); 00978 display.set_position(1,0); 00979 switch(stress_step) { 00980 case 1: 00981 display.write_string("----50%----"); 00982 break; 00983 case 2: 00984 display.write_string("----75%----"); 00985 break; 00986 case 3: 00987 display.write_string("---100%----"); 00988 break; 00989 } 00990 state = 0; 00991 break; 00992 } 00993 demo_timer.reset(); 00994 } 00995 if(demo_running == 1)demo_timeout.attach_us(&stress_demo_cycle,500); 00996 else { 00997 stop(); 00998 display.set_backlight_brightness(bl_brightness * 0.01f); 00999 } 01000 } 01001 01002 void spinning_demo_cycle() 01003 { 01004 if(demo_timer.read() > time_out) { 01005 switch(state) { 01006 case 0: //Robot is stopped 01007 set_leds(0,0xFF); 01008 set_center_led(1,1); 01009 speed = 0.1f; 01010 brake(); 01011 time_out = 0.5; 01012 state = 1; 01013 led_step = 0; 01014 break; 01015 case 1: //Motor is turning right, accelerating 01016 time_out = 0.1; 01017 set_center_led(2,1); 01018 switch(led_step) { 01019 case 0: 01020 set_leds(0x01,0); 01021 break; 01022 case 1: 01023 set_leds(0x02,0); 01024 break; 01025 case 2: 01026 set_leds(0x04,0); 01027 break; 01028 case 3: 01029 set_leds(0x08,0); 01030 break; 01031 case 4: 01032 set_leds(0x10,0); 01033 break; 01034 case 5: 01035 set_leds(0x20,0); 01036 break; 01037 case 6: 01038 set_leds(0x40,0); 01039 break; 01040 case 7: 01041 set_leds(0x80,0); 01042 break; 01043 } 01044 led_step ++; 01045 if(led_step == 8) led_step =0; 01046 if(speed < 1) { 01047 speed += 0.0125; 01048 turn(speed); 01049 } else { 01050 state = 2; 01051 spin_step = 0; 01052 led_step =0; 01053 } 01054 break; 01055 case 2: //Motor is turning right, full speed 01056 set_center_led(3,1); 01057 switch(led_step) { 01058 case 0: 01059 set_leds(0x33,0x33); 01060 break; 01061 case 1: 01062 set_leds(0x66,0x66); 01063 break; 01064 case 2: 01065 set_leds(0xCC,0xCC); 01066 break; 01067 case 3: 01068 set_leds(0x99,0x99); 01069 break; 01070 } 01071 led_step ++; 01072 if(led_step == 4) led_step = 0; 01073 spin_step ++; 01074 if(spin_step == 40) { 01075 state = 3; 01076 led_step = 0; 01077 } 01078 break; 01079 case 3: //Motor is turning right, decelerating 01080 set_center_led(2,1); 01081 switch(led_step) { 01082 case 0: 01083 set_leds(0x01,0); 01084 break; 01085 case 1: 01086 set_leds(0x02,0); 01087 break; 01088 case 2: 01089 set_leds(0x04,0); 01090 break; 01091 case 3: 01092 set_leds(0x08,0); 01093 break; 01094 case 4: 01095 set_leds(0x10,0); 01096 break; 01097 case 5: 01098 set_leds(0x20,0); 01099 break; 01100 case 6: 01101 set_leds(0x40,0); 01102 break; 01103 case 7: 01104 set_leds(0x80,0); 01105 break; 01106 } 01107 if(led_step == 0) led_step =8; 01108 led_step --; 01109 if(speed > 0.1) { 01110 speed -= 0.025; 01111 turn(speed); 01112 } else { 01113 state = 4; 01114 spin_step = 0; 01115 led_step =0; 01116 } 01117 break; 01118 case 4: //Robot is stopped 01119 set_leds(0,0xFF); 01120 set_center_led(1,1); 01121 speed = 0.1f; 01122 brake(); 01123 time_out = 0.5; 01124 led_step =0; 01125 state = 5; 01126 break; 01127 case 5: //Motor is turning left, accelerating 01128 time_out = 0.1; 01129 set_center_led(2,1); 01130 switch(led_step) { 01131 case 0: 01132 set_leds(0x01,0); 01133 break; 01134 case 1: 01135 set_leds(0x02,0); 01136 break; 01137 case 2: 01138 set_leds(0x04,0); 01139 break; 01140 case 3: 01141 set_leds(0x08,0); 01142 break; 01143 case 4: 01144 set_leds(0x10,0); 01145 break; 01146 case 5: 01147 set_leds(0x20,0); 01148 break; 01149 case 6: 01150 set_leds(0x40,0); 01151 break; 01152 case 7: 01153 set_leds(0x80,0); 01154 break; 01155 } 01156 if(led_step == 0) led_step =8; 01157 led_step --; 01158 if(speed < 1) { 01159 speed += 0.0125; 01160 turn(-speed); 01161 } else { 01162 state = 6; 01163 spin_step = 0; 01164 led_step = 0; 01165 } 01166 break; 01167 case 6: //Motor is turning left, full speed 01168 set_center_led(3,1); 01169 switch(led_step) { 01170 case 0: 01171 set_leds(0x33,0x33); 01172 break; 01173 case 1: 01174 set_leds(0x66,0x66); 01175 break; 01176 case 2: 01177 set_leds(0xCC,0xCC); 01178 break; 01179 case 3: 01180 set_leds(0x99,0x99); 01181 break; 01182 } 01183 if(led_step == 0) led_step = 4; 01184 led_step --; 01185 spin_step ++; 01186 if(spin_step == 40) { 01187 state = 7; 01188 led_step = 0; 01189 } 01190 break; 01191 case 7: //Motor is turning left, decelerating 01192 set_center_led(2,1); 01193 switch(led_step) { 01194 case 0: 01195 set_leds(0x01,0); 01196 break; 01197 case 1: 01198 set_leds(0x02,0); 01199 break; 01200 case 2: 01201 set_leds(0x04,0); 01202 break; 01203 case 3: 01204 set_leds(0x08,0); 01205 break; 01206 case 4: 01207 set_leds(0x10,0); 01208 break; 01209 case 5: 01210 set_leds(0x20,0); 01211 break; 01212 case 6: 01213 set_leds(0x40,0); 01214 break; 01215 case 7: 01216 set_leds(0x80,0); 01217 break; 01218 } 01219 led_step ++; 01220 if(led_step == 8) led_step =0; 01221 if(speed > 0.1) { 01222 speed -= 0.025; 01223 turn(-speed); 01224 } else { 01225 state = 0; 01226 spin_step = 0; 01227 led_step = 0; 01228 } 01229 break; 01230 } 01231 demo_timer.reset(); 01232 } 01233 if(demo_running == 1)demo_timeout.attach_us(&spinning_demo_cycle,500); 01234 else { 01235 stop(); 01236 display.set_backlight_brightness(bl_brightness * 0.01f); 01237 } 01238 } 01239 01240 void obstacle_demo_cycle() 01241 { 01242 if(demo_timer.read() > time_out) { 01243 switch(state) { 01244 case 0: //Robot is stopped 01245 set_leds(0,0xFF); 01246 set_center_led(1,0.4); 01247 speed = 0.3f; 01248 forward(speed); 01249 time_out = 0.05; 01250 state = 1; 01251 break; 01252 case 1: { //Motor is moving forward 01253 store_ir_values(); 01254 int front_right = read_illuminated_raw_ir_value(0); 01255 int front_left = read_illuminated_raw_ir_value(7); 01256 if(front_left > 400 || front_right > 400) { 01257 brake(); 01258 time_out = 0.04; 01259 if(front_left > front_right)state=2; 01260 else state=3; 01261 } else { 01262 if(speed < 0.5) { 01263 speed += 0.03; 01264 forward(speed); 01265 } 01266 switch(led_step) { 01267 case 0: 01268 set_leds(0x01,0); 01269 break; 01270 case 1: 01271 set_leds(0x38,0); 01272 break; 01273 case 2: 01274 set_leds(0x6C,0); 01275 break; 01276 case 3: 01277 set_leds(0xC6,0); 01278 break; 01279 case 4: 01280 set_leds(0x83,0); 01281 break; 01282 } 01283 set_center_led(2, 0.6); 01284 led_step ++; 01285 if(led_step == 5) led_step = 0; 01286 } 01287 break; 01288 } 01289 case 2: //Turn right 01290 set_left_motor_speed(0.85); 01291 set_right_motor_speed(-0.85); 01292 time_out = 0.4; 01293 state = 0; 01294 set_leds(0x0E,0x0E); 01295 set_center_led(3,0.5); 01296 break; 01297 case 3: //Turn left 01298 set_left_motor_speed(-0.85); 01299 set_right_motor_speed(0.85); 01300 time_out = 0.4; 01301 state = 0; 01302 set_leds(0xE0,0xE0); 01303 set_center_led(3,0.5); 01304 break; 01305 } 01306 demo_timer.reset(); 01307 } 01308 if(demo_running == 1)demo_timeout.attach_us(&obstacle_demo_cycle,200); 01309 else { 01310 stop(); 01311 display.set_backlight_brightness(bl_brightness * 0.01f); 01312 } 01313 } 01314 01315 void demo_update_leds() 01316 { 01317 char red = 0; 01318 char green = 0; 01319 for(int i=0; i<8; i++) { 01320 if(led_state[i]==1 || led_state[i]==3) red+=(1<<i); 01321 if(led_state[i]==2 || led_state[i]==3) green+=(1<<i); 01322 } 01323 set_leds(green,red); 01324 float brightness_f = brightness / 100.0f; 01325 set_center_led(led_state[8], brightness_f); 01326 set_base_led(base_led_state); 01327 }
Generated on Thu Jul 14 2022 08:02:39 by
1.7.2