UKESF Headstart Summer School
Modified version of the UKESF lab source which can be carried out with no knowledge of C
Fork of
PsiSwarm-Headstart
by 
UKESF Headstart Summer School
Diff: demo.cpp
- Revision:
- 0:d6269d17c8cf
- Child:
- 2:c6986ee3c7c5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/demo.cpp Thu Feb 04 21:48:54 2016 +0000
@@ -0,0 +1,1355 @@
+/* University of York Robotics Laboratory PsiSwarm Library: Demo Mode Source File
+ *
+ * File: demo.cpp
+ *
+ * (C) Dept. Electronics & Computer Science, University of York
+ * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
+ *
+ * PsiSwarm Library Version: 0.4
+ *
+ * Version 0.4 : Added PsiSwarmBasic menu
+ * Version 0.2 : Remove most the functionality from center-button push to allow all operations to be accessable from
+ * four directions alone.
+ * Added extra sensor information, added various testing demos
+ *
+ * February 2016
+ *
+ */
+
+
+#include "psiswarm.h"
+
+// PID terms
+#define LF_P_TERM 0.2
+#define LF_I_TERM 0
+#define LF_D_TERM 4
+
+char top_menu = 0;
+char sub_menu = 0;
+char level = 0;
+char started = 0;
+char topline[17];
+char bottomline[17];
+char led_state[9];
+char all_led_state = 0;
+char base_led_state = 0;
+char brightness = 20;
+char bl_brightness = 100;
+char base_ir_index = 0;
+char side_ir_index = 0;
+signed short left_speed = 0;
+signed short right_speed = 0;
+char both_motor_mode = 0;
+char last_switch_pressed;
+Timeout demo_event;
+char handling_event = 0;
+
+Timeout demo_timeout;
+char demo_running = 0;
+Timer demo_timer;
+float time_out;
+float speed;
+char state;
+char led_step = 0;
+char spin_step = 0;
+char stress_step = 0;
+
+
+float lf_right;
+float lf_left;
+float lf_current_pos_of_line = 0.0;
+float lf_previous_pos_of_line = 0.0;
+float lf_derivative,lf_proportional,lf_integral = 0;
+float lf_power;
+float lf_speed = 0.4;
+
+
+
+void demo_mode()
+{
+ debug("- Starting Demo Mode\n");
+ if(use_flash_basic == 1) top_menu = 7;
+ demo_on = 1;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ display.clear_display();
+ display.write_string("PSI SWARM SYSTEM");
+ display.set_position(1,0);
+ display.write_string(" DEMO MODE");
+ wait(0.5);
+ display.clear_display();
+ display.write_string("Use cursor to");
+ display.set_position(1,0);
+ display.write_string("navigate menus");
+ char step = 0;
+ while(demo_on) {
+ if(demo_running == 0) {
+ switch(step) {
+ case 0:
+ mbed_led1 = 1;
+ mbed_led2 = 0;
+ break;
+ case 1:
+ mbed_led2 = 1;
+ mbed_led1 = 0;
+ break;
+ }
+ step++;
+ if(step==2)step=0;
+ } else {
+ mbed_led1 = 0;
+ mbed_led2 = 0;
+ mbed_led3 = 0;
+ mbed_led4 = 0;
+ }
+ wait(0.5);
+ }
+ debug("- Demo mode ended\n");
+}
+
+void demo_handle_switch_event(char switch_pressed)
+{
+ if(!handling_event) {
+ handling_event = 1;
+ last_switch_pressed = switch_pressed;
+ demo_event.attach_us(&demo_event_thread, 1000);
+ }
+}
+
+void demo_event_thread()
+{
+ handling_event = 0;
+ if(started == 1) {
+ switch(last_switch_pressed) {
+ case 1: //Up pressed
+ switch(level) {
+ case 0:
+ if(top_menu != 6) {
+ level++;
+ sub_menu = 0;
+ } else {
+ demo_on = 0;
+ user_code_running = user_code_restore_mode;
+ }
+ break;
+ case 1:
+ switch(top_menu) {
+ case 7: // PsiBasic Menu
+ if(sub_menu == psi_basic_file_count) level = 0;
+ break;
+ case 0: //LED Menu
+ if(sub_menu < 9) {
+ if(led_state[sub_menu] == 0) led_state[sub_menu] = 3;
+ else led_state[sub_menu]--;
+ demo_update_leds();
+ }
+ if(sub_menu == 9) {
+ if(all_led_state == 0) all_led_state = 3;
+ else all_led_state--;
+ for(int i=0; i<9; i++) {
+ led_state[i]=all_led_state;
+ }
+ demo_update_leds();
+ }
+ if(sub_menu == 10) {
+ base_led_state = 1 - base_led_state;
+ demo_update_leds();
+ }
+ if(sub_menu == 11) {
+ switch(brightness) {
+ case 100:
+ brightness = 50;
+ break;
+ case 2:
+ brightness = 1;
+ break;
+ case 5:
+ brightness = 2;
+ break;
+ case 10:
+ brightness = 5;
+ break;
+ case 20:
+ brightness = 10;
+ break;
+ case 50:
+ brightness = 20;
+ break;
+ }
+ demo_update_leds();
+ }
+ if(sub_menu == 12) {
+ if(bl_brightness > 0) bl_brightness-=10;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ }
+ if(sub_menu == 13) level = 0;
+ break;
+ case 1: // Sensors Menu
+ if(sub_menu == 4 || sub_menu == 5) {
+ if(base_ir_index == 0) base_ir_index = 4;
+ else base_ir_index --;
+ }
+ if(sub_menu > 5 && sub_menu < 9) {
+ if(side_ir_index == 0) side_ir_index = 7;
+ else side_ir_index --;
+ }
+ if(sub_menu == 11) level = 0;
+ break;
+ case 2: // Motor Menu
+ if(sub_menu == 0) {
+ left_speed += 5;
+ if(left_speed > 100) left_speed = 100;
+ set_left_motor_speed(left_speed / 100.0f);
+ }
+ if(sub_menu == 1) {
+ right_speed += 5;
+ if(right_speed > 100) right_speed = 100;
+ set_right_motor_speed(right_speed / 100.0f);
+ }
+ if(sub_menu == 2) {
+ if(both_motor_mode == 0) both_motor_mode=5;
+ else both_motor_mode--;
+ switch(both_motor_mode) {
+ case 0:
+ stop();
+ break;
+ case 1:
+ brake();
+ break;
+ case 2:
+ forward(0.5);
+ break;
+ case 3:
+ forward(1);
+ break;
+ case 4:
+ backward(0.5);
+ break;
+ case 5:
+ backward(1.0);
+ break;
+ }
+ }
+ if(sub_menu == 3) {
+ level = 0;
+ }
+ break;
+ case 3: // Radio Menu
+ if(sub_menu == 0) level = 0;
+ break;
+ case 4: // Info Menu
+ if(sub_menu == 6) level = 0;
+ break;
+ case 5: // Demo Menu
+ if(sub_menu == 0) {
+ if(demo_running == 0) {
+ start_line_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ stop();
+ }
+ }
+ if(sub_menu == 1) {
+ if(demo_running == 0) {
+ start_obstacle_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ stop();
+ }
+ }
+ if(sub_menu == 2) {
+ if(demo_running == 0) {
+ start_spinning_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ stop();
+ }
+ }
+ if(sub_menu == 3) {
+ if(demo_running == 0) {
+ start_stress_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ stop();
+ }
+ }
+ if(sub_menu == 4) level = 0;
+ break;
+ }
+ break;
+ }
+ break;
+ case 2: //Down pressed
+ switch(level) {
+ case 0:
+ if(top_menu != 6) {
+ level++;
+ sub_menu = 0;
+ } else {
+ demo_on = 0;
+ user_code_running = user_code_restore_mode;
+ }
+ break;
+ case 1:
+ switch(top_menu) {
+ case 7: // PsiBasic Menu
+ if(sub_menu == psi_basic_file_count) level = 0;
+ break;
+ case 0: //LED Menu
+ if(sub_menu < 9) {
+ led_state[sub_menu]++;
+ if(led_state[sub_menu] == 4) led_state[sub_menu] = 0;
+ demo_update_leds();
+ }
+ if(sub_menu == 9) {
+ all_led_state++;
+ if(all_led_state == 4) all_led_state = 0;
+ for(int i=0; i<9; i++) {
+ led_state[i]=all_led_state;
+ }
+ demo_update_leds();
+ }
+ if(sub_menu == 10) {
+ base_led_state = 1 - base_led_state;
+ demo_update_leds();
+ }
+ if(sub_menu == 11) {
+ switch(brightness) {
+ case 1:
+ brightness = 2;
+ break;
+ case 2:
+ brightness = 5;
+ break;
+ case 5:
+ brightness = 10;
+ break;
+ case 10:
+ brightness = 20;
+ break;
+ case 20:
+ brightness = 50;
+ break;
+ case 50:
+ brightness = 100;
+ break;
+ }
+ demo_update_leds();
+ }
+
+ if(sub_menu == 12) {
+ if(bl_brightness < 100) bl_brightness+=10;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ }
+ if(sub_menu == 13) level = 0;
+
+ break;
+ case 1: // Sensors Menu
+ if(sub_menu == 4 || sub_menu == 5) {
+ if(base_ir_index == 4) base_ir_index = 0;
+ else base_ir_index ++;
+ }
+ if(sub_menu > 5 && sub_menu < 9) {
+ if(side_ir_index == 7) side_ir_index = 0;
+ else side_ir_index ++;
+ }
+ if(sub_menu == 11) level = 0;
+ break;
+ case 2: // Motor Menu
+ if(sub_menu == 0) {
+ left_speed -= 5;
+ if(left_speed < -100) left_speed = -100;
+ set_left_motor_speed(left_speed / 100.0f);
+ }
+ if(sub_menu == 1) {
+ right_speed -= 5;
+ if(right_speed < -100) right_speed = -100;
+ set_right_motor_speed(right_speed / 100.0f);
+ }
+ if(sub_menu == 2) {
+ both_motor_mode++;
+ if(both_motor_mode == 6) both_motor_mode=0;
+ switch(both_motor_mode) {
+ case 0:
+ stop();
+ break;
+ case 1:
+ brake();
+ break;
+ case 2:
+ forward(0.5);
+ break;
+ case 3:
+ forward(1);
+ break;
+ case 4:
+ backward(0.5);
+ break;
+ case 5:
+ backward(1.0);
+ break;
+ }
+ }
+ if(sub_menu == 3) {
+ level = 0;
+ }
+ break;
+ case 3: // Radio Menu
+ if(sub_menu == 0) level = 0;
+ break;
+ case 4: // Info Menu
+ if(sub_menu == 6) level = 0;
+ break;
+ case 5: // Demo Menu
+ if(sub_menu == 0) {
+ if(demo_running == 0) {
+ start_line_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ stop();
+ }
+ }
+ if(sub_menu == 1) {
+ if(demo_running == 0) {
+ start_obstacle_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ stop();
+
+ }
+ }
+ if(sub_menu == 2) {
+ if(demo_running == 0) {
+ start_spinning_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ stop();
+ }
+ }
+ if(sub_menu == 3) {
+ if(demo_running == 0) {
+ start_stress_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ stop();
+ }
+ }
+ if(sub_menu == 4) level = 0;
+ break;
+ }
+ break;
+ }
+ break;
+ case 4: //Left pressed
+ switch(level) {
+ case 0:
+ if(top_menu == 0) {
+ top_menu = 6;
+ if(use_flash_basic == 1) top_menu = 7;
+ }
+ else top_menu --;
+ break;
+ case 1:
+ switch(top_menu) {
+ case 0: //LED Menu
+ if(sub_menu == 0) sub_menu = 13;
+ else sub_menu --;
+ break;
+ case 1: //Sensors Menu
+ if(sub_menu == 0) sub_menu = 11;
+ else sub_menu --;
+ break;
+
+ case 2: //Motor Menu
+ if(sub_menu == 0) sub_menu = 3;
+ else sub_menu --;
+ break;
+ case 4: //Info Menu
+ if(sub_menu == 0) sub_menu = 6;
+ else sub_menu --;
+ break;
+ case 5: //Demo Menu
+ if(sub_menu == 0) sub_menu = 4;
+ else sub_menu --;
+ break;
+ case 7: //PsiBasic Menu
+ if(sub_menu == 0) sub_menu = psi_basic_file_count;
+ else sub_menu --;
+ }
+ break;
+
+ }
+ break;
+ case 8: //Right pressed
+ switch(level) {
+ case 0:
+ top_menu ++;
+ if((top_menu - use_flash_basic) > 6) top_menu = 0;
+ break;
+ case 1:
+ switch(top_menu) {
+ case 0: //LED Menu
+ if(sub_menu == 13) sub_menu = 0;
+ else sub_menu ++;
+ break;
+ case 1: //Sensors Menu
+ if(sub_menu == 11) sub_menu = 0;
+ else sub_menu ++;
+ break;
+ case 2: //Motor Menu
+ if(sub_menu == 3) sub_menu = 0;
+ else sub_menu ++;
+ break;
+ case 4: //Info Menu
+ if(sub_menu == 6) sub_menu = 0;
+ else sub_menu ++;
+ break;
+ case 5: //Demo Menu
+ if(sub_menu == 4) sub_menu = 0;
+ else sub_menu ++;
+ break;
+ case 7: //PsiBasic Menu
+ if(sub_menu == psi_basic_file_count) sub_menu = 0;
+ else sub_menu ++;
+ }
+ break;
+ }
+ break;
+ case 16: //Select pressed
+ switch(level) {
+ case 0:
+ if(top_menu != 6) {
+ level++;
+ sub_menu = 0;
+ } else {
+ demo_on = 0;
+ user_code_running = user_code_restore_mode;
+ }
+ break;
+ case 1:
+ switch(top_menu) {
+ case 0: //LED Menu
+ if(sub_menu == 9) {
+ for(int i=0; i<9; i++) {
+ led_state[i]=all_led_state;
+ }
+ demo_update_leds();
+ }
+ if(sub_menu == 13) level = 0;
+ break;
+ case 1: // Sensors Menu
+ if(sub_menu == 11) level = 0;
+ break;
+ case 2: //Motor Menu
+ if(sub_menu == 2) {
+ switch(both_motor_mode) {
+ case 0:
+ stop();
+ break;
+ case 1:
+ brake();
+ break;
+ case 2:
+ forward(0.5);
+ break;
+ case 3:
+ forward(1);
+ break;
+ case 4:
+ backward(0.5);
+ break;
+ case 5:
+ backward(1.0);
+ break;
+ }
+ }
+ if(sub_menu == 3) {
+ level = 0;
+ }
+ break;
+ case 3: // Radio Menu
+ if(sub_menu == 0) level = 0;
+ break;
+ case 4: // Info Menu
+ if(sub_menu == 6) level = 0;
+ break;
+ case 5: // Demo Menu
+ if(sub_menu == 4) level = 0;
+ break;
+ case 7: // PsiBasic Menu
+ if(sub_menu == psi_basic_file_count) level = 0;
+ break;
+ }
+ break;
+ }
+ break;
+ }
+ } else started = 1;
+ display.clear_display();
+ switch(level) {
+ case 0:
+ //Top level menu
+ switch(top_menu) {
+ case 0:
+ strcpy(topline,"---TEST LEDS----");
+ break;
+ case 1:
+ strcpy(topline,"--TEST SENSORS--");
+ break;
+ case 2:
+ strcpy(topline,"--TEST MOTORS---");
+ break;
+ case 3:
+ strcpy(topline,"---TEST RADIO---");
+ break;
+ case 4:
+ strcpy(topline,"------INFO------");
+ break;
+ case 5:
+ strcpy(topline,"---CODE DEMOS---");
+ break;
+ case 6:
+ strcpy(topline,"------EXIT------");
+ break;
+ case 7:
+ strcpy(topline,"---PSI BASIC----");
+ break;
+ }
+ strcpy(bottomline,"");
+ break;
+ case 1:
+ //Sub level menu
+ switch(top_menu) {
+ case 7:
+ strcpy(topline,"-PSI BASIC MENU-");
+ if(sub_menu == psi_basic_file_count) strcpy(bottomline,"EXIT");
+ else strcpy(bottomline,basic_filenames.at(sub_menu).c_str());
+ break;
+ case 0:
+ strcpy(topline,"----LED MENU----");
+ char led_status[7];
+ if(sub_menu<9) {
+ switch(led_state[sub_menu]) {
+ case 0:
+ strcpy(led_status,"OFF");
+ break;
+ case 1:
+ strcpy(led_status,"RED");
+ break;
+ case 2:
+ strcpy(led_status,"GREEN");
+ break;
+ case 3:
+ strcpy(led_status,"YELLOW");
+ break;
+ }
+ }
+ if(sub_menu < 8) sprintf(bottomline,"OUTER %d: %s",(sub_menu + 1),led_status);
+ if(sub_menu == 8) sprintf(bottomline,"CENTER: %s",led_status);
+ if(sub_menu == 9) {
+ switch(all_led_state) {
+ case 0:
+ strcpy(led_status,"OFF");
+ break;
+ case 1:
+ strcpy(led_status,"RED");
+ break;
+ case 2:
+ strcpy(led_status,"GREEN");
+ break;
+ case 3:
+ strcpy(led_status,"YELLOW");
+ break;
+ }
+ sprintf(bottomline,"SET ALL %s", led_status);
+ }
+ if(sub_menu == 10) {
+ if(base_led_state == 0) strcpy(led_status,"OFF");
+ else strcpy(led_status,"ON");
+ sprintf(bottomline,"BASE: %s",led_status);
+ }
+ if(sub_menu == 11) sprintf(bottomline,"CLED BNESS %d%%", brightness);
+ if(sub_menu == 12) sprintf(bottomline,"DISP BNESS %d%%", bl_brightness);
+ if(sub_menu == 13) strcpy(bottomline,"EXIT");
+ break;
+
+ case 1:
+ strcpy(topline,"--SENSORS MENU--");
+ switch(sub_menu) {
+ case 0: {
+ float battery = get_battery_voltage ();
+ sprintf(bottomline,"BATTERY: %1.3fV",battery);
+ break;
+ }
+ case 1: {
+ float dc = get_dc_voltage ();
+ sprintf(bottomline,"DC: %1.3fV",dc);
+ break;
+ }
+ case 2: {
+ float current = get_current ();
+ sprintf(bottomline,"CURRENT: %1.3fA",current);
+ break;
+ }
+ case 3: {
+ float temperature = get_temperature();
+ sprintf(bottomline,"TEMP: %3.2fC", temperature);
+ break;
+ }
+ case 4:
+ store_background_base_ir_values();
+ sprintf(bottomline,"BIR%dB:%d",base_ir_index+1,get_background_base_ir_value(base_ir_index));
+ break;
+ case 5:
+ store_illuminated_base_ir_values();
+ sprintf(bottomline,"BIR%dR:%d",base_ir_index+1,get_illuminated_base_ir_value(base_ir_index));
+ break;
+ case 6:
+ store_background_raw_ir_values();
+ sprintf(bottomline,"SIR%dB:%d",side_ir_index+1,get_background_raw_ir_value(side_ir_index));
+ break;
+ case 7:
+ store_illuminated_raw_ir_values();
+ sprintf(bottomline,"SIR%dR:%d",side_ir_index+1,get_illuminated_raw_ir_value(side_ir_index));
+ break;
+ case 8:
+ sprintf(bottomline,"SIR%dD:%3.1f",side_ir_index+1,0.0);
+ break;
+ case 9:
+ if(ultrasonic_distance_updated == 1) {
+ sprintf(bottomline,"USONIC:%3dcm",ultrasonic_distance);
+ } else sprintf(bottomline,"USONIC:---------");
+ update_ultrasonic_measure();
+ break;
+ case 10:
+ store_line_position();
+ if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position);
+ else sprintf(bottomline,"LINE:---------");
+ break;
+ case 11:
+ sprintf(bottomline,"EXIT");
+ break;
+ }
+ break;
+ case 2:
+ strcpy(topline,"--MOTORS MENU---");
+ switch(sub_menu) {
+ case 0:
+ sprintf(bottomline,"LEFT: %d%%", left_speed);
+ break;
+ case 1:
+ sprintf(bottomline,"RIGHT: %d%%", right_speed);
+ break;
+ case 2:
+ char both_mode_string[16];
+ switch(both_motor_mode) {
+ case 0:
+ strcpy(both_mode_string,"OFF");
+ break;
+ case 1:
+ strcpy(both_mode_string,"BRAKE");
+ break;
+ case 2:
+ strcpy(both_mode_string,"+50%");
+ break;
+ case 3:
+ strcpy(both_mode_string,"+100%");
+ break;
+ case 4:
+ strcpy(both_mode_string,"-50%");
+ break;
+ case 5:
+ strcpy(both_mode_string,"-100%");
+ break;
+ }
+ sprintf(bottomline,"BOTH TO %s",both_mode_string);
+ break;
+ case 3:
+ sprintf(bottomline,"EXIT");
+ break;
+ }
+ break;
+ case 3:
+ strcpy(topline,"---RADIO MENU---");
+ switch(sub_menu) {
+
+ case 0:
+ sprintf(bottomline,"EXIT");
+ break;
+ }
+ break;
+ case 4:
+ strcpy(topline,"---INFO MENU----");
+ switch(sub_menu) {
+ case 0:
+ sprintf(bottomline,"ROBOT ID: %d",robot_id);
+ break;
+ case 1:
+ sprintf(bottomline,"SOFTWARE: %1.2f",SOFTWARE_VERSION_CODE);
+ break;
+ case 2:
+ if(firmware_version > 0) sprintf(bottomline,"FIRMWARE: %1.2f",firmware_version);
+ else sprintf(bottomline,"FIRMWARE: ?????");
+ break;
+ case 3:
+ sprintf(bottomline,"PROG:%s",program_name);
+ break;
+ case 4:
+ sprintf(bottomline,"AUTH:%s",author_name);
+ break;
+ case 5:
+ sprintf(bottomline,"VER:%s",version_name);
+ break;
+ case 6:
+ sprintf(bottomline,"EXIT");
+ break;
+ }
+ break;
+ case 5:
+ strcpy(topline,"---CODE DEMOS---");
+ switch(sub_menu) {
+ case 0:
+ sprintf(bottomline,"LINE FOLLOW");
+ break;
+ case 1:
+ sprintf(bottomline,"OBST. AVOID");
+ break;
+ case 2:
+ sprintf(bottomline,"COLOUR SPIN");
+ break;
+ case 3:
+ sprintf(bottomline,"STRESS TEST");
+ break;
+ case 4:
+ sprintf(bottomline,"EXIT");
+ break;
+ }
+ break;
+ case 6:
+ strcpy(topline,"");
+ break;
+ }
+ break;
+ }
+ display.write_string(topline);
+ display.set_position(1,0);
+ display.write_string(bottomline);
+ if(top_menu == 1 && level == 1) {
+ demo_event.attach(&demo_event_thread, 0.25);
+ }
+}
+
+void start_line_demo()
+{
+ display.set_backlight_brightness(0);
+ time_out = 0.25f;
+ demo_timer.start();
+ state = 0;
+ speed = 0;
+ led_step = 0;
+ demo_running = 1;
+ demo_timeout.attach_us(&line_demo_cycle,1000);
+}
+
+void start_obstacle_demo()
+{
+ display.set_backlight_brightness(0);
+ time_out = 0.25f;
+ demo_timer.start();
+ state = 0;
+ speed = 0;
+ led_step = 0;
+ demo_running = 1;
+ demo_timeout.attach_us(&obstacle_demo_cycle,1000);
+}
+
+void start_stress_demo()
+{
+ display.set_backlight_brightness(0.25);
+ display.write_string("STRESS TEST");
+ display.set_position(1,0);
+ display.write_string("----25%----");
+ time_out = 0.04f;
+ demo_timer.start();
+ state = 0;
+ speed = 0;
+ stress_step = 0;
+ spin_step = 0;
+ demo_running = 1;
+ demo_timeout.attach_us(&stress_demo_cycle,1000);
+}
+
+void start_spinning_demo()
+{
+ display.set_backlight_brightness(0);
+ time_out = 0.0f;
+ demo_timer.start();
+ state = 0;
+ speed = 0;
+ led_step = 0;
+ spin_step = 0;
+ demo_running = 1;
+ demo_timeout.attach_us(&spinning_demo_cycle,1000);
+}
+
+void line_demo_cycle()
+{
+ if(demo_timer.read() > time_out) {
+ store_line_position();
+ if(line_found) {
+ time_out = 0.01f;
+ state = 0;
+ // Get the position of the line.
+ lf_current_pos_of_line = line_position;
+ lf_proportional = lf_current_pos_of_line;
+
+ // Compute the derivative
+ lf_derivative = lf_current_pos_of_line - lf_previous_pos_of_line;
+
+ // Compute the integral
+ lf_integral += lf_proportional;
+
+ // Remember the last position.
+ lf_previous_pos_of_line = lf_current_pos_of_line;
+
+ // Compute the power
+ lf_power = (lf_proportional * (LF_P_TERM) ) + (lf_integral*(LF_I_TERM)) + (lf_derivative*(LF_D_TERM)) ;
+
+ // Compute new speeds
+ lf_right = lf_speed-lf_power;
+ lf_left = lf_speed+lf_power;
+
+ // limit checks
+ if (lf_right < 0)
+ lf_right = 0;
+ else if (lf_right > 1.0f)
+ lf_right = 1.0f;
+
+ if (lf_left < 0)
+ lf_left = 0;
+ else if (lf_left > 1.0f)
+ lf_left = 1.0f;
+ }else{
+ //Cannot see line: hunt for it
+ if(lf_left > lf_right){
+ //Currently turning left, keep turning left
+ state ++;
+ float d_step = state * 0.04;
+ lf_left = 0.2 + d_step;
+ lf_right = -0.2 - d_step;
+ if(state > 20){
+ state = 0;
+ lf_right = 0.2;
+ lf_left = -0.2;
+ time_out += 0.01f;
+ if(time_out > 0.1f) demo_running = 0;
+ }
+ }else{
+ //Currently turning right, keep turning right
+ state ++;
+ float d_step = state * 0.04;
+ lf_left = -0.2 - d_step;
+ lf_right = 0.2 + d_step;
+ if(state > 20){
+ state = 0;
+ lf_right = -0.2;
+ lf_left = 0.2;
+ time_out += 0.01f;
+ if(time_out > 0.1f) demo_running = 0;
+ }
+ }
+ }
+ // set speed
+ set_left_motor_speed(lf_left);
+ set_right_motor_speed(lf_right);
+
+
+ demo_timer.reset();
+ }
+ if(demo_running == 1)demo_timeout.attach_us(&line_demo_cycle,500);
+ else {
+ stop();
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ }
+}
+
+void stress_demo_cycle()
+{
+ if(demo_timer.read() > time_out) {
+ float pct = 0.25 + (0.25 * stress_step);
+ switch(state) {
+ case 0:
+ if(spin_step % 2 == 0) {
+ forward(pct);
+ set_leds(0xFF,0xFF);
+ } else {
+ backward(pct);
+ set_leds(0,0xFF);
+ }
+ spin_step ++;
+ if(spin_step > 199) {
+ state ++;
+ spin_step = 0;
+ }
+ break;
+ case 1:
+ if(stress_step < 3) stress_step ++;
+ float pct = 0.25 + (0.25 * stress_step);
+ display.set_backlight_brightness(pct);
+ display.set_position(1,0);
+ switch(stress_step) {
+ case 1:
+ display.write_string("----50%----");
+ break;
+ case 2:
+ display.write_string("----75%----");
+ break;
+ case 3:
+ display.write_string("---100%----");
+ break;
+ }
+ state = 0;
+ break;
+ }
+ demo_timer.reset();
+ }
+ if(demo_running == 1)demo_timeout.attach_us(&stress_demo_cycle,500);
+ else {
+ stop();
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ }
+}
+
+void spinning_demo_cycle()
+{
+ if(demo_timer.read() > time_out) {
+ switch(state) {
+ case 0: //Robot is stopped
+ set_leds(0,0xFF);
+ set_center_led(1,1);
+ speed = 0.1f;
+ brake();
+ time_out = 0.5;
+ state = 1;
+ led_step = 0;
+ break;
+ case 1: //Motor is turning right, accelerating
+ time_out = 0.1;
+ set_center_led(2,1);
+ switch(led_step) {
+ case 0:
+ set_leds(0x01,0);
+ break;
+ case 1:
+ set_leds(0x02,0);
+ break;
+ case 2:
+ set_leds(0x04,0);
+ break;
+ case 3:
+ set_leds(0x08,0);
+ break;
+ case 4:
+ set_leds(0x10,0);
+ break;
+ case 5:
+ set_leds(0x20,0);
+ break;
+ case 6:
+ set_leds(0x40,0);
+ break;
+ case 7:
+ set_leds(0x80,0);
+ break;
+ }
+ led_step ++;
+ if(led_step == 8) led_step =0;
+ if(speed < 1) {
+ speed += 0.0125;
+ turn(speed);
+ } else {
+ state = 2;
+ spin_step = 0;
+ led_step =0;
+ }
+ break;
+ case 2: //Motor is turning right, full speed
+ set_center_led(3,1);
+ switch(led_step) {
+ case 0:
+ set_leds(0x33,0x33);
+ break;
+ case 1:
+ set_leds(0x66,0x66);
+ break;
+ case 2:
+ set_leds(0xCC,0xCC);
+ break;
+ case 3:
+ set_leds(0x99,0x99);
+ break;
+ }
+ led_step ++;
+ if(led_step == 4) led_step = 0;
+ spin_step ++;
+ if(spin_step == 40) {
+ state = 3;
+ led_step = 0;
+ }
+ break;
+ case 3: //Motor is turning right, decelerating
+ set_center_led(2,1);
+ switch(led_step) {
+ case 0:
+ set_leds(0x01,0);
+ break;
+ case 1:
+ set_leds(0x02,0);
+ break;
+ case 2:
+ set_leds(0x04,0);
+ break;
+ case 3:
+ set_leds(0x08,0);
+ break;
+ case 4:
+ set_leds(0x10,0);
+ break;
+ case 5:
+ set_leds(0x20,0);
+ break;
+ case 6:
+ set_leds(0x40,0);
+ break;
+ case 7:
+ set_leds(0x80,0);
+ break;
+ }
+ if(led_step == 0) led_step =8;
+ led_step --;
+ if(speed > 0.1) {
+ speed -= 0.025;
+ turn(speed);
+ } else {
+ state = 4;
+ spin_step = 0;
+ led_step =0;
+ }
+ break;
+ case 4: //Robot is stopped
+ set_leds(0,0xFF);
+ set_center_led(1,1);
+ speed = 0.1f;
+ brake();
+ time_out = 0.5;
+ led_step =0;
+ state = 5;
+ break;
+ case 5: //Motor is turning left, accelerating
+ time_out = 0.1;
+ set_center_led(2,1);
+ switch(led_step) {
+ case 0:
+ set_leds(0x01,0);
+ break;
+ case 1:
+ set_leds(0x02,0);
+ break;
+ case 2:
+ set_leds(0x04,0);
+ break;
+ case 3:
+ set_leds(0x08,0);
+ break;
+ case 4:
+ set_leds(0x10,0);
+ break;
+ case 5:
+ set_leds(0x20,0);
+ break;
+ case 6:
+ set_leds(0x40,0);
+ break;
+ case 7:
+ set_leds(0x80,0);
+ break;
+ }
+ if(led_step == 0) led_step =8;
+ led_step --;
+ if(speed < 1) {
+ speed += 0.0125;
+ turn(-speed);
+ } else {
+ state = 6;
+ spin_step = 0;
+ led_step = 0;
+ }
+ break;
+ case 6: //Motor is turning left, full speed
+ set_center_led(3,1);
+ switch(led_step) {
+ case 0:
+ set_leds(0x33,0x33);
+ break;
+ case 1:
+ set_leds(0x66,0x66);
+ break;
+ case 2:
+ set_leds(0xCC,0xCC);
+ break;
+ case 3:
+ set_leds(0x99,0x99);
+ break;
+ }
+ if(led_step == 0) led_step = 4;
+ led_step --;
+ spin_step ++;
+ if(spin_step == 40) {
+ state = 7;
+ led_step = 0;
+ }
+ break;
+ case 7: //Motor is turning left, decelerating
+ set_center_led(2,1);
+ switch(led_step) {
+ case 0:
+ set_leds(0x01,0);
+ break;
+ case 1:
+ set_leds(0x02,0);
+ break;
+ case 2:
+ set_leds(0x04,0);
+ break;
+ case 3:
+ set_leds(0x08,0);
+ break;
+ case 4:
+ set_leds(0x10,0);
+ break;
+ case 5:
+ set_leds(0x20,0);
+ break;
+ case 6:
+ set_leds(0x40,0);
+ break;
+ case 7:
+ set_leds(0x80,0);
+ break;
+ }
+ led_step ++;
+ if(led_step == 8) led_step =0;
+ if(speed > 0.1) {
+ speed -= 0.025;
+ turn(-speed);
+ } else {
+ state = 0;
+ spin_step = 0;
+ led_step = 0;
+ }
+ break;
+ }
+ demo_timer.reset();
+ }
+ if(demo_running == 1)demo_timeout.attach_us(&spinning_demo_cycle,500);
+ else {
+ stop();
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ }
+}
+
+void obstacle_demo_cycle()
+{
+ if(demo_timer.read() > time_out) {
+ switch(state) {
+ case 0: //Robot is stopped
+ set_leds(0,0xFF);
+ set_center_led(1,0.4);
+ speed = 0.3f;
+ forward(speed);
+ time_out = 0.05;
+ state = 1;
+ break;
+ case 1: { //Motor is moving forward
+ store_ir_values();
+ int front_right = read_illuminated_raw_ir_value(0);
+ int front_left = read_illuminated_raw_ir_value(7);
+ if(front_left > 400 || front_right > 400) {
+ brake();
+ time_out = 0.04;
+ if(front_left > front_right)state=2;
+ else state=3;
+ } else {
+ if(speed < 0.5) {
+ speed += 0.03;
+ forward(speed);
+ }
+ switch(led_step) {
+ case 0:
+ set_leds(0x01,0);
+ break;
+ case 1:
+ set_leds(0x38,0);
+ break;
+ case 2:
+ set_leds(0x6C,0);
+ break;
+ case 3:
+ set_leds(0xC6,0);
+ break;
+ case 4:
+ set_leds(0x83,0);
+ break;
+ }
+ set_center_led(2, 0.6);
+ led_step ++;
+ if(led_step == 5) led_step = 0;
+ }
+ break;
+ }
+ case 2: //Turn right
+ set_left_motor_speed(0.85);
+ set_right_motor_speed(-0.85);
+ time_out = 0.4;
+ state = 0;
+ set_leds(0x0E,0x0E);
+ set_center_led(3,0.5);
+ break;
+ case 3: //Turn left
+ set_left_motor_speed(-0.85);
+ set_right_motor_speed(0.85);
+ time_out = 0.4;
+ state = 0;
+ set_leds(0xE0,0xE0);
+ set_center_led(3,0.5);
+ break;
+ }
+ demo_timer.reset();
+ }
+ if(demo_running == 1)demo_timeout.attach_us(&obstacle_demo_cycle,200);
+ else {
+ stop();
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ }
+}
+
+void demo_update_leds()
+{
+ char red = 0;
+ char green = 0;
+ for(int i=0; i<8; i++) {
+ if(led_state[i]==1 || led_state[i]==3) red+=(1<<i);
+ if(led_state[i]==2 || led_state[i]==3) green+=(1<<i);
+ }
+ set_leds(green,red);
+ float brightness_f = brightness / 100.0f;
+ set_center_led(led_state[8], brightness_f);
+ set_base_led(base_led_state);
+}
\ No newline at end of file