James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/demo.cpp@2:a6214fd156ff, 2015-10-05 (annotated)

Committer:
jah128
Date:
Mon Oct 05 14:42:16 2015 +0000
Revision:
2:a6214fd156ff
Parent:
1:f6356cf1cefc
Child:
3:cd048f6e544e
Changed nothing

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