James Hilder
Psi Swarm robot library version 0.9
Fork of
PsiSwarmV9
by 
Psi Swarm Robot
Diff: demo.cpp
- Revision:
- 18:9204f74069b4
- Parent:
- 17:bf614e28668f
- Child:
- 19:3e3b03d80ea3
diff -r bf614e28668f -r 9204f74069b4 demo.cpp
--- 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