James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/demo.cpp@1:f6356cf1cefc, 2015-10-03 (annotated)

Committer:
jah128
Date:
Sat Oct 03 23:09:10 2015 +0000
Revision:
1:f6356cf1cefc
Parent:
0:8a5497a2e366
Child:
2:a6214fd156ff
Added software version

Who changed what in which revision?

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