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