James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/demo.cpp@3:cd048f6e544e, 2015-10-05 (annotated)

Committer:
jah128
Date:
Mon Oct 05 20:42:37 2015 +0000
Revision:
3:cd048f6e544e
Parent:
2:a6214fd156ff
Child:
5:598298aa4900
Added crude line following code

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