Psi Swarm Robot / PsiSwarmV8_CPP

C++ Library for the PsiSwarm Robot - Version 0.8

Dependents:   PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk

Fork of PsiSwarmV7_CPP by Psi Swarm Robot

demo.cpp@15:be92991621b4, 2017-07-14 (annotated)

Committer:
JessicaGao
Date:
Fri Jul 14 12:34:22 2017 +0000
Revision:
15:be92991621b4
Parent:
12:878c6e9d9e60 
detect beacon;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:d6269d17c8cf 1 /* University of York Robotics Laboratory PsiSwarm Library: Demo Mode Source File
jah128 6:b340a527add9 2 *
jah128 6:b340a527add9 3 * Copyright 2016 University of York
jah128 6:b340a527add9 4 *
jah128 6:b340a527add9 5 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
jah128 6:b340a527add9 6 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
jah128 6:b340a527add9 7 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
jah128 6:b340a527add9 8 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jah128 6:b340a527add9 9 * See the License for the specific language governing permissions and limitations under the License.
jah128 0:d6269d17c8cf 10 *
jah128 0:d6269d17c8cf 11 * File: demo.cpp
jah128 0:d6269d17c8cf 12 *
jah128 0:d6269d17c8cf 13 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:d6269d17c8cf 14 * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
jah128 0:d6269d17c8cf 15 *
jah128 8:6c92789d5f87 16 * PsiSwarm Library Version: 0.8
jah128 0:d6269d17c8cf 17 *
jah128 8:6c92789d5f87 18 * Version 0.8 : Rewritten as OO\C++
jah128 5:3cdd1a37cdd7 19 * Version 0.7 : Updated for new MBED API
jah128 4:1c621cb8cf0d 20 * Version 0.5 : Added motor calibration menu
jah128 0:d6269d17c8cf 21 * Version 0.4 : Added PsiSwarmBasic menu
jah128 0:d6269d17c8cf 22 * Version 0.2 : Remove most the functionality from center-button push to allow all operations to be accessable from
jah128 0:d6269d17c8cf 23 * four directions alone.
jah128 0:d6269d17c8cf 24 * Added extra sensor information, added various testing demos
jah128 0:d6269d17c8cf 25 *
jah128 2:c6986ee3c7c5 26 *
jah128 5:3cdd1a37cdd7 27 * October 2016
jah128 0:d6269d17c8cf 28 *
jah128 0:d6269d17c8cf 29 */
jah128 0:d6269d17c8cf 30
jah128 0:d6269d17c8cf 31
jah128 0:d6269d17c8cf 32 #include "psiswarm.h"
jah128 0:d6269d17c8cf 33
jah128 0:d6269d17c8cf 34 // PID terms
jah128 0:d6269d17c8cf 35 #define LF_P_TERM 0.2
jah128 0:d6269d17c8cf 36 #define LF_I_TERM 0
jah128 0:d6269d17c8cf 37 #define LF_D_TERM 4
jah128 0:d6269d17c8cf 38
jah128 0:d6269d17c8cf 39 char top_menu = 0;
jah128 0:d6269d17c8cf 40 char sub_menu = 0;
jah128 0:d6269d17c8cf 41 char level = 0;
jah128 0:d6269d17c8cf 42 char started = 0;
jah128 0:d6269d17c8cf 43 char topline[17];
jah128 0:d6269d17c8cf 44 char bottomline[17];
jah128 0:d6269d17c8cf 45 char led_state[9];
jah128 0:d6269d17c8cf 46 char all_led_state = 0;
jah128 0:d6269d17c8cf 47 char base_led_state = 0;
jah128 0:d6269d17c8cf 48 char brightness = 20;
jah128 0:d6269d17c8cf 49 char bl_brightness = 100;
jah128 0:d6269d17c8cf 50 char base_ir_index = 0;
jah128 0:d6269d17c8cf 51 char side_ir_index = 0;
jah128 0:d6269d17c8cf 52 signed short left_speed = 0;
jah128 0:d6269d17c8cf 53 signed short right_speed = 0;
jah128 0:d6269d17c8cf 54 char both_motor_mode = 0;
jah128 0:d6269d17c8cf 55 char last_switch_pressed;
jah128 0:d6269d17c8cf 56 Timeout demo_event;
jah128 0:d6269d17c8cf 57 char handling_event = 0;
jah128 0:d6269d17c8cf 58
jah128 0:d6269d17c8cf 59 Timeout demo_timeout;
jah128 0:d6269d17c8cf 60 char demo_running = 0;
jah128 0:d6269d17c8cf 61 Timer demo_timer;
jah128 0:d6269d17c8cf 62 float time_out;
jah128 0:d6269d17c8cf 63 float speed;
jah128 0:d6269d17c8cf 64 char state;
jah128 0:d6269d17c8cf 65 char led_step = 0;
jah128 0:d6269d17c8cf 66 char spin_step = 0;
jah128 0:d6269d17c8cf 67 char stress_step = 0;
jah128 0:d6269d17c8cf 68
jah128 0:d6269d17c8cf 69
jah128 0:d6269d17c8cf 70 float lf_right;
jah128 0:d6269d17c8cf 71 float lf_left;
jah128 0:d6269d17c8cf 72 float lf_current_pos_of_line = 0.0;
jah128 0:d6269d17c8cf 73 float lf_previous_pos_of_line = 0.0;
jah128 0:d6269d17c8cf 74 float lf_derivative,lf_proportional,lf_integral = 0;
jah128 0:d6269d17c8cf 75 float lf_power;
jah128 0:d6269d17c8cf 76 float lf_speed = 0.4;
jah128 0:d6269d17c8cf 77
jah128 0:d6269d17c8cf 78
jah128 0:d6269d17c8cf 79
jah128 11:312663037b8c 80 void Demo::start_demo_mode()
jah128 0:d6269d17c8cf 81 {
jah128 12:878c6e9d9e60 82 psi.debug("- Starting Demo Mode\n");
jah128 0:d6269d17c8cf 83 if(use_flash_basic == 1) top_menu = 7;
jah128 0:d6269d17c8cf 84 demo_on = 1;
jah128 0:d6269d17c8cf 85 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 0:d6269d17c8cf 86 display.clear_display();
jah128 0:d6269d17c8cf 87 display.write_string("PSI SWARM SYSTEM");
jah128 0:d6269d17c8cf 88 display.set_position(1,0);
jah128 0:d6269d17c8cf 89 display.write_string(" DEMO MODE");
jah128 0:d6269d17c8cf 90 wait(0.5);
jah128 0:d6269d17c8cf 91 display.clear_display();
jah128 0:d6269d17c8cf 92 display.write_string("Use cursor to");
jah128 0:d6269d17c8cf 93 display.set_position(1,0);
jah128 0:d6269d17c8cf 94 display.write_string("navigate menus");
jah128 0:d6269d17c8cf 95 char step = 0;
jah128 0:d6269d17c8cf 96 while(demo_on) {
jah128 0:d6269d17c8cf 97 if(demo_running == 0) {
jah128 0:d6269d17c8cf 98 switch(step) {
jah128 0:d6269d17c8cf 99 case 0:
jah128 0:d6269d17c8cf 100 mbed_led1 = 1;
jah128 0:d6269d17c8cf 101 mbed_led2 = 0;
jah128 0:d6269d17c8cf 102 break;
jah128 0:d6269d17c8cf 103 case 1:
jah128 0:d6269d17c8cf 104 mbed_led2 = 1;
jah128 0:d6269d17c8cf 105 mbed_led1 = 0;
jah128 0:d6269d17c8cf 106 break;
jah128 0:d6269d17c8cf 107 }
jah128 0:d6269d17c8cf 108 step++;
jah128 0:d6269d17c8cf 109 if(step==2)step=0;
jah128 0:d6269d17c8cf 110 } else {
jah128 0:d6269d17c8cf 111 mbed_led1 = 0;
jah128 0:d6269d17c8cf 112 mbed_led2 = 0;
jah128 0:d6269d17c8cf 113 mbed_led3 = 0;
jah128 0:d6269d17c8cf 114 mbed_led4 = 0;
jah128 0:d6269d17c8cf 115 }
jah128 0:d6269d17c8cf 116 wait(0.5);
jah128 0:d6269d17c8cf 117 }
jah128 12:878c6e9d9e60 118 psi.debug("- Demo mode ended\n");
jah128 0:d6269d17c8cf 119 }
jah128 0:d6269d17c8cf 120
jah128 11:312663037b8c 121 void Demo::demo_handle_switch_event(char switch_pressed)
jah128 0:d6269d17c8cf 122 {
jah128 0:d6269d17c8cf 123 if(!handling_event) {
jah128 0:d6269d17c8cf 124 handling_event = 1;
jah128 0:d6269d17c8cf 125 last_switch_pressed = switch_pressed;
jah128 11:312663037b8c 126 demo_event.attach_us(this,&Demo::demo_event_thread, 1000);
jah128 0:d6269d17c8cf 127 }
jah128 0:d6269d17c8cf 128 }
jah128 0:d6269d17c8cf 129
jah128 11:312663037b8c 130 void Demo::demo_event_thread()
jah128 0:d6269d17c8cf 131 {
jah128 0:d6269d17c8cf 132 handling_event = 0;
jah128 0:d6269d17c8cf 133 if(started == 1) {
jah128 0:d6269d17c8cf 134 switch(last_switch_pressed) {
jah128 0:d6269d17c8cf 135 case 1: //Up pressed
jah128 0:d6269d17c8cf 136 switch(level) {
jah128 0:d6269d17c8cf 137 case 0:
jah128 4:1c621cb8cf0d 138 if(top_menu != 8) {
jah128 0:d6269d17c8cf 139 level++;
jah128 0:d6269d17c8cf 140 sub_menu = 0;
jah128 0:d6269d17c8cf 141 } else {
jah128 0:d6269d17c8cf 142 demo_on = 0;
jah128 0:d6269d17c8cf 143 user_code_running = user_code_restore_mode;
jah128 0:d6269d17c8cf 144 }
jah128 0:d6269d17c8cf 145 break;
jah128 0:d6269d17c8cf 146 case 1:
jah128 0:d6269d17c8cf 147 switch(top_menu) {
jah128 0:d6269d17c8cf 148 case 7: // PsiBasic Menu
jah128 0:d6269d17c8cf 149 if(sub_menu == psi_basic_file_count) level = 0;
jah128 0:d6269d17c8cf 150 break;
jah128 0:d6269d17c8cf 151 case 0: //LED Menu
jah128 0:d6269d17c8cf 152 if(sub_menu < 9) {
jah128 0:d6269d17c8cf 153 if(led_state[sub_menu] == 0) led_state[sub_menu] = 3;
jah128 0:d6269d17c8cf 154 else led_state[sub_menu]--;
jah128 0:d6269d17c8cf 155 demo_update_leds();
jah128 0:d6269d17c8cf 156 }
jah128 0:d6269d17c8cf 157 if(sub_menu == 9) {
jah128 0:d6269d17c8cf 158 if(all_led_state == 0) all_led_state = 3;
jah128 0:d6269d17c8cf 159 else all_led_state--;
jah128 0:d6269d17c8cf 160 for(int i=0; i<9; i++) {
jah128 0:d6269d17c8cf 161 led_state[i]=all_led_state;
jah128 0:d6269d17c8cf 162 }
jah128 0:d6269d17c8cf 163 demo_update_leds();
jah128 0:d6269d17c8cf 164 }
jah128 0:d6269d17c8cf 165 if(sub_menu == 10) {
jah128 0:d6269d17c8cf 166 base_led_state = 1 - base_led_state;
jah128 0:d6269d17c8cf 167 demo_update_leds();
jah128 0:d6269d17c8cf 168 }
jah128 0:d6269d17c8cf 169 if(sub_menu == 11) {
jah128 0:d6269d17c8cf 170 switch(brightness) {
jah128 0:d6269d17c8cf 171 case 100:
jah128 0:d6269d17c8cf 172 brightness = 50;
jah128 0:d6269d17c8cf 173 break;
jah128 0:d6269d17c8cf 174 case 2:
jah128 0:d6269d17c8cf 175 brightness = 1;
jah128 0:d6269d17c8cf 176 break;
jah128 0:d6269d17c8cf 177 case 5:
jah128 0:d6269d17c8cf 178 brightness = 2;
jah128 0:d6269d17c8cf 179 break;
jah128 0:d6269d17c8cf 180 case 10:
jah128 0:d6269d17c8cf 181 brightness = 5;
jah128 0:d6269d17c8cf 182 break;
jah128 0:d6269d17c8cf 183 case 20:
jah128 0:d6269d17c8cf 184 brightness = 10;
jah128 0:d6269d17c8cf 185 break;
jah128 0:d6269d17c8cf 186 case 50:
jah128 0:d6269d17c8cf 187 brightness = 20;
jah128 0:d6269d17c8cf 188 break;
jah128 0:d6269d17c8cf 189 }
jah128 0:d6269d17c8cf 190 demo_update_leds();
jah128 0:d6269d17c8cf 191 }
jah128 0:d6269d17c8cf 192 if(sub_menu == 12) {
jah128 0:d6269d17c8cf 193 if(bl_brightness > 0) bl_brightness-=10;
jah128 0:d6269d17c8cf 194 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 0:d6269d17c8cf 195 }
jah128 0:d6269d17c8cf 196 if(sub_menu == 13) level = 0;
jah128 0:d6269d17c8cf 197 break;
jah128 0:d6269d17c8cf 198 case 1: // Sensors Menu
jah128 0:d6269d17c8cf 199 if(sub_menu == 4 || sub_menu == 5) {
jah128 0:d6269d17c8cf 200 if(base_ir_index == 0) base_ir_index = 4;
jah128 0:d6269d17c8cf 201 else base_ir_index --;
jah128 0:d6269d17c8cf 202 }
jah128 0:d6269d17c8cf 203 if(sub_menu > 5 && sub_menu < 9) {
jah128 0:d6269d17c8cf 204 if(side_ir_index == 0) side_ir_index = 7;
jah128 0:d6269d17c8cf 205 else side_ir_index --;
jah128 0:d6269d17c8cf 206 }
jah128 0:d6269d17c8cf 207 if(sub_menu == 11) level = 0;
jah128 0:d6269d17c8cf 208 break;
jah128 0:d6269d17c8cf 209 case 2: // Motor Menu
jah128 0:d6269d17c8cf 210 if(sub_menu == 0) {
jah128 0:d6269d17c8cf 211 left_speed += 5;
jah128 0:d6269d17c8cf 212 if(left_speed > 100) left_speed = 100;
jah128 8:6c92789d5f87 213 motors.set_left_motor_speed(left_speed / 100.0f);
jah128 0:d6269d17c8cf 214 }
jah128 0:d6269d17c8cf 215 if(sub_menu == 1) {
jah128 0:d6269d17c8cf 216 right_speed += 5;
jah128 0:d6269d17c8cf 217 if(right_speed > 100) right_speed = 100;
jah128 8:6c92789d5f87 218 motors.set_right_motor_speed(right_speed / 100.0f);
jah128 0:d6269d17c8cf 219 }
jah128 0:d6269d17c8cf 220 if(sub_menu == 2) {
jah128 0:d6269d17c8cf 221 if(both_motor_mode == 0) both_motor_mode=5;
jah128 0:d6269d17c8cf 222 else both_motor_mode--;
jah128 0:d6269d17c8cf 223 switch(both_motor_mode) {
jah128 0:d6269d17c8cf 224 case 0:
jah128 8:6c92789d5f87 225 motors.stop();
jah128 0:d6269d17c8cf 226 break;
jah128 0:d6269d17c8cf 227 case 1:
jah128 8:6c92789d5f87 228 motors.brake();
jah128 0:d6269d17c8cf 229 break;
jah128 0:d6269d17c8cf 230 case 2:
jah128 8:6c92789d5f87 231 motors.forward(0.5);
jah128 0:d6269d17c8cf 232 break;
jah128 0:d6269d17c8cf 233 case 3:
jah128 8:6c92789d5f87 234 motors.forward(1);
jah128 0:d6269d17c8cf 235 break;
jah128 0:d6269d17c8cf 236 case 4:
jah128 8:6c92789d5f87 237 motors.backward(0.5);
jah128 0:d6269d17c8cf 238 break;
jah128 0:d6269d17c8cf 239 case 5:
jah128 8:6c92789d5f87 240 motors.backward(1.0);
jah128 0:d6269d17c8cf 241 break;
jah128 0:d6269d17c8cf 242 }
jah128 0:d6269d17c8cf 243 }
jah128 0:d6269d17c8cf 244 if(sub_menu == 3) {
jah128 0:d6269d17c8cf 245 level = 0;
jah128 0:d6269d17c8cf 246 }
jah128 0:d6269d17c8cf 247 break;
jah128 0:d6269d17c8cf 248 case 3: // Radio Menu
jah128 0:d6269d17c8cf 249 if(sub_menu == 0) level = 0;
jah128 0:d6269d17c8cf 250 break;
jah128 0:d6269d17c8cf 251 case 4: // Info Menu
jah128 0:d6269d17c8cf 252 if(sub_menu == 6) level = 0;
jah128 0:d6269d17c8cf 253 break;
jah128 0:d6269d17c8cf 254 case 5: // Demo Menu
jah128 0:d6269d17c8cf 255 if(sub_menu == 0) {
jah128 0:d6269d17c8cf 256 if(demo_running == 0) {
jah128 0:d6269d17c8cf 257 start_line_demo();
jah128 0:d6269d17c8cf 258 } else {
jah128 0:d6269d17c8cf 259 demo_running = 0;
jah128 0:d6269d17c8cf 260 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 8:6c92789d5f87 261 motors.stop();
jah128 0:d6269d17c8cf 262 }
jah128 0:d6269d17c8cf 263 }
jah128 0:d6269d17c8cf 264 if(sub_menu == 1) {
jah128 0:d6269d17c8cf 265 if(demo_running == 0) {
jah128 0:d6269d17c8cf 266 start_obstacle_demo();
jah128 0:d6269d17c8cf 267 } else {
jah128 0:d6269d17c8cf 268 demo_running = 0;
jah128 0:d6269d17c8cf 269 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 8:6c92789d5f87 270 motors.stop();
jah128 0:d6269d17c8cf 271 }
jah128 0:d6269d17c8cf 272 }
jah128 0:d6269d17c8cf 273 if(sub_menu == 2) {
jah128 0:d6269d17c8cf 274 if(demo_running == 0) {
jah128 0:d6269d17c8cf 275 start_spinning_demo();
jah128 0:d6269d17c8cf 276 } else {
jah128 0:d6269d17c8cf 277 demo_running = 0;
jah128 0:d6269d17c8cf 278 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 8:6c92789d5f87 279 motors.stop();
jah128 0:d6269d17c8cf 280 }
jah128 0:d6269d17c8cf 281 }
jah128 0:d6269d17c8cf 282 if(sub_menu == 3) {
jah128 0:d6269d17c8cf 283 if(demo_running == 0) {
jah128 0:d6269d17c8cf 284 start_stress_demo();
jah128 0:d6269d17c8cf 285 } else {
jah128 0:d6269d17c8cf 286 demo_running = 0;
jah128 0:d6269d17c8cf 287 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 8:6c92789d5f87 288 motors.stop();
jah128 0:d6269d17c8cf 289 }
jah128 0:d6269d17c8cf 290 }
jah128 0:d6269d17c8cf 291 if(sub_menu == 4) level = 0;
jah128 0:d6269d17c8cf 292 break;
jah128 0:d6269d17c8cf 293 }
jah128 0:d6269d17c8cf 294 break;
jah128 0:d6269d17c8cf 295 }
jah128 0:d6269d17c8cf 296 break;
jah128 0:d6269d17c8cf 297 case 2: //Down pressed
jah128 0:d6269d17c8cf 298 switch(level) {
jah128 0:d6269d17c8cf 299 case 0:
jah128 4:1c621cb8cf0d 300 if(top_menu != 8) {
jah128 0:d6269d17c8cf 301 level++;
jah128 0:d6269d17c8cf 302 sub_menu = 0;
jah128 0:d6269d17c8cf 303 } else {
jah128 0:d6269d17c8cf 304 demo_on = 0;
jah128 0:d6269d17c8cf 305 user_code_running = user_code_restore_mode;
jah128 0:d6269d17c8cf 306 }
jah128 0:d6269d17c8cf 307 break;
jah128 0:d6269d17c8cf 308 case 1:
jah128 0:d6269d17c8cf 309 switch(top_menu) {
jah128 0:d6269d17c8cf 310 case 7: // PsiBasic Menu
jah128 0:d6269d17c8cf 311 if(sub_menu == psi_basic_file_count) level = 0;
jah128 0:d6269d17c8cf 312 break;
jah128 0:d6269d17c8cf 313 case 0: //LED Menu
jah128 0:d6269d17c8cf 314 if(sub_menu < 9) {
jah128 0:d6269d17c8cf 315 led_state[sub_menu]++;
jah128 0:d6269d17c8cf 316 if(led_state[sub_menu] == 4) led_state[sub_menu] = 0;
jah128 0:d6269d17c8cf 317 demo_update_leds();
jah128 0:d6269d17c8cf 318 }
jah128 0:d6269d17c8cf 319 if(sub_menu == 9) {
jah128 0:d6269d17c8cf 320 all_led_state++;
jah128 0:d6269d17c8cf 321 if(all_led_state == 4) all_led_state = 0;
jah128 0:d6269d17c8cf 322 for(int i=0; i<9; i++) {
jah128 0:d6269d17c8cf 323 led_state[i]=all_led_state;
jah128 0:d6269d17c8cf 324 }
jah128 0:d6269d17c8cf 325 demo_update_leds();
jah128 0:d6269d17c8cf 326 }
jah128 0:d6269d17c8cf 327 if(sub_menu == 10) {
jah128 0:d6269d17c8cf 328 base_led_state = 1 - base_led_state;
jah128 0:d6269d17c8cf 329 demo_update_leds();
jah128 0:d6269d17c8cf 330 }
jah128 0:d6269d17c8cf 331 if(sub_menu == 11) {
jah128 0:d6269d17c8cf 332 switch(brightness) {
jah128 0:d6269d17c8cf 333 case 1:
jah128 0:d6269d17c8cf 334 brightness = 2;
jah128 0:d6269d17c8cf 335 break;
jah128 0:d6269d17c8cf 336 case 2:
jah128 0:d6269d17c8cf 337 brightness = 5;
jah128 0:d6269d17c8cf 338 break;
jah128 0:d6269d17c8cf 339 case 5:
jah128 0:d6269d17c8cf 340 brightness = 10;
jah128 0:d6269d17c8cf 341 break;
jah128 0:d6269d17c8cf 342 case 10:
jah128 0:d6269d17c8cf 343 brightness = 20;
jah128 0:d6269d17c8cf 344 break;
jah128 0:d6269d17c8cf 345 case 20:
jah128 0:d6269d17c8cf 346 brightness = 50;
jah128 0:d6269d17c8cf 347 break;
jah128 0:d6269d17c8cf 348 case 50:
jah128 0:d6269d17c8cf 349 brightness = 100;
jah128 0:d6269d17c8cf 350 break;
jah128 0:d6269d17c8cf 351 }
jah128 0:d6269d17c8cf 352 demo_update_leds();
jah128 0:d6269d17c8cf 353 }
jah128 0:d6269d17c8cf 354
jah128 0:d6269d17c8cf 355 if(sub_menu == 12) {
jah128 0:d6269d17c8cf 356 if(bl_brightness < 100) bl_brightness+=10;
jah128 0:d6269d17c8cf 357 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 0:d6269d17c8cf 358 }
jah128 0:d6269d17c8cf 359 if(sub_menu == 13) level = 0;
jah128 0:d6269d17c8cf 360
jah128 0:d6269d17c8cf 361 break;
jah128 0:d6269d17c8cf 362 case 1: // Sensors Menu
jah128 0:d6269d17c8cf 363 if(sub_menu == 4 || sub_menu == 5) {
jah128 0:d6269d17c8cf 364 if(base_ir_index == 4) base_ir_index = 0;
jah128 0:d6269d17c8cf 365 else base_ir_index ++;
jah128 0:d6269d17c8cf 366 }
jah128 0:d6269d17c8cf 367 if(sub_menu > 5 && sub_menu < 9) {
jah128 0:d6269d17c8cf 368 if(side_ir_index == 7) side_ir_index = 0;
jah128 0:d6269d17c8cf 369 else side_ir_index ++;
jah128 0:d6269d17c8cf 370 }
jah128 0:d6269d17c8cf 371 if(sub_menu == 11) level = 0;
jah128 0:d6269d17c8cf 372 break;
jah128 0:d6269d17c8cf 373 case 2: // Motor Menu
jah128 0:d6269d17c8cf 374 if(sub_menu == 0) {
jah128 0:d6269d17c8cf 375 left_speed -= 5;
jah128 0:d6269d17c8cf 376 if(left_speed < -100) left_speed = -100;
jah128 8:6c92789d5f87 377 motors.set_left_motor_speed(left_speed / 100.0f);
jah128 0:d6269d17c8cf 378 }
jah128 0:d6269d17c8cf 379 if(sub_menu == 1) {
jah128 0:d6269d17c8cf 380 right_speed -= 5;
jah128 0:d6269d17c8cf 381 if(right_speed < -100) right_speed = -100;
jah128 8:6c92789d5f87 382 motors.set_right_motor_speed(right_speed / 100.0f);
jah128 0:d6269d17c8cf 383 }
jah128 0:d6269d17c8cf 384 if(sub_menu == 2) {
jah128 0:d6269d17c8cf 385 both_motor_mode++;
jah128 0:d6269d17c8cf 386 if(both_motor_mode == 6) both_motor_mode=0;
jah128 0:d6269d17c8cf 387 switch(both_motor_mode) {
jah128 0:d6269d17c8cf 388 case 0:
jah128 8:6c92789d5f87 389 motors.stop();
jah128 0:d6269d17c8cf 390 break;
jah128 0:d6269d17c8cf 391 case 1:
jah128 8:6c92789d5f87 392 motors.brake();
jah128 0:d6269d17c8cf 393 break;
jah128 0:d6269d17c8cf 394 case 2:
jah128 8:6c92789d5f87 395 motors.forward(0.5);
jah128 0:d6269d17c8cf 396 break;
jah128 0:d6269d17c8cf 397 case 3:
jah128 8:6c92789d5f87 398 motors.forward(1);
jah128 0:d6269d17c8cf 399 break;
jah128 0:d6269d17c8cf 400 case 4:
jah128 8:6c92789d5f87 401 motors.backward(0.5);
jah128 0:d6269d17c8cf 402 break;
jah128 0:d6269d17c8cf 403 case 5:
jah128 8:6c92789d5f87 404 motors.backward(1.0);
jah128 0:d6269d17c8cf 405 break;
jah128 0:d6269d17c8cf 406 }
jah128 0:d6269d17c8cf 407 }
jah128 0:d6269d17c8cf 408 if(sub_menu == 3) {
jah128 0:d6269d17c8cf 409 level = 0;
jah128 0:d6269d17c8cf 410 }
jah128 0:d6269d17c8cf 411 break;
jah128 0:d6269d17c8cf 412 case 3: // Radio Menu
jah128 0:d6269d17c8cf 413 if(sub_menu == 0) level = 0;
jah128 0:d6269d17c8cf 414 break;
jah128 0:d6269d17c8cf 415 case 4: // Info Menu
jah128 0:d6269d17c8cf 416 if(sub_menu == 6) level = 0;
jah128 0:d6269d17c8cf 417 break;
jah128 0:d6269d17c8cf 418 case 5: // Demo Menu
jah128 0:d6269d17c8cf 419 if(sub_menu == 0) {
jah128 0:d6269d17c8cf 420 if(demo_running == 0) {
jah128 0:d6269d17c8cf 421 start_line_demo();
jah128 0:d6269d17c8cf 422 } else {
jah128 0:d6269d17c8cf 423 demo_running = 0;
jah128 0:d6269d17c8cf 424 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 8:6c92789d5f87 425 motors.stop();
jah128 0:d6269d17c8cf 426 }
jah128 0:d6269d17c8cf 427 }
jah128 0:d6269d17c8cf 428 if(sub_menu == 1) {
jah128 0:d6269d17c8cf 429 if(demo_running == 0) {
jah128 0:d6269d17c8cf 430 start_obstacle_demo();
jah128 0:d6269d17c8cf 431 } else {
jah128 0:d6269d17c8cf 432 demo_running = 0;
jah128 0:d6269d17c8cf 433 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 8:6c92789d5f87 434 motors.stop();
jah128 0:d6269d17c8cf 435
jah128 0:d6269d17c8cf 436 }
jah128 0:d6269d17c8cf 437 }
jah128 0:d6269d17c8cf 438 if(sub_menu == 2) {
jah128 0:d6269d17c8cf 439 if(demo_running == 0) {
jah128 0:d6269d17c8cf 440 start_spinning_demo();
jah128 0:d6269d17c8cf 441 } else {
jah128 0:d6269d17c8cf 442 demo_running = 0;
jah128 0:d6269d17c8cf 443 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 8:6c92789d5f87 444 motors.stop();
jah128 0:d6269d17c8cf 445 }
jah128 0:d6269d17c8cf 446 }
jah128 0:d6269d17c8cf 447 if(sub_menu == 3) {
jah128 0:d6269d17c8cf 448 if(demo_running == 0) {
jah128 0:d6269d17c8cf 449 start_stress_demo();
jah128 0:d6269d17c8cf 450 } else {
jah128 0:d6269d17c8cf 451 demo_running = 0;
jah128 0:d6269d17c8cf 452 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 8:6c92789d5f87 453 motors.stop();
jah128 0:d6269d17c8cf 454 }
jah128 0:d6269d17c8cf 455 }
jah128 0:d6269d17c8cf 456 if(sub_menu == 4) level = 0;
jah128 0:d6269d17c8cf 457 break;
jah128 0:d6269d17c8cf 458 }
jah128 0:d6269d17c8cf 459 break;
jah128 0:d6269d17c8cf 460 }
jah128 0:d6269d17c8cf 461 break;
jah128 0:d6269d17c8cf 462 case 4: //Left pressed
jah128 0:d6269d17c8cf 463 switch(level) {
jah128 0:d6269d17c8cf 464 case 0:
jah128 0:d6269d17c8cf 465 if(top_menu == 0) {
jah128 4:1c621cb8cf0d 466 top_menu = 8;
jah128 0:d6269d17c8cf 467 }
jah128 4:1c621cb8cf0d 468 else {
jah128 4:1c621cb8cf0d 469 if(use_flash_basic == 0 && top_menu == 7) top_menu = 6;
jah128 4:1c621cb8cf0d 470 top_menu --;
jah128 4:1c621cb8cf0d 471 }
jah128 0:d6269d17c8cf 472 break;
jah128 0:d6269d17c8cf 473 case 1:
jah128 0:d6269d17c8cf 474 switch(top_menu) {
jah128 0:d6269d17c8cf 475 case 0: //LED Menu
jah128 0:d6269d17c8cf 476 if(sub_menu == 0) sub_menu = 13;
jah128 0:d6269d17c8cf 477 else sub_menu --;
jah128 0:d6269d17c8cf 478 break;
jah128 0:d6269d17c8cf 479 case 1: //Sensors Menu
jah128 0:d6269d17c8cf 480 if(sub_menu == 0) sub_menu = 11;
jah128 0:d6269d17c8cf 481 else sub_menu --;
jah128 0:d6269d17c8cf 482 break;
jah128 0:d6269d17c8cf 483
jah128 0:d6269d17c8cf 484 case 2: //Motor Menu
jah128 0:d6269d17c8cf 485 if(sub_menu == 0) sub_menu = 3;
jah128 0:d6269d17c8cf 486 else sub_menu --;
jah128 0:d6269d17c8cf 487 break;
jah128 0:d6269d17c8cf 488 case 4: //Info Menu
jah128 0:d6269d17c8cf 489 if(sub_menu == 0) sub_menu = 6;
jah128 0:d6269d17c8cf 490 else sub_menu --;
jah128 0:d6269d17c8cf 491 break;
jah128 0:d6269d17c8cf 492 case 5: //Demo Menu
jah128 0:d6269d17c8cf 493 if(sub_menu == 0) sub_menu = 4;
jah128 0:d6269d17c8cf 494 else sub_menu --;
jah128 0:d6269d17c8cf 495 break;
jah128 4:1c621cb8cf0d 496 case 6: //Calibrate Menu
jah128 4:1c621cb8cf0d 497 if(sub_menu == 0) sub_menu = 2;
jah128 4:1c621cb8cf0d 498 else sub_menu --;
jah128 4:1c621cb8cf0d 499 break;
jah128 0:d6269d17c8cf 500 case 7: //PsiBasic Menu
jah128 0:d6269d17c8cf 501 if(sub_menu == 0) sub_menu = psi_basic_file_count;
jah128 0:d6269d17c8cf 502 else sub_menu --;
jah128 0:d6269d17c8cf 503 }
jah128 0:d6269d17c8cf 504 break;
jah128 0:d6269d17c8cf 505
jah128 0:d6269d17c8cf 506 }
jah128 0:d6269d17c8cf 507 break;
jah128 0:d6269d17c8cf 508 case 8: //Right pressed
jah128 0:d6269d17c8cf 509 switch(level) {
jah128 0:d6269d17c8cf 510 case 0:
jah128 0:d6269d17c8cf 511 top_menu ++;
jah128 4:1c621cb8cf0d 512 if((top_menu - use_flash_basic) > 7) top_menu = 0;
jah128 0:d6269d17c8cf 513 break;
jah128 0:d6269d17c8cf 514 case 1:
jah128 0:d6269d17c8cf 515 switch(top_menu) {
jah128 0:d6269d17c8cf 516 case 0: //LED Menu
jah128 0:d6269d17c8cf 517 if(sub_menu == 13) sub_menu = 0;
jah128 0:d6269d17c8cf 518 else sub_menu ++;
jah128 0:d6269d17c8cf 519 break;
jah128 0:d6269d17c8cf 520 case 1: //Sensors Menu
jah128 0:d6269d17c8cf 521 if(sub_menu == 11) sub_menu = 0;
jah128 0:d6269d17c8cf 522 else sub_menu ++;
jah128 0:d6269d17c8cf 523 break;
jah128 0:d6269d17c8cf 524 case 2: //Motor Menu
jah128 0:d6269d17c8cf 525 if(sub_menu == 3) sub_menu = 0;
jah128 0:d6269d17c8cf 526 else sub_menu ++;
jah128 0:d6269d17c8cf 527 break;
jah128 0:d6269d17c8cf 528 case 4: //Info Menu
jah128 0:d6269d17c8cf 529 if(sub_menu == 6) sub_menu = 0;
jah128 0:d6269d17c8cf 530 else sub_menu ++;
jah128 0:d6269d17c8cf 531 break;
jah128 0:d6269d17c8cf 532 case 5: //Demo Menu
jah128 0:d6269d17c8cf 533 if(sub_menu == 4) sub_menu = 0;
jah128 0:d6269d17c8cf 534 else sub_menu ++;
jah128 0:d6269d17c8cf 535 break;
jah128 4:1c621cb8cf0d 536 case 6: //Calibrate Menu
jah128 4:1c621cb8cf0d 537 if(sub_menu == 2) sub_menu = 0;
jah128 4:1c621cb8cf0d 538 else sub_menu ++;
jah128 4:1c621cb8cf0d 539 break;
jah128 0:d6269d17c8cf 540 case 7: //PsiBasic Menu
jah128 0:d6269d17c8cf 541 if(sub_menu == psi_basic_file_count) sub_menu = 0;
jah128 0:d6269d17c8cf 542 else sub_menu ++;
jah128 0:d6269d17c8cf 543 }
jah128 0:d6269d17c8cf 544 break;
jah128 0:d6269d17c8cf 545 }
jah128 0:d6269d17c8cf 546 break;
jah128 0:d6269d17c8cf 547 case 16: //Select pressed
jah128 0:d6269d17c8cf 548 switch(level) {
jah128 0:d6269d17c8cf 549 case 0:
jah128 4:1c621cb8cf0d 550 if(top_menu != 8) {
jah128 0:d6269d17c8cf 551 level++;
jah128 0:d6269d17c8cf 552 sub_menu = 0;
jah128 0:d6269d17c8cf 553 } else {
jah128 0:d6269d17c8cf 554 demo_on = 0;
jah128 0:d6269d17c8cf 555 user_code_running = user_code_restore_mode;
jah128 0:d6269d17c8cf 556 }
jah128 0:d6269d17c8cf 557 break;
jah128 0:d6269d17c8cf 558 case 1:
jah128 0:d6269d17c8cf 559 switch(top_menu) {
jah128 0:d6269d17c8cf 560 case 0: //LED Menu
jah128 0:d6269d17c8cf 561 if(sub_menu == 9) {
jah128 0:d6269d17c8cf 562 for(int i=0; i<9; i++) {
jah128 0:d6269d17c8cf 563 led_state[i]=all_led_state;
jah128 0:d6269d17c8cf 564 }
jah128 0:d6269d17c8cf 565 demo_update_leds();
jah128 0:d6269d17c8cf 566 }
jah128 0:d6269d17c8cf 567 if(sub_menu == 13) level = 0;
jah128 0:d6269d17c8cf 568 break;
jah128 0:d6269d17c8cf 569 case 1: // Sensors Menu
jah128 0:d6269d17c8cf 570 if(sub_menu == 11) level = 0;
jah128 0:d6269d17c8cf 571 break;
jah128 0:d6269d17c8cf 572 case 2: //Motor Menu
jah128 0:d6269d17c8cf 573 if(sub_menu == 2) {
jah128 0:d6269d17c8cf 574 switch(both_motor_mode) {
jah128 0:d6269d17c8cf 575 case 0:
jah128 8:6c92789d5f87 576 motors.stop();
jah128 0:d6269d17c8cf 577 break;
jah128 0:d6269d17c8cf 578 case 1:
jah128 8:6c92789d5f87 579 motors.brake();
jah128 0:d6269d17c8cf 580 break;
jah128 0:d6269d17c8cf 581 case 2:
jah128 8:6c92789d5f87 582 motors.forward(0.5);
jah128 0:d6269d17c8cf 583 break;
jah128 0:d6269d17c8cf 584 case 3:
jah128 8:6c92789d5f87 585 motors.forward(1);
jah128 0:d6269d17c8cf 586 break;
jah128 0:d6269d17c8cf 587 case 4:
jah128 8:6c92789d5f87 588 motors.backward(0.5);
jah128 0:d6269d17c8cf 589 break;
jah128 0:d6269d17c8cf 590 case 5:
jah128 8:6c92789d5f87 591 motors.backward(1.0);
jah128 0:d6269d17c8cf 592 break;
jah128 0:d6269d17c8cf 593 }
jah128 0:d6269d17c8cf 594 }
jah128 0:d6269d17c8cf 595 if(sub_menu == 3) {
jah128 0:d6269d17c8cf 596 level = 0;
jah128 0:d6269d17c8cf 597 }
jah128 0:d6269d17c8cf 598 break;
jah128 0:d6269d17c8cf 599 case 3: // Radio Menu
jah128 0:d6269d17c8cf 600 if(sub_menu == 0) level = 0;
jah128 0:d6269d17c8cf 601 break;
jah128 0:d6269d17c8cf 602 case 4: // Info Menu
jah128 0:d6269d17c8cf 603 if(sub_menu == 6) level = 0;
jah128 0:d6269d17c8cf 604 break;
jah128 0:d6269d17c8cf 605 case 5: // Demo Menu
jah128 0:d6269d17c8cf 606 if(sub_menu == 4) level = 0;
jah128 0:d6269d17c8cf 607 break;
jah128 4:1c621cb8cf0d 608 case 6: // Calibrate Menu
jah128 4:1c621cb8cf0d 609 if(sub_menu == 2) level = 0;
jah128 4:1c621cb8cf0d 610 break;
jah128 0:d6269d17c8cf 611 case 7: // PsiBasic Menu
jah128 0:d6269d17c8cf 612 if(sub_menu == psi_basic_file_count) level = 0;
jah128 0:d6269d17c8cf 613 break;
jah128 0:d6269d17c8cf 614 }
jah128 0:d6269d17c8cf 615 break;
jah128 0:d6269d17c8cf 616 }
jah128 0:d6269d17c8cf 617 break;
jah128 0:d6269d17c8cf 618 }
jah128 0:d6269d17c8cf 619 } else started = 1;
jah128 0:d6269d17c8cf 620 display.clear_display();
jah128 0:d6269d17c8cf 621 switch(level) {
jah128 0:d6269d17c8cf 622 case 0:
jah128 0:d6269d17c8cf 623 //Top level menu
jah128 0:d6269d17c8cf 624 switch(top_menu) {
jah128 0:d6269d17c8cf 625 case 0:
jah128 0:d6269d17c8cf 626 strcpy(topline,"---TEST LEDS----");
jah128 0:d6269d17c8cf 627 break;
jah128 0:d6269d17c8cf 628 case 1:
jah128 0:d6269d17c8cf 629 strcpy(topline,"--TEST SENSORS--");
jah128 0:d6269d17c8cf 630 break;
jah128 0:d6269d17c8cf 631 case 2:
jah128 0:d6269d17c8cf 632 strcpy(topline,"--TEST MOTORS---");
jah128 0:d6269d17c8cf 633 break;
jah128 0:d6269d17c8cf 634 case 3:
jah128 0:d6269d17c8cf 635 strcpy(topline,"---TEST RADIO---");
jah128 0:d6269d17c8cf 636 break;
jah128 0:d6269d17c8cf 637 case 4:
jah128 0:d6269d17c8cf 638 strcpy(topline,"------INFO------");
jah128 0:d6269d17c8cf 639 break;
jah128 0:d6269d17c8cf 640 case 5:
jah128 0:d6269d17c8cf 641 strcpy(topline,"---CODE DEMOS---");
jah128 0:d6269d17c8cf 642 break;
jah128 0:d6269d17c8cf 643 case 6:
jah128 4:1c621cb8cf0d 644 strcpy(topline,"-MOTOR CALIBRATE");
jah128 0:d6269d17c8cf 645 break;
jah128 0:d6269d17c8cf 646 case 7:
jah128 0:d6269d17c8cf 647 strcpy(topline,"---PSI BASIC----");
jah128 0:d6269d17c8cf 648 break;
jah128 4:1c621cb8cf0d 649 case 8:
jah128 4:1c621cb8cf0d 650 strcpy(topline,"------EXIT------");
jah128 4:1c621cb8cf0d 651 break;
jah128 0:d6269d17c8cf 652 }
jah128 0:d6269d17c8cf 653 strcpy(bottomline,"");
jah128 0:d6269d17c8cf 654 break;
jah128 0:d6269d17c8cf 655 case 1:
jah128 0:d6269d17c8cf 656 //Sub level menu
jah128 0:d6269d17c8cf 657 switch(top_menu) {
jah128 0:d6269d17c8cf 658 case 7:
jah128 0:d6269d17c8cf 659 strcpy(topline,"-PSI BASIC MENU-");
jah128 0:d6269d17c8cf 660 if(sub_menu == psi_basic_file_count) strcpy(bottomline,"EXIT");
jah128 0:d6269d17c8cf 661 else strcpy(bottomline,basic_filenames.at(sub_menu).c_str());
jah128 0:d6269d17c8cf 662 break;
jah128 0:d6269d17c8cf 663 case 0:
jah128 0:d6269d17c8cf 664 strcpy(topline,"----LED MENU----");
jah128 0:d6269d17c8cf 665 char led_status[7];
jah128 0:d6269d17c8cf 666 if(sub_menu<9) {
jah128 0:d6269d17c8cf 667 switch(led_state[sub_menu]) {
jah128 0:d6269d17c8cf 668 case 0:
jah128 0:d6269d17c8cf 669 strcpy(led_status,"OFF");
jah128 0:d6269d17c8cf 670 break;
jah128 0:d6269d17c8cf 671 case 1:
jah128 0:d6269d17c8cf 672 strcpy(led_status,"RED");
jah128 0:d6269d17c8cf 673 break;
jah128 0:d6269d17c8cf 674 case 2:
jah128 0:d6269d17c8cf 675 strcpy(led_status,"GREEN");
jah128 0:d6269d17c8cf 676 break;
jah128 0:d6269d17c8cf 677 case 3:
jah128 0:d6269d17c8cf 678 strcpy(led_status,"YELLOW");
jah128 0:d6269d17c8cf 679 break;
jah128 0:d6269d17c8cf 680 }
jah128 0:d6269d17c8cf 681 }
jah128 0:d6269d17c8cf 682 if(sub_menu < 8) sprintf(bottomline,"OUTER %d: %s",(sub_menu + 1),led_status);
jah128 0:d6269d17c8cf 683 if(sub_menu == 8) sprintf(bottomline,"CENTER: %s",led_status);
jah128 0:d6269d17c8cf 684 if(sub_menu == 9) {
jah128 0:d6269d17c8cf 685 switch(all_led_state) {
jah128 0:d6269d17c8cf 686 case 0:
jah128 0:d6269d17c8cf 687 strcpy(led_status,"OFF");
jah128 0:d6269d17c8cf 688 break;
jah128 0:d6269d17c8cf 689 case 1:
jah128 0:d6269d17c8cf 690 strcpy(led_status,"RED");
jah128 0:d6269d17c8cf 691 break;
jah128 0:d6269d17c8cf 692 case 2:
jah128 0:d6269d17c8cf 693 strcpy(led_status,"GREEN");
jah128 0:d6269d17c8cf 694 break;
jah128 0:d6269d17c8cf 695 case 3:
jah128 0:d6269d17c8cf 696 strcpy(led_status,"YELLOW");
jah128 0:d6269d17c8cf 697 break;
jah128 0:d6269d17c8cf 698 }
jah128 0:d6269d17c8cf 699 sprintf(bottomline,"SET ALL %s", led_status);
jah128 0:d6269d17c8cf 700 }
jah128 0:d6269d17c8cf 701 if(sub_menu == 10) {
jah128 0:d6269d17c8cf 702 if(base_led_state == 0) strcpy(led_status,"OFF");
jah128 0:d6269d17c8cf 703 else strcpy(led_status,"ON");
jah128 0:d6269d17c8cf 704 sprintf(bottomline,"BASE: %s",led_status);
jah128 0:d6269d17c8cf 705 }
jah128 0:d6269d17c8cf 706 if(sub_menu == 11) sprintf(bottomline,"CLED BNESS %d%%", brightness);
jah128 0:d6269d17c8cf 707 if(sub_menu == 12) sprintf(bottomline,"DISP BNESS %d%%", bl_brightness);
jah128 0:d6269d17c8cf 708 if(sub_menu == 13) strcpy(bottomline,"EXIT");
jah128 0:d6269d17c8cf 709 break;
jah128 0:d6269d17c8cf 710
jah128 0:d6269d17c8cf 711 case 1:
jah128 0:d6269d17c8cf 712 strcpy(topline,"--SENSORS MENU--");
jah128 0:d6269d17c8cf 713 switch(sub_menu) {
jah128 0:d6269d17c8cf 714 case 0: {
jah128 10:e58323951c08 715 float battery = sensors.get_battery_voltage ();
jah128 0:d6269d17c8cf 716 sprintf(bottomline,"BATTERY: %1.3fV",battery);
jah128 0:d6269d17c8cf 717 break;
jah128 0:d6269d17c8cf 718 }
jah128 0:d6269d17c8cf 719 case 1: {
jah128 10:e58323951c08 720 float dc = sensors.get_dc_voltage ();
jah128 0:d6269d17c8cf 721 sprintf(bottomline,"DC: %1.3fV",dc);
jah128 0:d6269d17c8cf 722 break;
jah128 0:d6269d17c8cf 723 }
jah128 0:d6269d17c8cf 724 case 2: {
jah128 10:e58323951c08 725 float current = sensors.get_current ();
jah128 0:d6269d17c8cf 726 sprintf(bottomline,"CURRENT: %1.3fA",current);
jah128 0:d6269d17c8cf 727 break;
jah128 0:d6269d17c8cf 728 }
jah128 0:d6269d17c8cf 729 case 3: {
jah128 10:e58323951c08 730 float temperature = sensors.get_temperature();
jah128 0:d6269d17c8cf 731 sprintf(bottomline,"TEMP: %3.2fC", temperature);
jah128 0:d6269d17c8cf 732 break;
jah128 0:d6269d17c8cf 733 }
jah128 0:d6269d17c8cf 734 case 4:
jah128 10:e58323951c08 735 sensors.store_background_base_ir_values();
jah128 10:e58323951c08 736 sprintf(bottomline,"BIR%dB:%d",base_ir_index+1,sensors.get_background_base_ir_value(base_ir_index));
jah128 0:d6269d17c8cf 737 break;
jah128 0:d6269d17c8cf 738 case 5:
jah128 10:e58323951c08 739 sensors.store_illuminated_base_ir_values();
jah128 10:e58323951c08 740 sprintf(bottomline,"BIR%dR:%d",base_ir_index+1,sensors.get_illuminated_base_ir_value(base_ir_index));
jah128 0:d6269d17c8cf 741 break;
jah128 0:d6269d17c8cf 742 case 6:
jah128 10:e58323951c08 743 sensors.store_background_raw_ir_values();
jah128 10:e58323951c08 744 sprintf(bottomline,"SIR%dB:%d",side_ir_index+1,sensors.get_background_raw_ir_value(side_ir_index));
jah128 0:d6269d17c8cf 745 break;
jah128 0:d6269d17c8cf 746 case 7:
jah128 10:e58323951c08 747 sensors.store_illuminated_raw_ir_values();
jah128 10:e58323951c08 748 sprintf(bottomline,"SIR%dR:%d",side_ir_index+1,sensors.get_illuminated_raw_ir_value(side_ir_index));
jah128 0:d6269d17c8cf 749 break;
jah128 0:d6269d17c8cf 750 case 8:
jah128 0:d6269d17c8cf 751 sprintf(bottomline,"SIR%dD:%3.1f",side_ir_index+1,0.0);
jah128 0:d6269d17c8cf 752 break;
jah128 0:d6269d17c8cf 753 case 9:
jah128 0:d6269d17c8cf 754 if(ultrasonic_distance_updated == 1) {
jah128 0:d6269d17c8cf 755 sprintf(bottomline,"USONIC:%3dcm",ultrasonic_distance);
jah128 0:d6269d17c8cf 756 } else sprintf(bottomline,"USONIC:---------");
jah128 10:e58323951c08 757 sensors.update_ultrasonic_measure();
jah128 0:d6269d17c8cf 758 break;
jah128 0:d6269d17c8cf 759 case 10:
jah128 10:e58323951c08 760 sensors.store_line_position();
jah128 0:d6269d17c8cf 761 if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position);
jah128 0:d6269d17c8cf 762 else sprintf(bottomline,"LINE:---------");
jah128 0:d6269d17c8cf 763 break;
jah128 0:d6269d17c8cf 764 case 11:
jah128 0:d6269d17c8cf 765 sprintf(bottomline,"EXIT");
jah128 0:d6269d17c8cf 766 break;
jah128 0:d6269d17c8cf 767 }
jah128 0:d6269d17c8cf 768 break;
jah128 0:d6269d17c8cf 769 case 2:
jah128 0:d6269d17c8cf 770 strcpy(topline,"--MOTORS MENU---");
jah128 0:d6269d17c8cf 771 switch(sub_menu) {
jah128 0:d6269d17c8cf 772 case 0:
jah128 0:d6269d17c8cf 773 sprintf(bottomline,"LEFT: %d%%", left_speed);
jah128 0:d6269d17c8cf 774 break;
jah128 0:d6269d17c8cf 775 case 1:
jah128 0:d6269d17c8cf 776 sprintf(bottomline,"RIGHT: %d%%", right_speed);
jah128 0:d6269d17c8cf 777 break;
jah128 0:d6269d17c8cf 778 case 2:
jah128 0:d6269d17c8cf 779 char both_mode_string[16];
jah128 0:d6269d17c8cf 780 switch(both_motor_mode) {
jah128 0:d6269d17c8cf 781 case 0:
jah128 0:d6269d17c8cf 782 strcpy(both_mode_string,"OFF");
jah128 0:d6269d17c8cf 783 break;
jah128 0:d6269d17c8cf 784 case 1:
jah128 0:d6269d17c8cf 785 strcpy(both_mode_string,"BRAKE");
jah128 0:d6269d17c8cf 786 break;
jah128 0:d6269d17c8cf 787 case 2:
jah128 0:d6269d17c8cf 788 strcpy(both_mode_string,"+50%");
jah128 0:d6269d17c8cf 789 break;
jah128 0:d6269d17c8cf 790 case 3:
jah128 0:d6269d17c8cf 791 strcpy(both_mode_string,"+100%");
jah128 0:d6269d17c8cf 792 break;
jah128 0:d6269d17c8cf 793 case 4:
jah128 0:d6269d17c8cf 794 strcpy(both_mode_string,"-50%");
jah128 0:d6269d17c8cf 795 break;
jah128 0:d6269d17c8cf 796 case 5:
jah128 0:d6269d17c8cf 797 strcpy(both_mode_string,"-100%");
jah128 0:d6269d17c8cf 798 break;
jah128 0:d6269d17c8cf 799 }
jah128 0:d6269d17c8cf 800 sprintf(bottomline,"BOTH TO %s",both_mode_string);
jah128 0:d6269d17c8cf 801 break;
jah128 0:d6269d17c8cf 802 case 3:
jah128 0:d6269d17c8cf 803 sprintf(bottomline,"EXIT");
jah128 0:d6269d17c8cf 804 break;
jah128 0:d6269d17c8cf 805 }
jah128 0:d6269d17c8cf 806 break;
jah128 0:d6269d17c8cf 807 case 3:
jah128 0:d6269d17c8cf 808 strcpy(topline,"---RADIO MENU---");
jah128 0:d6269d17c8cf 809 switch(sub_menu) {
jah128 0:d6269d17c8cf 810
jah128 0:d6269d17c8cf 811 case 0:
jah128 0:d6269d17c8cf 812 sprintf(bottomline,"EXIT");
jah128 0:d6269d17c8cf 813 break;
jah128 0:d6269d17c8cf 814 }
jah128 0:d6269d17c8cf 815 break;
jah128 0:d6269d17c8cf 816 case 4:
jah128 0:d6269d17c8cf 817 strcpy(topline,"---INFO MENU----");
jah128 0:d6269d17c8cf 818 switch(sub_menu) {
jah128 0:d6269d17c8cf 819 case 0:
jah128 0:d6269d17c8cf 820 sprintf(bottomline,"ROBOT ID: %d",robot_id);
jah128 0:d6269d17c8cf 821 break;
jah128 0:d6269d17c8cf 822 case 1:
jah128 0:d6269d17c8cf 823 sprintf(bottomline,"SOFTWARE: %1.2f",SOFTWARE_VERSION_CODE);
jah128 0:d6269d17c8cf 824 break;
jah128 0:d6269d17c8cf 825 case 2:
jah128 0:d6269d17c8cf 826 if(firmware_version > 0) sprintf(bottomline,"FIRMWARE: %1.2f",firmware_version);
jah128 0:d6269d17c8cf 827 else sprintf(bottomline,"FIRMWARE: ?????");
jah128 0:d6269d17c8cf 828 break;
jah128 0:d6269d17c8cf 829 case 3:
jah128 0:d6269d17c8cf 830 sprintf(bottomline,"PROG:%s",program_name);
jah128 0:d6269d17c8cf 831 break;
jah128 0:d6269d17c8cf 832 case 4:
jah128 0:d6269d17c8cf 833 sprintf(bottomline,"AUTH:%s",author_name);
jah128 0:d6269d17c8cf 834 break;
jah128 0:d6269d17c8cf 835 case 5:
jah128 0:d6269d17c8cf 836 sprintf(bottomline,"VER:%s",version_name);
jah128 0:d6269d17c8cf 837 break;
jah128 0:d6269d17c8cf 838 case 6:
jah128 0:d6269d17c8cf 839 sprintf(bottomline,"EXIT");
jah128 0:d6269d17c8cf 840 break;
jah128 0:d6269d17c8cf 841 }
jah128 0:d6269d17c8cf 842 break;
jah128 0:d6269d17c8cf 843 case 5:
jah128 0:d6269d17c8cf 844 strcpy(topline,"---CODE DEMOS---");
jah128 0:d6269d17c8cf 845 switch(sub_menu) {
jah128 0:d6269d17c8cf 846 case 0:
jah128 0:d6269d17c8cf 847 sprintf(bottomline,"LINE FOLLOW");
jah128 0:d6269d17c8cf 848 break;
jah128 0:d6269d17c8cf 849 case 1:
jah128 0:d6269d17c8cf 850 sprintf(bottomline,"OBST. AVOID");
jah128 0:d6269d17c8cf 851 break;
jah128 0:d6269d17c8cf 852 case 2:
jah128 0:d6269d17c8cf 853 sprintf(bottomline,"COLOUR SPIN");
jah128 0:d6269d17c8cf 854 break;
jah128 0:d6269d17c8cf 855 case 3:
jah128 0:d6269d17c8cf 856 sprintf(bottomline,"STRESS TEST");
jah128 0:d6269d17c8cf 857 break;
jah128 0:d6269d17c8cf 858 case 4:
jah128 0:d6269d17c8cf 859 sprintf(bottomline,"EXIT");
jah128 0:d6269d17c8cf 860 break;
jah128 0:d6269d17c8cf 861 }
jah128 0:d6269d17c8cf 862 break;
jah128 0:d6269d17c8cf 863 case 6:
jah128 4:1c621cb8cf0d 864 strcpy(topline,"-MOTOR CALIBRATE");
jah128 4:1c621cb8cf0d 865 switch(sub_menu) {
jah128 4:1c621cb8cf0d 866 case 0:
jah128 4:1c621cb8cf0d 867 sprintf(bottomline,"RUN A3 TEST");
jah128 4:1c621cb8cf0d 868 break;
jah128 4:1c621cb8cf0d 869 case 1:
jah128 4:1c621cb8cf0d 870 sprintf(bottomline,"ENTER VALUE");
jah128 4:1c621cb8cf0d 871 break;
jah128 4:1c621cb8cf0d 872 case 2:
jah128 4:1c621cb8cf0d 873 sprintf(bottomline,"EXIT");
jah128 4:1c621cb8cf0d 874 break;
jah128 4:1c621cb8cf0d 875 }
jah128 0:d6269d17c8cf 876 break;
jah128 0:d6269d17c8cf 877 }
jah128 0:d6269d17c8cf 878 break;
jah128 0:d6269d17c8cf 879 }
jah128 0:d6269d17c8cf 880 display.write_string(topline);
jah128 0:d6269d17c8cf 881 display.set_position(1,0);
jah128 0:d6269d17c8cf 882 display.write_string(bottomline);
jah128 0:d6269d17c8cf 883 if(top_menu == 1 && level == 1) {
jah128 11:312663037b8c 884 demo_event.attach(this,&Demo::demo_event_thread, 0.25);
jah128 0:d6269d17c8cf 885 }
jah128 0:d6269d17c8cf 886 }
jah128 0:d6269d17c8cf 887
jah128 11:312663037b8c 888 void Demo::start_line_demo()
jah128 0:d6269d17c8cf 889 {
jah128 0:d6269d17c8cf 890 display.set_backlight_brightness(0);
jah128 0:d6269d17c8cf 891 time_out = 0.25f;
jah128 0:d6269d17c8cf 892 demo_timer.start();
jah128 0:d6269d17c8cf 893 state = 0;
jah128 0:d6269d17c8cf 894 speed = 0;
jah128 0:d6269d17c8cf 895 led_step = 0;
jah128 0:d6269d17c8cf 896 demo_running = 1;
jah128 11:312663037b8c 897 demo_timeout.attach_us(this,&Demo::line_demo_cycle,1000);
jah128 0:d6269d17c8cf 898 }
jah128 0:d6269d17c8cf 899
jah128 11:312663037b8c 900 void Demo::start_obstacle_demo()
jah128 0:d6269d17c8cf 901 {
jah128 0:d6269d17c8cf 902 display.set_backlight_brightness(0);
jah128 0:d6269d17c8cf 903 time_out = 0.25f;
jah128 0:d6269d17c8cf 904 demo_timer.start();
jah128 0:d6269d17c8cf 905 state = 0;
jah128 0:d6269d17c8cf 906 speed = 0;
jah128 0:d6269d17c8cf 907 led_step = 0;
jah128 0:d6269d17c8cf 908 demo_running = 1;
jah128 11:312663037b8c 909 demo_timeout.attach_us(this,&Demo::obstacle_demo_cycle,1000);
jah128 0:d6269d17c8cf 910 }
jah128 0:d6269d17c8cf 911
jah128 11:312663037b8c 912 void Demo::start_stress_demo()
jah128 0:d6269d17c8cf 913 {
jah128 0:d6269d17c8cf 914 display.set_backlight_brightness(0.25);
jah128 0:d6269d17c8cf 915 display.write_string("STRESS TEST");
jah128 0:d6269d17c8cf 916 display.set_position(1,0);
jah128 0:d6269d17c8cf 917 display.write_string("----25%----");
jah128 0:d6269d17c8cf 918 time_out = 0.04f;
jah128 0:d6269d17c8cf 919 demo_timer.start();
jah128 0:d6269d17c8cf 920 state = 0;
jah128 0:d6269d17c8cf 921 speed = 0;
jah128 0:d6269d17c8cf 922 stress_step = 0;
jah128 0:d6269d17c8cf 923 spin_step = 0;
jah128 0:d6269d17c8cf 924 demo_running = 1;
jah128 11:312663037b8c 925 demo_timeout.attach_us(this,&Demo::stress_demo_cycle,1000);
jah128 0:d6269d17c8cf 926 }
jah128 0:d6269d17c8cf 927
jah128 11:312663037b8c 928 void Demo::start_spinning_demo()
jah128 0:d6269d17c8cf 929 {
jah128 0:d6269d17c8cf 930 display.set_backlight_brightness(0);
jah128 0:d6269d17c8cf 931 time_out = 0.0f;
jah128 0:d6269d17c8cf 932 demo_timer.start();
jah128 0:d6269d17c8cf 933 state = 0;
jah128 0:d6269d17c8cf 934 speed = 0;
jah128 0:d6269d17c8cf 935 led_step = 0;
jah128 0:d6269d17c8cf 936 spin_step = 0;
jah128 0:d6269d17c8cf 937 demo_running = 1;
jah128 11:312663037b8c 938 demo_timeout.attach_us(this,&Demo::spinning_demo_cycle,1000);
jah128 0:d6269d17c8cf 939 }
jah128 0:d6269d17c8cf 940
jah128 11:312663037b8c 941 void Demo::line_demo_cycle()
jah128 0:d6269d17c8cf 942 {
jah128 0:d6269d17c8cf 943 if(demo_timer.read() > time_out) {
jah128 10:e58323951c08 944 sensors.store_line_position();
jah128 0:d6269d17c8cf 945 if(line_found) {
jah128 0:d6269d17c8cf 946 time_out = 0.01f;
jah128 0:d6269d17c8cf 947 state = 0;
jah128 0:d6269d17c8cf 948 // Get the position of the line.
jah128 0:d6269d17c8cf 949 lf_current_pos_of_line = line_position;
jah128 0:d6269d17c8cf 950 lf_proportional = lf_current_pos_of_line;
jah128 0:d6269d17c8cf 951
jah128 0:d6269d17c8cf 952 // Compute the derivative
jah128 0:d6269d17c8cf 953 lf_derivative = lf_current_pos_of_line - lf_previous_pos_of_line;
jah128 0:d6269d17c8cf 954
jah128 0:d6269d17c8cf 955 // Compute the integral
jah128 0:d6269d17c8cf 956 lf_integral += lf_proportional;
jah128 0:d6269d17c8cf 957
jah128 0:d6269d17c8cf 958 // Remember the last position.
jah128 0:d6269d17c8cf 959 lf_previous_pos_of_line = lf_current_pos_of_line;
jah128 0:d6269d17c8cf 960
jah128 0:d6269d17c8cf 961 // Compute the power
jah128 0:d6269d17c8cf 962 lf_power = (lf_proportional * (LF_P_TERM) ) + (lf_integral*(LF_I_TERM)) + (lf_derivative*(LF_D_TERM)) ;
jah128 0:d6269d17c8cf 963
jah128 0:d6269d17c8cf 964 // Compute new speeds
jah128 0:d6269d17c8cf 965 lf_right = lf_speed-lf_power;
jah128 0:d6269d17c8cf 966 lf_left = lf_speed+lf_power;
jah128 0:d6269d17c8cf 967
jah128 0:d6269d17c8cf 968 // limit checks
jah128 0:d6269d17c8cf 969 if (lf_right < 0)
jah128 0:d6269d17c8cf 970 lf_right = 0;
jah128 0:d6269d17c8cf 971 else if (lf_right > 1.0f)
jah128 0:d6269d17c8cf 972 lf_right = 1.0f;
jah128 0:d6269d17c8cf 973
jah128 0:d6269d17c8cf 974 if (lf_left < 0)
jah128 0:d6269d17c8cf 975 lf_left = 0;
jah128 0:d6269d17c8cf 976 else if (lf_left > 1.0f)
jah128 0:d6269d17c8cf 977 lf_left = 1.0f;
jah128 0:d6269d17c8cf 978 }else{
jah128 0:d6269d17c8cf 979 //Cannot see line: hunt for it
jah128 0:d6269d17c8cf 980 if(lf_left > lf_right){
jah128 0:d6269d17c8cf 981 //Currently turning left, keep turning left
jah128 0:d6269d17c8cf 982 state ++;
jah128 0:d6269d17c8cf 983 float d_step = state * 0.04;
jah128 0:d6269d17c8cf 984 lf_left = 0.2 + d_step;
jah128 0:d6269d17c8cf 985 lf_right = -0.2 - d_step;
jah128 0:d6269d17c8cf 986 if(state > 20){
jah128 0:d6269d17c8cf 987 state = 0;
jah128 0:d6269d17c8cf 988 lf_right = 0.2;
jah128 0:d6269d17c8cf 989 lf_left = -0.2;
jah128 0:d6269d17c8cf 990 time_out += 0.01f;
jah128 0:d6269d17c8cf 991 if(time_out > 0.1f) demo_running = 0;
jah128 0:d6269d17c8cf 992 }
jah128 0:d6269d17c8cf 993 }else{
jah128 0:d6269d17c8cf 994 //Currently turning right, keep turning right
jah128 0:d6269d17c8cf 995 state ++;
jah128 0:d6269d17c8cf 996 float d_step = state * 0.04;
jah128 0:d6269d17c8cf 997 lf_left = -0.2 - d_step;
jah128 0:d6269d17c8cf 998 lf_right = 0.2 + d_step;
jah128 0:d6269d17c8cf 999 if(state > 20){
jah128 0:d6269d17c8cf 1000 state = 0;
jah128 0:d6269d17c8cf 1001 lf_right = -0.2;
jah128 0:d6269d17c8cf 1002 lf_left = 0.2;
jah128 0:d6269d17c8cf 1003 time_out += 0.01f;
jah128 0:d6269d17c8cf 1004 if(time_out > 0.1f) demo_running = 0;
jah128 0:d6269d17c8cf 1005 }
jah128 0:d6269d17c8cf 1006 }
jah128 0:d6269d17c8cf 1007 }
jah128 0:d6269d17c8cf 1008 // set speed
jah128 8:6c92789d5f87 1009 motors.set_left_motor_speed(lf_left);
jah128 8:6c92789d5f87 1010 motors.set_right_motor_speed(lf_right);
jah128 0:d6269d17c8cf 1011
jah128 0:d6269d17c8cf 1012
jah128 0:d6269d17c8cf 1013 demo_timer.reset();
jah128 0:d6269d17c8cf 1014 }
jah128 11:312663037b8c 1015 if(demo_running == 1)demo_timeout.attach_us(this,&Demo::line_demo_cycle,500);
jah128 0:d6269d17c8cf 1016 else {
jah128 8:6c92789d5f87 1017 motors.stop();
jah128 0:d6269d17c8cf 1018 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 0:d6269d17c8cf 1019 }
jah128 0:d6269d17c8cf 1020 }
jah128 0:d6269d17c8cf 1021
jah128 11:312663037b8c 1022 void Demo::stress_demo_cycle()
jah128 0:d6269d17c8cf 1023 {
jah128 0:d6269d17c8cf 1024 if(demo_timer.read() > time_out) {
jah128 0:d6269d17c8cf 1025 float pct = 0.25 + (0.25 * stress_step);
jah128 0:d6269d17c8cf 1026 switch(state) {
jah128 0:d6269d17c8cf 1027 case 0:
jah128 0:d6269d17c8cf 1028 if(spin_step % 2 == 0) {
jah128 8:6c92789d5f87 1029 motors.forward(pct);
jah128 9:dde9e21030eb 1030 led.set_leds(0xFF,0xFF);
jah128 0:d6269d17c8cf 1031 } else {
jah128 8:6c92789d5f87 1032 motors.backward(pct);
jah128 9:dde9e21030eb 1033 led.set_leds(0,0xFF);
jah128 0:d6269d17c8cf 1034 }
jah128 0:d6269d17c8cf 1035 spin_step ++;
jah128 0:d6269d17c8cf 1036 if(spin_step > 199) {
jah128 0:d6269d17c8cf 1037 state ++;
jah128 0:d6269d17c8cf 1038 spin_step = 0;
jah128 0:d6269d17c8cf 1039 }
jah128 0:d6269d17c8cf 1040 break;
jah128 0:d6269d17c8cf 1041 case 1:
jah128 0:d6269d17c8cf 1042 if(stress_step < 3) stress_step ++;
jah128 0:d6269d17c8cf 1043 float pct = 0.25 + (0.25 * stress_step);
jah128 0:d6269d17c8cf 1044 display.set_backlight_brightness(pct);
jah128 0:d6269d17c8cf 1045 display.set_position(1,0);
jah128 0:d6269d17c8cf 1046 switch(stress_step) {
jah128 0:d6269d17c8cf 1047 case 1:
jah128 0:d6269d17c8cf 1048 display.write_string("----50%----");
jah128 0:d6269d17c8cf 1049 break;
jah128 0:d6269d17c8cf 1050 case 2:
jah128 0:d6269d17c8cf 1051 display.write_string("----75%----");
jah128 0:d6269d17c8cf 1052 break;
jah128 0:d6269d17c8cf 1053 case 3:
jah128 0:d6269d17c8cf 1054 display.write_string("---100%----");
jah128 0:d6269d17c8cf 1055 break;
jah128 0:d6269d17c8cf 1056 }
jah128 0:d6269d17c8cf 1057 state = 0;
jah128 0:d6269d17c8cf 1058 break;
jah128 0:d6269d17c8cf 1059 }
jah128 0:d6269d17c8cf 1060 demo_timer.reset();
jah128 0:d6269d17c8cf 1061 }
jah128 11:312663037b8c 1062 if(demo_running == 1)demo_timeout.attach_us(this,&Demo::stress_demo_cycle,500);
jah128 0:d6269d17c8cf 1063 else {
jah128 8:6c92789d5f87 1064 motors.stop();
jah128 0:d6269d17c8cf 1065 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 0:d6269d17c8cf 1066 }
jah128 0:d6269d17c8cf 1067 }
jah128 0:d6269d17c8cf 1068
jah128 11:312663037b8c 1069 void Demo::spinning_demo_cycle()
jah128 0:d6269d17c8cf 1070 {
jah128 0:d6269d17c8cf 1071 if(demo_timer.read() > time_out) {
jah128 0:d6269d17c8cf 1072 switch(state) {
jah128 0:d6269d17c8cf 1073 case 0: //Robot is stopped
jah128 9:dde9e21030eb 1074 led.set_leds(0,0xFF);
jah128 9:dde9e21030eb 1075 led.set_center_led(1,1);
jah128 0:d6269d17c8cf 1076 speed = 0.1f;
jah128 8:6c92789d5f87 1077 motors.brake();
jah128 0:d6269d17c8cf 1078 time_out = 0.5;
jah128 0:d6269d17c8cf 1079 state = 1;
jah128 0:d6269d17c8cf 1080 led_step = 0;
jah128 0:d6269d17c8cf 1081 break;
jah128 0:d6269d17c8cf 1082 case 1: //Motor is turning right, accelerating
jah128 0:d6269d17c8cf 1083 time_out = 0.1;
jah128 9:dde9e21030eb 1084 led.set_center_led(2,1);
jah128 0:d6269d17c8cf 1085 switch(led_step) {
jah128 0:d6269d17c8cf 1086 case 0:
jah128 9:dde9e21030eb 1087 led.set_leds(0x01,0);
jah128 0:d6269d17c8cf 1088 break;
jah128 0:d6269d17c8cf 1089 case 1:
jah128 9:dde9e21030eb 1090 led.set_leds(0x02,0);
jah128 0:d6269d17c8cf 1091 break;
jah128 0:d6269d17c8cf 1092 case 2:
jah128 9:dde9e21030eb 1093 led.set_leds(0x04,0);
jah128 0:d6269d17c8cf 1094 break;
jah128 0:d6269d17c8cf 1095 case 3:
jah128 9:dde9e21030eb 1096 led.set_leds(0x08,0);
jah128 0:d6269d17c8cf 1097 break;
jah128 0:d6269d17c8cf 1098 case 4:
jah128 9:dde9e21030eb 1099 led.set_leds(0x10,0);
jah128 0:d6269d17c8cf 1100 break;
jah128 0:d6269d17c8cf 1101 case 5:
jah128 9:dde9e21030eb 1102 led.set_leds(0x20,0);
jah128 0:d6269d17c8cf 1103 break;
jah128 0:d6269d17c8cf 1104 case 6:
jah128 9:dde9e21030eb 1105 led.set_leds(0x40,0);
jah128 0:d6269d17c8cf 1106 break;
jah128 0:d6269d17c8cf 1107 case 7:
jah128 9:dde9e21030eb 1108 led.set_leds(0x80,0);
jah128 0:d6269d17c8cf 1109 break;
jah128 0:d6269d17c8cf 1110 }
jah128 0:d6269d17c8cf 1111 led_step ++;
jah128 0:d6269d17c8cf 1112 if(led_step == 8) led_step =0;
jah128 0:d6269d17c8cf 1113 if(speed < 1) {
jah128 0:d6269d17c8cf 1114 speed += 0.0125;
jah128 8:6c92789d5f87 1115 motors.turn(speed);
jah128 0:d6269d17c8cf 1116 } else {
jah128 0:d6269d17c8cf 1117 state = 2;
jah128 0:d6269d17c8cf 1118 spin_step = 0;
jah128 0:d6269d17c8cf 1119 led_step =0;
jah128 0:d6269d17c8cf 1120 }
jah128 0:d6269d17c8cf 1121 break;
jah128 0:d6269d17c8cf 1122 case 2: //Motor is turning right, full speed
jah128 9:dde9e21030eb 1123 led.set_center_led(3,1);
jah128 0:d6269d17c8cf 1124 switch(led_step) {
jah128 0:d6269d17c8cf 1125 case 0:
jah128 9:dde9e21030eb 1126 led.set_leds(0x33,0x33);
jah128 0:d6269d17c8cf 1127 break;
jah128 0:d6269d17c8cf 1128 case 1:
jah128 9:dde9e21030eb 1129 led.set_leds(0x66,0x66);
jah128 0:d6269d17c8cf 1130 break;
jah128 0:d6269d17c8cf 1131 case 2:
jah128 9:dde9e21030eb 1132 led.set_leds(0xCC,0xCC);
jah128 0:d6269d17c8cf 1133 break;
jah128 0:d6269d17c8cf 1134 case 3:
jah128 9:dde9e21030eb 1135 led.set_leds(0x99,0x99);
jah128 0:d6269d17c8cf 1136 break;
jah128 0:d6269d17c8cf 1137 }
jah128 0:d6269d17c8cf 1138 led_step ++;
jah128 0:d6269d17c8cf 1139 if(led_step == 4) led_step = 0;
jah128 0:d6269d17c8cf 1140 spin_step ++;
jah128 0:d6269d17c8cf 1141 if(spin_step == 40) {
jah128 0:d6269d17c8cf 1142 state = 3;
jah128 0:d6269d17c8cf 1143 led_step = 0;
jah128 0:d6269d17c8cf 1144 }
jah128 0:d6269d17c8cf 1145 break;
jah128 0:d6269d17c8cf 1146 case 3: //Motor is turning right, decelerating
jah128 9:dde9e21030eb 1147 led.set_center_led(2,1);
jah128 0:d6269d17c8cf 1148 switch(led_step) {
jah128 0:d6269d17c8cf 1149 case 0:
jah128 9:dde9e21030eb 1150 led.set_leds(0x01,0);
jah128 0:d6269d17c8cf 1151 break;
jah128 0:d6269d17c8cf 1152 case 1:
jah128 9:dde9e21030eb 1153 led.set_leds(0x02,0);
jah128 0:d6269d17c8cf 1154 break;
jah128 0:d6269d17c8cf 1155 case 2:
jah128 9:dde9e21030eb 1156 led.set_leds(0x04,0);
jah128 0:d6269d17c8cf 1157 break;
jah128 0:d6269d17c8cf 1158 case 3:
jah128 9:dde9e21030eb 1159 led.set_leds(0x08,0);
jah128 0:d6269d17c8cf 1160 break;
jah128 0:d6269d17c8cf 1161 case 4:
jah128 9:dde9e21030eb 1162 led.set_leds(0x10,0);
jah128 0:d6269d17c8cf 1163 break;
jah128 0:d6269d17c8cf 1164 case 5:
jah128 9:dde9e21030eb 1165 led.set_leds(0x20,0);
jah128 0:d6269d17c8cf 1166 break;
jah128 0:d6269d17c8cf 1167 case 6:
jah128 9:dde9e21030eb 1168 led.set_leds(0x40,0);
jah128 0:d6269d17c8cf 1169 break;
jah128 0:d6269d17c8cf 1170 case 7:
jah128 9:dde9e21030eb 1171 led.set_leds(0x80,0);
jah128 0:d6269d17c8cf 1172 break;
jah128 0:d6269d17c8cf 1173 }
jah128 0:d6269d17c8cf 1174 if(led_step == 0) led_step =8;
jah128 0:d6269d17c8cf 1175 led_step --;
jah128 0:d6269d17c8cf 1176 if(speed > 0.1) {
jah128 0:d6269d17c8cf 1177 speed -= 0.025;
jah128 8:6c92789d5f87 1178 motors.turn(speed);
jah128 0:d6269d17c8cf 1179 } else {
jah128 0:d6269d17c8cf 1180 state = 4;
jah128 0:d6269d17c8cf 1181 spin_step = 0;
jah128 0:d6269d17c8cf 1182 led_step =0;
jah128 0:d6269d17c8cf 1183 }
jah128 0:d6269d17c8cf 1184 break;
jah128 0:d6269d17c8cf 1185 case 4: //Robot is stopped
jah128 9:dde9e21030eb 1186 led.set_leds(0,0xFF);
jah128 9:dde9e21030eb 1187 led.set_center_led(1,1);
jah128 0:d6269d17c8cf 1188 speed = 0.1f;
jah128 8:6c92789d5f87 1189 motors.brake();
jah128 0:d6269d17c8cf 1190 time_out = 0.5;
jah128 0:d6269d17c8cf 1191 led_step =0;
jah128 0:d6269d17c8cf 1192 state = 5;
jah128 0:d6269d17c8cf 1193 break;
jah128 0:d6269d17c8cf 1194 case 5: //Motor is turning left, accelerating
jah128 0:d6269d17c8cf 1195 time_out = 0.1;
jah128 9:dde9e21030eb 1196 led.set_center_led(2,1);
jah128 0:d6269d17c8cf 1197 switch(led_step) {
jah128 0:d6269d17c8cf 1198 case 0:
jah128 9:dde9e21030eb 1199 led.set_leds(0x01,0);
jah128 0:d6269d17c8cf 1200 break;
jah128 0:d6269d17c8cf 1201 case 1:
jah128 9:dde9e21030eb 1202 led.set_leds(0x02,0);
jah128 0:d6269d17c8cf 1203 break;
jah128 0:d6269d17c8cf 1204 case 2:
jah128 9:dde9e21030eb 1205 led.set_leds(0x04,0);
jah128 0:d6269d17c8cf 1206 break;
jah128 0:d6269d17c8cf 1207 case 3:
jah128 9:dde9e21030eb 1208 led.set_leds(0x08,0);
jah128 0:d6269d17c8cf 1209 break;
jah128 0:d6269d17c8cf 1210 case 4:
jah128 9:dde9e21030eb 1211 led.set_leds(0x10,0);
jah128 0:d6269d17c8cf 1212 break;
jah128 0:d6269d17c8cf 1213 case 5:
jah128 9:dde9e21030eb 1214 led.set_leds(0x20,0);
jah128 0:d6269d17c8cf 1215 break;
jah128 0:d6269d17c8cf 1216 case 6:
jah128 9:dde9e21030eb 1217 led.set_leds(0x40,0);
jah128 0:d6269d17c8cf 1218 break;
jah128 0:d6269d17c8cf 1219 case 7:
jah128 9:dde9e21030eb 1220 led.set_leds(0x80,0);
jah128 0:d6269d17c8cf 1221 break;
jah128 0:d6269d17c8cf 1222 }
jah128 0:d6269d17c8cf 1223 if(led_step == 0) led_step =8;
jah128 0:d6269d17c8cf 1224 led_step --;
jah128 0:d6269d17c8cf 1225 if(speed < 1) {
jah128 0:d6269d17c8cf 1226 speed += 0.0125;
jah128 8:6c92789d5f87 1227 motors.turn(-speed);
jah128 0:d6269d17c8cf 1228 } else {
jah128 0:d6269d17c8cf 1229 state = 6;
jah128 0:d6269d17c8cf 1230 spin_step = 0;
jah128 0:d6269d17c8cf 1231 led_step = 0;
jah128 0:d6269d17c8cf 1232 }
jah128 0:d6269d17c8cf 1233 break;
jah128 0:d6269d17c8cf 1234 case 6: //Motor is turning left, full speed
jah128 9:dde9e21030eb 1235 led.set_center_led(3,1);
jah128 0:d6269d17c8cf 1236 switch(led_step) {
jah128 0:d6269d17c8cf 1237 case 0:
jah128 9:dde9e21030eb 1238 led.set_leds(0x33,0x33);
jah128 0:d6269d17c8cf 1239 break;
jah128 0:d6269d17c8cf 1240 case 1:
jah128 9:dde9e21030eb 1241 led.set_leds(0x66,0x66);
jah128 0:d6269d17c8cf 1242 break;
jah128 0:d6269d17c8cf 1243 case 2:
jah128 9:dde9e21030eb 1244 led.set_leds(0xCC,0xCC);
jah128 0:d6269d17c8cf 1245 break;
jah128 0:d6269d17c8cf 1246 case 3:
jah128 9:dde9e21030eb 1247 led.set_leds(0x99,0x99);
jah128 0:d6269d17c8cf 1248 break;
jah128 0:d6269d17c8cf 1249 }
jah128 0:d6269d17c8cf 1250 if(led_step == 0) led_step = 4;
jah128 0:d6269d17c8cf 1251 led_step --;
jah128 0:d6269d17c8cf 1252 spin_step ++;
jah128 0:d6269d17c8cf 1253 if(spin_step == 40) {
jah128 0:d6269d17c8cf 1254 state = 7;
jah128 0:d6269d17c8cf 1255 led_step = 0;
jah128 0:d6269d17c8cf 1256 }
jah128 0:d6269d17c8cf 1257 break;
jah128 0:d6269d17c8cf 1258 case 7: //Motor is turning left, decelerating
jah128 9:dde9e21030eb 1259 led.set_center_led(2,1);
jah128 0:d6269d17c8cf 1260 switch(led_step) {
jah128 0:d6269d17c8cf 1261 case 0:
jah128 9:dde9e21030eb 1262 led.set_leds(0x01,0);
jah128 0:d6269d17c8cf 1263 break;
jah128 0:d6269d17c8cf 1264 case 1:
jah128 9:dde9e21030eb 1265 led.set_leds(0x02,0);
jah128 0:d6269d17c8cf 1266 break;
jah128 0:d6269d17c8cf 1267 case 2:
jah128 9:dde9e21030eb 1268 led.set_leds(0x04,0);
jah128 0:d6269d17c8cf 1269 break;
jah128 0:d6269d17c8cf 1270 case 3:
jah128 9:dde9e21030eb 1271 led.set_leds(0x08,0);
jah128 0:d6269d17c8cf 1272 break;
jah128 0:d6269d17c8cf 1273 case 4:
jah128 9:dde9e21030eb 1274 led.set_leds(0x10,0);
jah128 0:d6269d17c8cf 1275 break;
jah128 0:d6269d17c8cf 1276 case 5:
jah128 9:dde9e21030eb 1277 led.set_leds(0x20,0);
jah128 0:d6269d17c8cf 1278 break;
jah128 0:d6269d17c8cf 1279 case 6:
jah128 9:dde9e21030eb 1280 led.set_leds(0x40,0);
jah128 0:d6269d17c8cf 1281 break;
jah128 0:d6269d17c8cf 1282 case 7:
jah128 9:dde9e21030eb 1283 led.set_leds(0x80,0);
jah128 0:d6269d17c8cf 1284 break;
jah128 0:d6269d17c8cf 1285 }
jah128 0:d6269d17c8cf 1286 led_step ++;
jah128 0:d6269d17c8cf 1287 if(led_step == 8) led_step =0;
jah128 0:d6269d17c8cf 1288 if(speed > 0.1) {
jah128 0:d6269d17c8cf 1289 speed -= 0.025;
jah128 8:6c92789d5f87 1290 motors.turn(-speed);
jah128 0:d6269d17c8cf 1291 } else {
jah128 0:d6269d17c8cf 1292 state = 0;
jah128 0:d6269d17c8cf 1293 spin_step = 0;
jah128 0:d6269d17c8cf 1294 led_step = 0;
jah128 0:d6269d17c8cf 1295 }
jah128 0:d6269d17c8cf 1296 break;
jah128 0:d6269d17c8cf 1297 }
jah128 0:d6269d17c8cf 1298 demo_timer.reset();
jah128 0:d6269d17c8cf 1299 }
jah128 11:312663037b8c 1300 if(demo_running == 1)demo_timeout.attach_us(this,&Demo::spinning_demo_cycle,500);
jah128 0:d6269d17c8cf 1301 else {
jah128 8:6c92789d5f87 1302 motors.stop();
jah128 0:d6269d17c8cf 1303 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 0:d6269d17c8cf 1304 }
jah128 0:d6269d17c8cf 1305 }
jah128 0:d6269d17c8cf 1306
jah128 11:312663037b8c 1307 void Demo::obstacle_demo_cycle()
jah128 0:d6269d17c8cf 1308 {
jah128 0:d6269d17c8cf 1309 if(demo_timer.read() > time_out) {
jah128 0:d6269d17c8cf 1310 switch(state) {
jah128 0:d6269d17c8cf 1311 case 0: //Robot is stopped
jah128 9:dde9e21030eb 1312 led.set_leds(0,0xFF);
jah128 9:dde9e21030eb 1313 led.set_center_led(1,0.4);
jah128 0:d6269d17c8cf 1314 speed = 0.3f;
jah128 8:6c92789d5f87 1315 motors.forward(speed);
jah128 0:d6269d17c8cf 1316 time_out = 0.05;
jah128 0:d6269d17c8cf 1317 state = 1;
jah128 0:d6269d17c8cf 1318 break;
jah128 0:d6269d17c8cf 1319 case 1: { //Motor is moving forward
jah128 10:e58323951c08 1320 sensors.store_ir_values();
jah128 10:e58323951c08 1321 int front_right = sensors.read_illuminated_raw_ir_value(0);
jah128 10:e58323951c08 1322 int front_left = sensors.read_illuminated_raw_ir_value(7);
jah128 0:d6269d17c8cf 1323 if(front_left > 400 || front_right > 400) {
jah128 8:6c92789d5f87 1324 motors.brake();
jah128 0:d6269d17c8cf 1325 time_out = 0.04;
jah128 0:d6269d17c8cf 1326 if(front_left > front_right)state=2;
jah128 0:d6269d17c8cf 1327 else state=3;
jah128 0:d6269d17c8cf 1328 } else {
jah128 0:d6269d17c8cf 1329 if(speed < 0.5) {
jah128 0:d6269d17c8cf 1330 speed += 0.03;
jah128 8:6c92789d5f87 1331 motors.forward(speed);
jah128 0:d6269d17c8cf 1332 }
jah128 0:d6269d17c8cf 1333 switch(led_step) {
jah128 0:d6269d17c8cf 1334 case 0:
jah128 9:dde9e21030eb 1335 led.set_leds(0x01,0);
jah128 0:d6269d17c8cf 1336 break;
jah128 0:d6269d17c8cf 1337 case 1:
jah128 9:dde9e21030eb 1338 led.set_leds(0x38,0);
jah128 0:d6269d17c8cf 1339 break;
jah128 0:d6269d17c8cf 1340 case 2:
jah128 9:dde9e21030eb 1341 led.set_leds(0x6C,0);
jah128 0:d6269d17c8cf 1342 break;
jah128 0:d6269d17c8cf 1343 case 3:
jah128 9:dde9e21030eb 1344 led.set_leds(0xC6,0);
jah128 0:d6269d17c8cf 1345 break;
jah128 0:d6269d17c8cf 1346 case 4:
jah128 9:dde9e21030eb 1347 led.set_leds(0x83,0);
jah128 0:d6269d17c8cf 1348 break;
jah128 0:d6269d17c8cf 1349 }
jah128 9:dde9e21030eb 1350 led.set_center_led(2, 0.6);
jah128 0:d6269d17c8cf 1351 led_step ++;
jah128 0:d6269d17c8cf 1352 if(led_step == 5) led_step = 0;
jah128 0:d6269d17c8cf 1353 }
jah128 0:d6269d17c8cf 1354 break;
jah128 0:d6269d17c8cf 1355 }
jah128 0:d6269d17c8cf 1356 case 2: //Turn right
jah128 8:6c92789d5f87 1357 motors.set_left_motor_speed(0.85);
jah128 8:6c92789d5f87 1358 motors.set_right_motor_speed(-0.85);
jah128 0:d6269d17c8cf 1359 time_out = 0.4;
jah128 0:d6269d17c8cf 1360 state = 0;
jah128 9:dde9e21030eb 1361 led.set_leds(0x0E,0x0E);
jah128 9:dde9e21030eb 1362 led.set_center_led(3,0.5);
jah128 0:d6269d17c8cf 1363 break;
jah128 0:d6269d17c8cf 1364 case 3: //Turn left
jah128 8:6c92789d5f87 1365 motors.set_left_motor_speed(-0.85);
jah128 8:6c92789d5f87 1366 motors.set_right_motor_speed(0.85);
jah128 0:d6269d17c8cf 1367 time_out = 0.4;
jah128 0:d6269d17c8cf 1368 state = 0;
jah128 9:dde9e21030eb 1369 led.set_leds(0xE0,0xE0);
jah128 9:dde9e21030eb 1370 led.set_center_led(3,0.5);
jah128 0:d6269d17c8cf 1371 break;
jah128 0:d6269d17c8cf 1372 }
jah128 0:d6269d17c8cf 1373 demo_timer.reset();
jah128 0:d6269d17c8cf 1374 }
jah128 11:312663037b8c 1375 if(demo_running == 1)demo_timeout.attach_us(this,&Demo::obstacle_demo_cycle,200);
jah128 0:d6269d17c8cf 1376 else {
jah128 8:6c92789d5f87 1377 motors.stop();
jah128 0:d6269d17c8cf 1378 display.set_backlight_brightness(bl_brightness * 0.01f);
jah128 0:d6269d17c8cf 1379 }
jah128 0:d6269d17c8cf 1380 }
jah128 0:d6269d17c8cf 1381
jah128 11:312663037b8c 1382 void Demo::demo_update_leds()
jah128 0:d6269d17c8cf 1383 {
jah128 0:d6269d17c8cf 1384 char red = 0;
jah128 0:d6269d17c8cf 1385 char green = 0;
jah128 0:d6269d17c8cf 1386 for(int i=0; i<8; i++) {
jah128 0:d6269d17c8cf 1387 if(led_state[i]==1 || led_state[i]==3) red+=(1<<i);
jah128 0:d6269d17c8cf 1388 if(led_state[i]==2 || led_state[i]==3) green+=(1<<i);
jah128 0:d6269d17c8cf 1389 }
jah128 9:dde9e21030eb 1390 led.set_leds(green,red);
jah128 0:d6269d17c8cf 1391 float brightness_f = brightness / 100.0f;
jah128 9:dde9e21030eb 1392 led.set_center_led(led_state[8], brightness_f);
jah128 9:dde9e21030eb 1393 led.set_base_led(base_led_state);
jah128 0:d6269d17c8cf 1394 }