UKESF Headstart Summer School
/
PsiSwarm-Headstart
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
PsiSwarmLibrary
by 
James Hilder
Diff: demo.cpp
- Revision:
- 0:d6269d17c8cf
- Child:
- 2:c6986ee3c7c5
diff -r 000000000000 -r d6269d17c8cf demo.cpp
--- /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