James Hilder / PsiSwarmV9-Fork

Psi Swarm robot library version 0.9

Fork of PsiSwarmV9 by Psi Swarm Robot

demo.cpp@15:66be5ec52c3b, 2017-06-01 (annotated)

Committer:
jah128
Date:
Thu Jun 01 21:58:14 2017 +0000
Revision:
15:66be5ec52c3b
Parent:
14:2f1ad77d281e
Child:
16:50686c07ad07
Added colour detection code

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