Alan Millard / Mbed 2 deprecated BeautifulMemeProjectBT

Bluetooth communication for flocking.

Dependencies:   mbed

Fork of BeautifulMemeProject by James Hilder

PsiSwarm/demo.cpp@27:7eb032772bc2, 2016-01-31 (annotated)

Committer:
alanmillard
Date:
Sun Jan 31 15:14:54 2016 +0000
Revision:
27:7eb032772bc2
Parent:
6:ff3c66f7372b 
Flocking seems to work better now, however the robot controller sometimes "crashes" (unsure why).

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