James Hilder / PsiSwarmV9-Fork

Psi Swarm robot library version 0.9

Fork of PsiSwarmV9 by Psi Swarm Robot

demo.cpp@16:50686c07ad07, 2017-06-01 (annotated)

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