Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
Diff: demo.cpp
- Revision:
- 18:9204f74069b4
- Parent:
- 17:bf614e28668f
- Child:
- 19:3e3b03d80ea3
--- a/demo.cpp Sun Jun 04 13:11:09 2017 +0000 +++ b/demo.cpp Sun Jun 04 20:22:41 2017 +0000 @@ -260,6 +260,7 @@ break; case 7: // Calibration Menu if(sub_menu == 0) sensors.calibrate_base_sensors(); + if(sub_menu == 1) motors.calibrate_motors(); if(sub_menu == 2) level = 0; break; case 6: // Demo Menu @@ -441,8 +442,9 @@ break; case 7: // Calibration Menu if(sub_menu == 0) sensors.calibrate_base_sensors(); + if(sub_menu == 1) motors.calibrate_motors(); if(sub_menu == 2) level = 0; - break; + break; case 6: // Demo Menu if(sub_menu == 0) { if(demo_running == 0) { @@ -734,7 +736,7 @@ if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position); else sprintf(bottomline,"LINE:---------"); break; - + case 12: sprintf(bottomline,"COLOUR: %s",colour.get_colour_string(colour.detect_colour_once())); break; @@ -834,7 +836,7 @@ break; case 4: sprintf(bottomline,"COLOUR WALK"); - break; + break; case 5: sprintf(bottomline,"EXIT"); break; @@ -876,7 +878,7 @@ char next_step [] = {4,5,8,3}; char test_message [17]; int wait_period = SELF_TEST_PERIOD * 10; - + if(test_substep == 0) { display.clear_display(); switch(test_step) { @@ -898,6 +900,37 @@ break; } } + if(test_step == 1) { + //Base IR tests + sensors.store_background_base_ir_values(); + wait(0.05); + sensors.store_illuminated_base_ir_values(); + pc.printf("Sample %d 1: %04d-%04d-%1.2f 2: %04d-%04d-%1.2f 3: %04d-%04d-%1.2f 4: %04d-%04d-%1.2f 5: %04d-%04d-%1.2f\n", (test_substep+1), + sensors.get_background_base_ir_value(0), sensors.get_illuminated_base_ir_value(0), sensors.get_calibrated_base_ir_value (0), + sensors.get_background_base_ir_value(1), sensors.get_illuminated_base_ir_value(1),sensors.get_calibrated_base_ir_value (1), + sensors.get_background_base_ir_value(2), sensors.get_illuminated_base_ir_value(2),sensors.get_calibrated_base_ir_value (2), + sensors.get_background_base_ir_value(3), sensors.get_illuminated_base_ir_value(3),sensors.get_calibrated_base_ir_value (3), + sensors.get_background_base_ir_value(4), sensors.get_illuminated_base_ir_value(4),sensors.get_calibrated_base_ir_value (4)); + sprintf(test_message,"%d:%4d-%4d-%1.2f",test_substep+1,sensors.get_background_base_ir_value(test_substep),sensors.get_illuminated_base_ir_value(test_substep),sensors.get_calibrated_base_ir_value(test_substep)); + wait_us(SELF_TEST_PERIOD); + } + if(test_step == 2) { + //Side IR tests + sensors.store_background_raw_ir_values(); + wait(0.05); + sensors.store_illuminated_raw_ir_values(); + pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d 6: %04d-%04d 7: %04d-%04d 8: %04d-%04d\n", (test_substep+1), + sensors.get_background_raw_ir_value(0), sensors.get_illuminated_raw_ir_value(0), + sensors.get_background_raw_ir_value(1), sensors.get_illuminated_raw_ir_value(1), + sensors.get_background_raw_ir_value(2), sensors.get_illuminated_raw_ir_value(2), + sensors.get_background_raw_ir_value(3), sensors.get_illuminated_raw_ir_value(3), + sensors.get_background_raw_ir_value(4), sensors.get_illuminated_raw_ir_value(4), + sensors.get_background_raw_ir_value(5), sensors.get_illuminated_raw_ir_value(5), + sensors.get_background_raw_ir_value(6), sensors.get_illuminated_raw_ir_value(6), + sensors.get_background_raw_ir_value(7), sensors.get_illuminated_raw_ir_value(7)); + sprintf(test_message,"%d:%4d-%4d",test_substep+1,sensors.get_background_raw_ir_value(test_substep),sensors.get_illuminated_raw_ir_value(test_substep)); + wait_us(SELF_TEST_PERIOD); + } if(test_step == 0) { // Power self-test switch(test_substep) { @@ -959,14 +992,14 @@ sd_current = sqrt(sd_current/SAMPLE_SIZE); sprintf(test_message,"CURRENT: %1.3fV",mean_current); - pc.printf(" - Load Current : %1.4fA [SD = % 1.4f]\n",mean_current,sd_current); + pc.printf(" - Load Current : %1.4fA [SD = % 1.4f]\n",mean_current,sd_current); if(mean_current > 0.2) { pc.printf(" - WARNING : Higher than expected load current\n"); test_warnings++; } break; } - + case 3: {// System temperature float temps[SAMPLE_SIZE]; float mean_temp = 0; @@ -981,7 +1014,7 @@ sd_temp = sqrt(sd_temp/SAMPLE_SIZE); sprintf(test_message,"TEMP : %3.2fC",mean_temp); - pc.printf(" - System Temperature : %3.2fC [SD = % 1.4f]\n",mean_temp,sd_temp); + pc.printf(" - System Temperature : %3.2fC [SD = % 1.4f]\n",mean_temp,sd_temp); if(mean_temp > 32) { pc.printf(" - WARNING : High system temperature detected\n"); test_warnings++; @@ -1470,19 +1503,23 @@ { if(demo_timer.read() > time_out) { int col = colour.get_detected_colour(); - switch(col){ - case 0: led.set_leds(0,0xFF); - led.set_center_led(1,1); - break; - case 1: led.set_leds(0xFF,0xFF); - led.set_center_led(3,1); - break; - case 2: led.set_leds(0xFF,0); - led.set_center_led(2,1); - break; - default: led.set_leds(0,0); - led.set_center_led(0,0); - break; + switch(col) { + case 0: + led.set_leds(0,0xFF); + led.set_center_led(1,1); + break; + case 1: + led.set_leds(0xFF,0xFF); + led.set_center_led(3,1); + break; + case 2: + led.set_leds(0xFF,0); + led.set_center_led(2,1); + break; + default: + led.set_leds(0,0); + led.set_center_led(0,0); + break; } switch(state) { case 0: //Robot is stopped @@ -1533,7 +1570,7 @@ void Demo::obstacle_demo_cycle() { - + if(demo_timer.read() > time_out) { switch(state) { case 0: //Robot is stopped