James Hilder
Psi Swarm robot library version 0.9
Fork of
PsiSwarmV9
by 
Psi Swarm Robot
Diff: demo.cpp
- Revision:
- 15:66be5ec52c3b
- Parent:
- 14:2f1ad77d281e
- Child:
- 16:50686c07ad07
--- a/demo.cpp Tue May 30 21:03:18 2017 +0000
+++ b/demo.cpp Thu Jun 01 21:58:14 2017 +0000
@@ -1,11 +1,11 @@
/* University of York Robotics Laboratory PsiSwarm Library: Demo Mode Source File
- *
+ *
* Copyright 2017 University of York
*
- * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
* You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
* Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and limitations under the License.
*
* File: demo.cpp
@@ -37,6 +37,7 @@
#define LF_I_TERM 0
#define LF_D_TERM 4
+char quick_test = 0;
char top_menu = 0;
char sub_menu = 0;
char level = 0;
@@ -136,20 +137,24 @@
case 1: //Up pressed
switch(level) {
case 0:
- if(top_menu != 8) {
+ if(top_menu != 9 && top_menu!= 0) {
level++;
sub_menu = 0;
} else {
- demo_on = 0;
- user_code_running = user_code_restore_mode;
+ if(top_menu == 0) {
+ toggle_quick_test();
+ } else {
+ demo_on = 0;
+ user_code_running = user_code_restore_mode;
+ }
}
break;
case 1:
switch(top_menu) {
- case 7: // PsiBasic Menu
+ case 8: // PsiBasic Menu
if(sub_menu == psi_basic_file_count) level = 0;
break;
- case 0: //LED Menu
+ case 7: //LED Menu
if(sub_menu < 9) {
if(led_state[sub_menu] == 0) led_state[sub_menu] = 3;
else led_state[sub_menu]--;
@@ -196,7 +201,7 @@
}
if(sub_menu == 13) level = 0;
break;
- case 1: // Sensors Menu
+ case 2: // Sensors Menu
if(sub_menu == 4 || sub_menu == 5) {
if(base_ir_index == 0) base_ir_index = 4;
else base_ir_index --;
@@ -207,7 +212,7 @@
}
if(sub_menu == 11) level = 0;
break;
- case 2: // Motor Menu
+ case 3: // Motor Menu
if(sub_menu == 0) {
left_speed += 5;
if(left_speed > 100) left_speed = 100;
@@ -246,13 +251,13 @@
level = 0;
}
break;
- case 3: // Radio Menu
+ case 4: // Radio Menu
if(sub_menu == 0) level = 0;
break;
- case 4: // Info Menu
+ case 5: // Info Menu
if(sub_menu == 6) level = 0;
break;
- case 5: // Demo Menu
+ case 6: // Demo Menu
if(sub_menu == 0) {
if(demo_running == 0) {
start_line_demo();
@@ -298,20 +303,24 @@
case 2: //Down pressed
switch(level) {
case 0:
- if(top_menu != 8) {
+ if(top_menu != 9 && top_menu != 0) {
level++;
sub_menu = 0;
} else {
- demo_on = 0;
- user_code_running = user_code_restore_mode;
+ if(top_menu == 0) {
+ toggle_quick_test();
+ } else {
+ demo_on = 0;
+ user_code_running = user_code_restore_mode;
+ }
}
break;
case 1:
switch(top_menu) {
- case 7: // PsiBasic Menu
+ case 8: // PsiBasic Menu
if(sub_menu == psi_basic_file_count) level = 0;
break;
- case 0: //LED Menu
+ case 1: //LED Menu
if(sub_menu < 9) {
led_state[sub_menu]++;
if(led_state[sub_menu] == 4) led_state[sub_menu] = 0;
@@ -360,7 +369,7 @@
if(sub_menu == 13) level = 0;
break;
- case 1: // Sensors Menu
+ case 2: // Sensors Menu
if(sub_menu == 4 || sub_menu == 5) {
if(base_ir_index == 4) base_ir_index = 0;
else base_ir_index ++;
@@ -371,7 +380,7 @@
}
if(sub_menu == 11) level = 0;
break;
- case 2: // Motor Menu
+ case 3: // Motor Menu
if(sub_menu == 0) {
left_speed -= 5;
if(left_speed < -100) left_speed = -100;
@@ -410,13 +419,13 @@
level = 0;
}
break;
- case 3: // Radio Menu
+ case 4: // Radio Menu
if(sub_menu == 0) level = 0;
break;
- case 4: // Info Menu
+ case 5: // Info Menu
if(sub_menu == 6) level = 0;
break;
- case 5: // Demo Menu
+ case 6: // Demo Menu
if(sub_menu == 0) {
if(demo_running == 0) {
start_line_demo();
@@ -464,41 +473,40 @@
switch(level) {
case 0:
if(top_menu == 0) {
- top_menu = 8;
- }
- else {
- if(use_flash_basic == 0 && top_menu == 7) top_menu = 6;
+ top_menu = 9;
+ } else {
+ if(use_flash_basic == 0 && top_menu == 8) top_menu = 7;
top_menu --;
}
break;
case 1:
switch(top_menu) {
- case 0: //LED Menu
+ case 1: //LED Menu
if(sub_menu == 0) sub_menu = 13;
else sub_menu --;
break;
- case 1: //Sensors Menu
+ case 2: //Sensors Menu
if(sub_menu == 0) sub_menu = 11;
else sub_menu --;
break;
- case 2: //Motor Menu
+ case 3: //Motor Menu
if(sub_menu == 0) sub_menu = 3;
else sub_menu --;
break;
- case 4: //Info Menu
+ case 5: //Info Menu
if(sub_menu == 0) sub_menu = 6;
else sub_menu --;
break;
- case 5: //Demo Menu
+ case 6: //Demo Menu
if(sub_menu == 0) sub_menu = 4;
else sub_menu --;
break;
- case 6: //Calibrate Menu
+ case 7: //Calibrate Menu
if(sub_menu == 0) sub_menu = 2;
else sub_menu --;
break;
- case 7: //PsiBasic Menu
+ case 8: //PsiBasic Menu
if(sub_menu == 0) sub_menu = psi_basic_file_count;
else sub_menu --;
}
@@ -514,108 +522,37 @@
break;
case 1:
switch(top_menu) {
- case 0: //LED Menu
+ case 1: //LED Menu
if(sub_menu == 13) sub_menu = 0;
else sub_menu ++;
break;
- case 1: //Sensors Menu
+ case 2: //Sensors Menu
if(sub_menu == 11) sub_menu = 0;
else sub_menu ++;
break;
- case 2: //Motor Menu
+ case 3: //Motor Menu
if(sub_menu == 3) sub_menu = 0;
else sub_menu ++;
break;
- case 4: //Info Menu
+ case 5: //Info Menu
if(sub_menu == 6) sub_menu = 0;
else sub_menu ++;
break;
- case 5: //Demo Menu
+ case 6: //Demo Menu
if(sub_menu == 4) sub_menu = 0;
else sub_menu ++;
break;
- case 6: //Calibrate Menu
+ case 7: //Calibrate Menu
if(sub_menu == 2) sub_menu = 0;
else sub_menu ++;
break;
- case 7: //PsiBasic Menu
+ case 8: //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 != 8) {
- 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:
- motors.stop();
- break;
- case 1:
- motors.brake();
- break;
- case 2:
- motors.forward(0.5);
- break;
- case 3:
- motors.forward(1);
- break;
- case 4:
- motors.backward(0.5);
- break;
- case 5:
- motors.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 6: // Calibrate Menu
- if(sub_menu == 2) 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();
@@ -624,30 +561,33 @@
//Top level menu
switch(top_menu) {
case 0:
+ strcpy(topline,"---QUICK TEST---");
+ break;
+ case 1:
strcpy(topline,"---TEST LEDS----");
break;
- case 1:
+ case 2:
strcpy(topline,"--TEST SENSORS--");
break;
- case 2:
+ case 3:
strcpy(topline,"--TEST MOTORS---");
break;
- case 3:
- strcpy(topline,"---TEST RADIO---");
- break;
case 4:
- strcpy(topline,"------INFO------");
+ strcpy(topline,"---TEST RADIO---");
break;
case 5:
- strcpy(topline,"---CODE DEMOS---");
+ strcpy(topline,"------INFO------");
break;
case 6:
- strcpy(topline,"-MOTOR CALIBRATE");
+ strcpy(topline,"---CODE DEMOS---");
break;
case 7:
+ strcpy(topline,"-MOTOR CALIBRATE");
+ break;
+ case 8:
strcpy(topline,"---PSI BASIC----");
break;
- case 8:
+ case 9:
strcpy(topline,"------EXIT------");
break;
}
@@ -656,12 +596,12 @@
case 1:
//Sub level menu
switch(top_menu) {
- case 7:
+ 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 0:
+ case 1:
strcpy(topline,"----LED MENU----");
char led_status[7];
if(sub_menu<9) {
@@ -709,7 +649,7 @@
if(sub_menu == 13) strcpy(bottomline,"EXIT");
break;
- case 1:
+ case 2:
strcpy(topline,"--SENSORS MENU--");
switch(sub_menu) {
case 0: {
@@ -767,7 +707,7 @@
break;
}
break;
- case 2:
+ case 3:
strcpy(topline,"--MOTORS MENU---");
switch(sub_menu) {
case 0:
@@ -805,7 +745,7 @@
break;
}
break;
- case 3:
+ case 4:
strcpy(topline,"---RADIO MENU---");
switch(sub_menu) {
@@ -814,7 +754,7 @@
break;
}
break;
- case 4:
+ case 5:
strcpy(topline,"---INFO MENU----");
switch(sub_menu) {
case 0:
@@ -841,7 +781,7 @@
break;
}
break;
- case 5:
+ case 6:
strcpy(topline,"---CODE DEMOS---");
switch(sub_menu) {
case 0:
@@ -861,7 +801,7 @@
break;
}
break;
- case 6:
+ case 7:
strcpy(topline,"-MOTOR CALIBRATE");
switch(sub_menu) {
case 0:
@@ -869,7 +809,7 @@
break;
case 1:
sprintf(bottomline,"ENTER VALUE");
- break;
+ break;
case 2:
sprintf(bottomline,"EXIT");
break;
@@ -886,6 +826,174 @@
}
}
+char test_step = 0;
+char test_substep = 0;
+char test_warnings = 0;
+
+
+void Demo::quick_test_cycle()
+{
+ 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) {
+ case 0:
+ display.write_string("01 - Power ");
+ pc.printf("\nTest 01: Power quick tests [%f]\n",psi.get_uptime());
+ break;
+ case 1:
+ display.write_string("02 - Base IR ");
+ pc.printf("\nTest 02: Base infrared tests [%f]\n",psi.get_uptime());
+ break;
+ case 2:
+ display.write_string("03 - Side IR ");
+ pc.printf("\nTest 03: Side infrared tests [%f]\n",psi.get_uptime());
+ break;
+ case 3:
+ display.write_string("04 - LEDs ");
+ pc.printf("\nTest 04: LED quick tests [%f]\n",psi.get_uptime());
+ break;
+ }
+ }
+ if(test_step == 0) {
+ // Power self-test
+ switch(test_substep) {
+ case 0: { // Battery Voltage
+ float battery_voltages [SAMPLE_SIZE];
+ float mean_battery_voltage = 0;
+ float sd_battery_voltage = 0;
+ for(int i=0; i<SAMPLE_SIZE; i++) {
+ battery_voltages[i]=sensors.get_battery_voltage ();
+ mean_battery_voltage += battery_voltages[i];
+ wait_us(SELF_TEST_PERIOD);
+ }
+ mean_battery_voltage /= SAMPLE_SIZE;
+ for(int i=0; i<SAMPLE_SIZE; ++i) sd_battery_voltage += pow(battery_voltages[i] - mean_battery_voltage, 2);
+ sd_battery_voltage = sqrt(sd_battery_voltage/SAMPLE_SIZE);
+
+ sprintf(test_message,"BATTERY: %1.3fV",mean_battery_voltage);
+ pc.printf(" - Battery Voltage : %1.4fV [SD = % 1.4f]\n",mean_battery_voltage,sd_battery_voltage);
+ if(mean_battery_voltage < 3.6) {
+ pc.printf(" - WARNING : Battery voltage low\n");
+ test_warnings++;
+ }
+ break;
+ }
+ case 1: {// DC Voltage
+ float dc_voltages [SAMPLE_SIZE];
+ float mean_dc_voltage = 0;
+ float sd_dc_voltage = 0;
+ for(int i=0; i<SAMPLE_SIZE; i++) {
+ dc_voltages[i]=sensors.get_dc_voltage ();
+ mean_dc_voltage += dc_voltages[i];
+ wait_us(SELF_TEST_PERIOD);
+ }
+ mean_dc_voltage /= SAMPLE_SIZE;
+ for(int i=0; i<SAMPLE_SIZE; ++i) sd_dc_voltage += pow(dc_voltages[i] - mean_dc_voltage, 2);
+ sd_dc_voltage = sqrt(sd_dc_voltage/SAMPLE_SIZE);
+
+ sprintf(test_message,"DC : %1.3fV",mean_dc_voltage);
+ pc.printf(" - DC Input Voltage : %1.4fV [SD = % 1.4f]\n",mean_dc_voltage,sd_dc_voltage);
+ if(mean_dc_voltage < 0.5) {
+ pc.printf(" - WARNING : DC voltage low - check charging resistor\n");
+ test_warnings++;
+ }
+ break;
+ }
+
+
+ case 2: { // Current
+ float currents [SAMPLE_SIZE];
+ float mean_current = 0;
+ float sd_current = 0;
+ for(int i=0; i<SAMPLE_SIZE; i++) {
+ currents[i]=sensors.get_current ();
+ mean_current += currents[i];
+ wait_us(SELF_TEST_PERIOD);
+ }
+ mean_current /= SAMPLE_SIZE;
+ for(int i=0; i<SAMPLE_SIZE; ++i) sd_current += pow(currents[i] - mean_current, 2);
+ 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);
+ 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;
+ float sd_temp = 0;
+ for(int i=0; i<SAMPLE_SIZE; i++) {
+ temps[i]=sensors.get_temperature();
+ mean_temp += temps[i];
+ wait_us(SELF_TEST_PERIOD);
+ }
+ mean_temp /= SAMPLE_SIZE;
+ for(int i=0; i<SAMPLE_SIZE; ++i) sd_temp += pow(temps[i] - mean_temp, 2);
+ 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);
+ if(mean_temp > 32) {
+ pc.printf(" - WARNING : High system temperature detected\n");
+ test_warnings++;
+ }
+ if(mean_temp < 10) {
+ pc.printf(" - WARNING : Low system temperature detected\n");
+ test_warnings++;
+ }
+ break;
+ }
+ }
+ }
+
+ display.set_position(1,0);
+ display.write_string(" ");
+ display.set_position(1,0);
+ display.write_string(test_message);
+ test_substep++;
+ if(test_substep >= next_step[test_step]) {
+ test_substep = 0;
+ test_step ++;
+ if(test_step == 4) test_step = 0;
+ }
+
+ demo_timeout.attach_us(this,&Demo::quick_test_cycle, wait_period);
+}
+
+void Demo::toggle_quick_test()
+{
+ quick_test = 1 - quick_test;
+ 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---");
+
+
+ } else {
+ // Reset all LEDs, motors, display
+ pc.printf("\n________________________________________\n");
+ pc.printf("Self test started at %f\n\n",psi.get_uptime(),test_warnings);
+ display.clear_display();
+ test_step = 0;
+ test_substep = 0;
+ demo_running = 1;
+ demo_timeout.attach_us(this,&Demo::quick_test_cycle,1000);
+ }
+}
+
void Demo::start_line_demo()
{
display.set_backlight_brightness(0);
@@ -976,35 +1084,35 @@
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 {
+ //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;
+ }
}
- }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
motors.set_left_motor_speed(lf_left);