Alan Millard
/
BeautifulMemeProjectBT
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.
Fork of
BeautifulMemeProject
by 
James Hilder
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 Fri Jul 15 2022 08:26:10 by
1.7.2