Diff: AplicationLayer/SelfTest.cpp

Revision:

1:a6bcf44b90df

Parent:

0:633cef71e6ba

Child:

2:cff0fa966e58

--- a/AplicationLayer/SelfTest.cpp	Sun Aug 09 09:37:55 2015 +0000
+++ b/AplicationLayer/SelfTest.cpp	Tue Aug 18 01:44:45 2015 +0000
@@ -1,6 +1,90 @@
 #include "SysConfig.h"
 
+void SpeedTest(void)
+{
+    printf("[Speed Test]\n\rMOT1 = %drpm, MOT2 = %drpm, MOT3 = %drpm, MOT4 = %drpm.\n\r", Mot_GetSpeed(1), Mot_GetSpeed(2), Mot_GetSpeed(3), Mot_GetSpeed(4));  
+}
+
+void VoltageTest(void)
+{
+    printf("[Voltage Test]\n\rVBAT = %.2fV, VDD12V = %.2fV, VDD5V = %.2fV, VDD3V3 = %.2fV.\n\r", Vmon_ReadVolt(1), Vmon_ReadVolt(2), Vmon_ReadVolt(3), Vmon_ReadVolt(4)); 
+}
+
+
+void MotTest(void)
+{
+    int dir, turn;
+    printf("\n\r==== Motor Test ====\n\r");
+    for(int i=0; i<100; i+=20)
+    {
+        printf("\n\rMotor speed set to %d%%.\n\r", i);
+        printf("-->Motor will move forward.\n\r");
+        dir = 'f';
+        
+        printf("---->Motor will move straight.\n\r");
+        turn = 'm';
+        Mot_Ctrl(dir, turn, (float)i / 100.0);
+        SpeedTest();
+        VoltageTest();
+        LedToggle(0);
+        wait(3);
+        
+        printf("---->Motor will turn left.\n\r");
+        turn = 'l';
+        Mot_Ctrl(dir, turn, (float)i / 100.0);
+        SpeedTest();
+        VoltageTest();
+        LedToggle(0);
+        wait(3);
+        
+        printf("---->Motor will turn right.\n\r");
+        turn = 'r';
+        Mot_Ctrl(dir, turn, (float)i / 100.0);
+        SpeedTest();
+        VoltageTest();
+        LedToggle(0);
+        wait(3);
+        
+        printf("-->Motor will move forward.\n\r");
+        dir = 'b';
+        
+        printf("---->Motor will move straight.\n\r");
+        turn = 'm';
+        Mot_Ctrl(dir, turn, (float)i / 100.0);
+        SpeedTest();
+        VoltageTest();
+        LedToggle(0);
+        wait(3);
+        
+        printf("---->Motor will turn left.\n\r");
+        turn = 'l';
+        Mot_Ctrl(dir, turn, (float)i / 100.0);
+        SpeedTest();
+        VoltageTest();
+        LedToggle(0);
+        wait(3);
+        
+        printf("---->Motor will turn right.\n\r");
+        turn = 'r';
+        Mot_Ctrl(dir, turn, (float)i / 100.0);
+        SpeedTest();
+        VoltageTest();
+        LedToggle(0);
+        wait(3);
+    }
+}
+
 void SelfTest(void)
+#if 1
+{
+    printf("Starting motor velocimeter...\n\r");
+    Mot_StartVelocimeter();
+    while(1)
+    {
+        MotTest();
+    }   
+}
+#elif
 {
     //Breath LED
     int i, j;
@@ -29,4 +113,5 @@
         }
         wait_ms(200);
     }      
-}
\ No newline at end of file
+}
+#endif
\ No newline at end of file