James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/demo.cpp@5:598298aa4900, 2015-10-13 (annotated)

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