Psi Swarm robot library version 0.9
Fork of PsiSwarmV8_CPP by
Diff: demo.cpp
- Revision:
- 15:66be5ec52c3b
- Parent:
- 14:2f1ad77d281e
diff -r 2f1ad77d281e -r 66be5ec52c3b demo.cpp --- 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);