James Hilder
/
PsiSwarmV9-Fork
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
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 |
--- 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();
}
--- 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