Psi Swarm Robot / PsiSwarmV7

Library for the PsiSwarm Robot - Version 0.7

Dependents:   PsiSwarm_V7_Blank

Fork of PsiSwarmLibrary by James Hilder

demo.cpp@6:b340a527add9, 2016-10-15 (annotated)

Committer:
jah128
Date:
Sat Oct 15 13:51:39 2016 +0000
Revision:
6:b340a527add9
Parent:
5:3cdd1a37cdd7 
Added Apache license to files

Who changed what in which revision?

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