Psi Swarm Robot / PsiSwarmV9

Psi Swarm robot library version 0.9

Dependents:   PsiSwarm_V9_Blank

Fork of PsiSwarmV9 by James Hilder

demo.cpp@19:3e3b03d80ea3, 2017-06-05 (annotated)

Committer:
jah128
Date:
Mon Jun 05 22:47:14 2017 +0000
Revision:
19:3e3b03d80ea3
Parent:
18:9204f74069b4
Child:
20:2b6ebe60929d
Added firmware writer

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