James Hilder
Psi Swarm robot library version 0.9
Fork of
PsiSwarmV9
by 
Psi Swarm Robot
Revision 20:1bc6c6dc477b, committed 2018-05-14
- Comitter:
- jah128
- Date:
- Mon May 14 15:35:38 2018 +0000
- Parent:
- 19:3e3b03d80ea3
- Commit message:
- Updated?
Changed in this revision
| demo.cpp | Show annotated file Show diff for this revision Revisions of this file |
| demo.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 3e3b03d80ea3 -r 1bc6c6dc477b demo.cpp
--- a/demo.cpp Mon Jun 05 22:47:14 2017 +0000
+++ b/demo.cpp Mon May 14 15:35:38 2018 +0000
@@ -501,6 +501,7 @@
case 4: //Left pressed
switch(level) {
case 0:
+ stop_demo();
if(top_menu == 0) {
top_menu = 9;
} else {
@@ -529,6 +530,7 @@
else sub_menu --;
break;
case 6: //Demo Menu
+ stop_demo();
if(sub_menu == 0) sub_menu = 5;
else sub_menu --;
break;
@@ -547,6 +549,7 @@
case 8: //Right pressed
switch(level) {
case 0:
+ stop_demo();
top_menu ++;
if(top_menu == 8 && use_flash_basic == 0) top_menu = 9;
if(has_433_radio == 0 && top_menu == 4) top_menu = 5;
@@ -571,6 +574,7 @@
else sub_menu ++;
break;
case 6: //Demo Menu
+ stop_demo();
if(sub_menu == 5) sub_menu = 0;
else sub_menu ++;
break;
@@ -587,286 +591,288 @@
break;
}
} else started = 1;
- display.clear_display();
- switch(level) {
- case 0:
- //Top level menu
- switch(top_menu) {
- case 0:
- strcpy(topline,"---QUICK TEST---");
- break;
- case 1:
- strcpy(topline,"---TEST LEDS----");
- break;
- case 2:
- strcpy(topline,"--TEST SENSORS--");
- break;
- case 3:
- strcpy(topline,"--TEST MOTORS---");
- break;
- case 4:
- strcpy(topline,"---TEST RADIO---");
- break;
- case 5:
- strcpy(topline,"------INFO------");
- break;
- case 6:
- strcpy(topline,"---CODE DEMOS---");
- break;
- case 7:
- strcpy(topline,"---CALIBRATION--");
- break;
- case 8:
- strcpy(topline,"---PSI BASIC----");
- break;
- case 9:
- strcpy(topline,"------EXIT------");
- break;
- }
- strcpy(bottomline,"");
- break;
- case 1:
- //Sub level menu
- switch(top_menu) {
- case 8:
- 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 1:
- strcpy(topline,"----LED MENU----");
- char led_status[7];
- if(sub_menu<9) {
- switch(led_state[sub_menu]) {
- case 0:
- strcpy(led_status,"OFF");
+ if(demo_running == 0) {
+ display.clear_display();
+ switch(level) {
+ case 0:
+ //Top level menu
+ switch(top_menu) {
+ case 0:
+ strcpy(topline,"---QUICK TEST---");
+ break;
+ case 1:
+ strcpy(topline,"---TEST LEDS----");
+ break;
+ case 2:
+ strcpy(topline,"--TEST SENSORS--");
+ break;
+ case 3:
+ strcpy(topline,"--TEST MOTORS---");
+ break;
+ case 4:
+ strcpy(topline,"---TEST RADIO---");
+ break;
+ case 5:
+ strcpy(topline,"------INFO------");
+ break;
+ case 6:
+ strcpy(topline,"-----DEMOS------");
+ break;
+ case 7:
+ strcpy(topline,"---CALIBRATION--");
+ break;
+ case 8:
+ strcpy(topline,"---PSI BASIC----");
+ break;
+ case 9:
+ strcpy(topline,"------EXIT------");
+ break;
+ }
+ strcpy(bottomline,"");
+ break;
+ case 1:
+ //Sub level menu
+ switch(top_menu) {
+ case 8:
+ 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 1:
+ 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 2:
+ strcpy(topline,"--SENSORS MENU--");
+ switch(sub_menu) {
+ case 0: {
+ float battery = sensors.get_battery_voltage ();
+ sprintf(bottomline,"BATTERY: %1.3fV",battery);
break;
- case 1:
- strcpy(led_status,"RED");
+ }
+ case 1: {
+ float dc = sensors.get_dc_voltage ();
+ sprintf(bottomline,"DC: %1.3fV",dc);
+ break;
+ }
+ case 2: {
+ float current = sensors.get_current ();
+ sprintf(bottomline,"CURRENT: %1.3fA",current);
break;
- case 2:
- strcpy(led_status,"GREEN");
+ }
+ case 3: {
+ float temperature = sensors.get_temperature();
+ sprintf(bottomline,"TEMP: %3.2fC", temperature);
break;
- case 3:
- strcpy(led_status,"YELLOW");
+ }
+ case 4:
+ sensors.store_background_base_ir_values();
+ sprintf(bottomline,"BIR%dB:%d",base_ir_index+1,sensors.get_background_base_ir_value(base_ir_index));
+ break;
+ case 5:
+ sensors.store_illuminated_base_ir_values();
+ sprintf(bottomline,"BIR%dR:%d",base_ir_index+1,sensors.get_illuminated_base_ir_value(base_ir_index));
+ break;
+ case 6:
+ sensors.store_illuminated_base_ir_values();
+ sprintf(bottomline,"BIC%dR:%1.3f",base_ir_index+1,sensors.get_calibrated_base_ir_value(base_ir_index));
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");
+ case 7:
+ sensors.store_background_raw_ir_values();
+ sprintf(bottomline,"SIR%dB:%d",side_ir_index+1,sensors.get_background_raw_ir_value(side_ir_index));
+ break;
+ case 8:
+ sensors.store_illuminated_raw_ir_values();
+ sprintf(bottomline,"SIR%dR:%d",side_ir_index+1,sensors.get_illuminated_raw_ir_value(side_ir_index));
+ break;
+ case 9:
+ sprintf(bottomline,"SIR%dD:%3.1f",side_ir_index+1,0.0);
break;
- case 1:
- strcpy(led_status,"RED");
+ case 10:
+ if(ultrasonic_distance_updated == 1) {
+ sprintf(bottomline,"USONIC:%3dcm",ultrasonic_distance);
+ } else sprintf(bottomline,"USONIC:---------");
+ sensors.update_ultrasonic_measure();
break;
- case 2:
- strcpy(led_status,"GREEN");
+ case 11:
+ sensors.store_line_position();
+ if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position);
+ else sprintf(bottomline,"LINE:---------");
break;
- case 3:
- strcpy(led_status,"YELLOW");
+
+ case 12:
+ sprintf(bottomline,"COLOUR: %s",colour.get_colour_string(colour.detect_colour_once()));
+ break;
+ case 13:
+ sprintf(bottomline,"EXIT");
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 2:
- strcpy(topline,"--SENSORS MENU--");
- switch(sub_menu) {
- case 0: {
- float battery = sensors.get_battery_voltage ();
- sprintf(bottomline,"BATTERY: %1.3fV",battery);
- break;
- }
- case 1: {
- float dc = sensors.get_dc_voltage ();
- sprintf(bottomline,"DC: %1.3fV",dc);
- break;
- }
- case 2: {
- float current = sensors.get_current ();
- sprintf(bottomline,"CURRENT: %1.3fA",current);
- break;
- }
- case 3: {
- float temperature = sensors.get_temperature();
- sprintf(bottomline,"TEMP: %3.2fC", temperature);
- break;
+ break;
+ case 3:
+ 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;
}
- case 4:
- sensors.store_background_base_ir_values();
- sprintf(bottomline,"BIR%dB:%d",base_ir_index+1,sensors.get_background_base_ir_value(base_ir_index));
- break;
- case 5:
- sensors.store_illuminated_base_ir_values();
- sprintf(bottomline,"BIR%dR:%d",base_ir_index+1,sensors.get_illuminated_base_ir_value(base_ir_index));
- break;
- case 6:
- sensors.store_illuminated_base_ir_values();
- sprintf(bottomline,"BIC%dR:%1.3f",base_ir_index+1,sensors.get_calibrated_base_ir_value(base_ir_index));
- break;
- case 7:
- sensors.store_background_raw_ir_values();
- sprintf(bottomline,"SIR%dB:%d",side_ir_index+1,sensors.get_background_raw_ir_value(side_ir_index));
- break;
- case 8:
- sensors.store_illuminated_raw_ir_values();
- sprintf(bottomline,"SIR%dR:%d",side_ir_index+1,sensors.get_illuminated_raw_ir_value(side_ir_index));
- break;
- case 9:
- sprintf(bottomline,"SIR%dD:%3.1f",side_ir_index+1,0.0);
- break;
- case 10:
- if(ultrasonic_distance_updated == 1) {
- sprintf(bottomline,"USONIC:%3dcm",ultrasonic_distance);
- } else sprintf(bottomline,"USONIC:---------");
- sensors.update_ultrasonic_measure();
- break;
- case 11:
- sensors.store_line_position();
- if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position);
- else sprintf(bottomline,"LINE:---------");
- break;
+ break;
+ case 4:
+ strcpy(topline,"---RADIO MENU---");
+ switch(sub_menu) {
- case 12:
- sprintf(bottomline,"COLOUR: %s",colour.get_colour_string(colour.detect_colour_once()));
- break;
- case 13:
- sprintf(bottomline,"EXIT");
- break;
- }
- break;
- case 3:
- 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 4:
- strcpy(topline,"---RADIO MENU---");
- switch(sub_menu) {
-
- case 0:
- sprintf(bottomline,"EXIT");
- break;
- }
- break;
- case 5:
- 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 6:
- 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,"COLOUR WALK");
- break;
- case 5:
- sprintf(bottomline,"EXIT");
- break;
- }
- break;
- case 7:
- strcpy(topline,"---CALIB. MENU--");
- switch(sub_menu) {
- case 0:
- sprintf(bottomline,"BASE SENSOR");
- break;
- case 1:
- sprintf(bottomline,"MOTOR");
- break;
- case 2:
- sprintf(bottomline,"EXIT");
- break;
- }
- break;
- }
- break;
- }
- display.write_string(topline);
- display.set_position(1,0);
- display.write_string(bottomline);
- // Periodically update when on sensors menu
- if(top_menu == 2 && level == 1) {
- demo_event.attach(this,&Demo::demo_event_thread, 0.25);
+ case 0:
+ sprintf(bottomline,"EXIT");
+ break;
+ }
+ break;
+ case 5:
+ 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 6:
+ strcpy(topline,"---DEMOS MENU---");
+ 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,"COLOUR WALK");
+ break;
+ case 5:
+ sprintf(bottomline,"EXIT");
+ break;
+ }
+ break;
+ case 7:
+ strcpy(topline,"---CALIB. MENU--");
+ switch(sub_menu) {
+ case 0:
+ sprintf(bottomline,"BASE SENSOR");
+ break;
+ case 1:
+ sprintf(bottomline,"MOTOR");
+ break;
+ case 2:
+ sprintf(bottomline,"EXIT");
+ break;
+ }
+ break;
+ }
+ break;
+ }
+ display.write_string(topline);
+ display.set_position(1,0);
+ display.write_string(bottomline);
+ // Periodically update when on sensors menu
+ if(top_menu == 2 && level == 1) {
+ demo_event.attach(this,&Demo::demo_event_thread, 0.25);
+ }
}
}
@@ -877,16 +883,17 @@
void Demo::quick_test_cycle()
{
- char next_step [] = {4,5,8,3};
+ char next_step [] = {4,5,8,3,6,4};
char test_message [17];
- int wait_period = SELF_TEST_PERIOD * 10;
+ int wait_period = SELF_TEST_PERIOD * 5;
if(test_substep == 0) {
display.clear_display();
+ if(test_step == 5 && has_base_colour_sensor == 0)test_step ++;
switch(test_step) {
case 0:
display.write_string("01 - Power ");
- pc.printf("\nTest 01: Power quick tests [%f]\n",psi.get_uptime());
+ pc.printf("\nTest 01: Power tests [%f]\n",psi.get_uptime());
break;
case 1:
display.write_string("02 - Base IR ");
@@ -898,10 +905,98 @@
break;
case 3:
display.write_string("04 - LEDs ");
- pc.printf("\nTest 04: LED quick tests [%f]\n",psi.get_uptime());
+ pc.printf("\nTest 04: LED tests [%f]\n",psi.get_uptime());
+ break;
+ case 4:
+ display.write_string("05 - Motors ");
+ pc.printf("\nTest 05: Motor tests [%f]\n",psi.get_uptime());
+ break;
+ case 5:
+ display.write_string("06 - Colour ");
+ pc.printf("\nTest 06: Base colour sensor tests [%f]\n",psi.get_uptime());
break;
}
}
+ char wait_time = 8;
+ if(test_step == 5) {
+ // Base colour sensor tests
+ int dark_rgb_readings [4];
+ int light_rgb_readings [4];
+ colour.enable_base_colour_sensor();
+ wait(0.03);
+ colour.read_base_colour_sensor_values( dark_rgb_readings);
+ led.set_base_led(1);
+ wait(0.05);
+ colour.read_base_colour_sensor_values( light_rgb_readings);
+ led.set_base_led(0);
+ colour.disable_base_colour_sensor();
+ wait(0.02);
+ float adjusted[4];
+ colour.get_calibrated_colour(light_rgb_readings,adjusted);
+ int last_detected_colour = colour.identify_colour_from_calibrated_colour_scores(adjusted);
+ pc.printf("Sample %d Dark: C%04d-R%04d-G%04d-B%04d Light: C%04d-R%04d-G%04d-B%04d Calib: C%1.3f-R%1.3f-G%1.3f-B%1.3f Colour: %s\n",
+ dark_rgb_readings[0],dark_rgb_readings[1],dark_rgb_readings[2],dark_rgb_readings[3],
+ light_rgb_readings[0],light_rgb_readings[1],light_rgb_readings[2],light_rgb_readings[3],
+ adjusted[0],adjusted[1],adjusted[2],adjusted[3],colour.get_colour_string(last_detected_colour) );
+ switch(test_substep){
+ case 0:sprintf(test_message,"D:%4d %4d %4d",dark_rgb_readings[1],dark_rgb_readings[2],dark_rgb_readings[3]);break;
+ case 1:sprintf(test_message,"L:%4d %4d %4d",light_rgb_readings[1],light_rgb_readings[2],light_rgb_readings[3]);break;
+ case 2:sprintf(test_message,"C:%1.2f %1.2f %1.2f",adjusted[1],adjusted[2],adjusted[3]);break;
+ case 3:sprintf(test_message,"COLOUR: %s",colour.get_colour_string(last_detected_colour));break;
+ }
+ }
+
+ if(test_step == 4) {
+ // Motor tests
+ switch(test_substep){
+ case 0:sprintf(test_message,"20%% Forward");
+ pc.printf("20%% Forward\n");
+ motors.forward(0.2);
+ wait_time = 16;
+ break;
+ case 1:sprintf(test_message,"40%% Forward");
+ pc.printf("40%% Forward\n");
+ motors.forward(0.4);
+ break;
+ case 3:sprintf(test_message,"20%% Backward");
+ pc.printf("20%% Backward\n");
+ motors.backward(0.2);
+ wait_time = 16;
+ break;
+ case 4:sprintf(test_message,"40%% Backward");
+ pc.printf("40%% Backward\n");
+ motors.backward(0.4);
+ break;
+ case 2:case 5:sprintf(test_message,"BRAKE");
+ pc.printf("Brake motors\n");
+ motors.brake();
+ break;
+ }
+ wait_us(SELF_TEST_PERIOD * wait_time);
+ motors.stop();
+ wait_us(SELF_TEST_PERIOD);
+ }
+ if(test_step == 3) {
+ //LED tests
+ switch(test_substep){
+ case 0:sprintf(test_message,"RED");
+ led.set_leds(0x00,0xFF);
+ led.set_center_led(1,1);
+ break;
+ case 1:sprintf(test_message,"GREEN");
+ led.set_leds(0xFF,0x00);
+ led.set_center_led(2,1);
+ break;
+ case 2:sprintf(test_message,"YELLOW");
+ led.set_leds(0xFF,0xFF);
+ led.set_center_led(3,1);
+ break;
+ }
+ wait_us(SELF_TEST_PERIOD * 10);
+ led.set_leds(0x00,0x00);
+ led.set_center_led(0);
+ wait_us(SELF_TEST_PERIOD);
+ }
if(test_step == 1) {
//Base IR tests
sensors.store_background_base_ir_values();
@@ -1038,10 +1133,12 @@
if(test_substep >= next_step[test_step]) {
test_substep = 0;
test_step ++;
- if(test_step == 4) test_step = 0;
+ if(test_step >= 6) test_step = 0;
}
- demo_timeout.attach_us(this,&Demo::quick_test_cycle, wait_period);
+ if(demo_running == 1) {
+ demo_timeout.attach_us(this,&Demo::quick_test_cycle, wait_period);
+ } else pc.printf("- Quick test terminated\n");
}
void Demo::toggle_quick_test()
@@ -1050,13 +1147,9 @@
if(quick_test == 0) {
pc.printf("________________________________________\n");
pc.printf("Self test stopped at %f with %d warnings\n\n",psi.get_uptime(),test_warnings);
- demo_running = 0;
- demo_timeout.detach();
- display.set_backlight_brightness(1);
display.clear_display();
display.write_string("---QUICK TEST---");
-
-
+ stop_demo();
} else {
// Reset all LEDs, motors, display
pc.printf("\n________________________________________\n");
@@ -1069,7 +1162,7 @@
}
}
-void Demo::start_line_demo()
+void Demo::setup_demo()
{
display.set_backlight_brightness(0);
time_out = 0.25f;
@@ -1078,30 +1171,23 @@
speed = 0;
led_step = 0;
demo_running = 1;
+}
+
+void Demo::start_line_demo()
+{
+ setup_demo();
demo_timeout.attach_us(this,&Demo::line_demo_cycle,1000);
}
void Demo::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;
+ setup_demo();
demo_timeout.attach_us(this,&Demo::obstacle_demo_cycle,1000);
}
void Demo::start_colour_demo()
{
- display.set_backlight_brightness(0);
- time_out = 0.25f;
- demo_timer.start();
- state = 0;
- speed = 0;
- led_step = 0;
- demo_running = 1;
+ setup_demo();
colour.start_colour_ticker(100);
demo_timeout.attach_us(this,&Demo::colour_demo_cycle,1000);
}
@@ -1112,26 +1198,16 @@
display.write_string("STRESS TEST");
display.set_position(1,0);
display.write_string("----25%----");
+ setup_demo();
time_out = 0.04f;
- demo_timer.start();
- state = 0;
- speed = 0;
- stress_step = 0;
- spin_step = 0;
- demo_running = 1;
demo_timeout.attach_us(this,&Demo::stress_demo_cycle,1000);
}
void Demo::start_spinning_demo()
{
display.set_backlight_brightness(0);
+ setup_demo();
time_out = 0.0f;
- demo_timer.start();
- state = 0;
- speed = 0;
- led_step = 0;
- spin_step = 0;
- demo_running = 1;
demo_timeout.attach_us(this,&Demo::spinning_demo_cycle,1000);
}
@@ -1210,10 +1286,7 @@
demo_timer.reset();
}
if(demo_running == 1)demo_timeout.attach_us(this,&Demo::line_demo_cycle,500);
- else {
- motors.stop();
- display.set_backlight_brightness(bl_brightness * 0.01f);
- }
+ else stop_demo();
}
void Demo::stress_demo_cycle()
@@ -1257,10 +1330,15 @@
demo_timer.reset();
}
if(demo_running == 1)demo_timeout.attach_us(this,&Demo::stress_demo_cycle,500);
- else {
- motors.stop();
- display.set_backlight_brightness(bl_brightness * 0.01f);
- }
+ else stop_demo();
+}
+
+void Demo::stop_demo()
+{
+ demo_running = 0;
+ demo_timeout.detach();
+ motors.stop();
+ display.set_backlight_brightness(bl_brightness * 0.01f);
}
void Demo::spinning_demo_cycle()
@@ -1495,10 +1573,7 @@
demo_timer.reset();
}
if(demo_running == 1)demo_timeout.attach_us(this,&Demo::spinning_demo_cycle,500);
- else {
- motors.stop();
- display.set_backlight_brightness(bl_brightness * 0.01f);
- }
+ else stop_demo();
}
void Demo::colour_demo_cycle()
@@ -1563,10 +1638,7 @@
demo_timer.reset();
}
if(demo_running == 1)demo_timeout.attach_us(this,&Demo::colour_demo_cycle,200);
- else {
- motors.stop();
- display.set_backlight_brightness(bl_brightness * 0.01f);
- }
+ else stop_demo();
}
@@ -1640,10 +1712,7 @@
demo_timer.reset();
}
if(demo_running == 1)demo_timeout.attach_us(this,&Demo::obstacle_demo_cycle,200);
- else {
- motors.stop();
- display.set_backlight_brightness(bl_brightness * 0.01f);
- }
+ else stop_demo();
}
diff -r 3e3b03d80ea3 -r 1bc6c6dc477b demo.h
--- a/demo.h Mon Jun 05 22:47:14 2017 +0000
+++ b/demo.h Mon May 14 15:35:38 2018 +0000
@@ -42,13 +42,16 @@
void start_demo_mode(void);
void demo_handle_switch_event(char switch_pressed);
-private:
- void demo_update_leds(void);
- void demo_event_thread(void);
void start_line_demo(void);
void start_obstacle_demo(void);
void start_spinning_demo(void);
void start_stress_demo(void);
+ void stop_demo(void);
+
+private:
+ void demo_update_leds(void);
+ void demo_event_thread(void);
+
void start_colour_demo(void);
void line_demo_cycle(void);
void obstacle_demo_cycle(void);
@@ -57,5 +60,6 @@
void colour_demo_cycle(void);
void quick_test_cycle(void);
void toggle_quick_test(void);
+ void setup_demo(void);
};
#endif
\ No newline at end of file