Diff: demo.cpp

Revision:

15:66be5ec52c3b

Parent:

14:2f1ad77d281e

Child:

16:50686c07ad07

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);