Revision 3:171f4d0ca77b, committed 2015-08-20

Comitter:

nightseas

Date:

Thu Aug 20 11:36:01 2015 +0000

Parent:

2:cff0fa966e58

Child:

4:fb5235d39a9c

Commit message:

Add debug commander tool.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AplicationLayer/DebugCommander.cpp	Thu Aug 20 11:36:01 2015 +0000
@@ -0,0 +1,214 @@
+#include "SysConfig.h"
+
+void LedFlasher(void)
+{
+    int ledNumOn;
+    for(ledNumOn = 1; ledNumOn < LED_NUM_MAX; ledNumOn++)
+    {
+        LedOffAll();
+        LedOn(ledNumOn);
+        wait_ms(300);
+    }
+}
+
+void DebugFunc_Motor(void)
+{
+    int cmdDebug;
+    int dir = 's', turn = 'm';
+    float speed = 0;
+    
+    Mot_StartVelocimeter();
+    
+    uart_db.printf("\n\r\n\r++++++++ Motor Test ++++++++\n\r\n\r");
+    uart_db.printf("Usage: +.speed up, -.speed down, p.stop, \n\rw.forward, s.backward, a.turn left, d.turn right, q.quit test\n\r");
+    uart_db.printf(" Input:");    
+    
+    while(1)
+    {             
+        for(int i=0;i<3000;i++)
+        {
+            cmdDebug = uart_db.getc();
+            switch(cmdDebug)
+            {
+                case '+':
+                    speed += 0.1;
+                    if(speed > 1)
+                        speed = 1;
+                    Mot_Ctrl(dir, turn, speed);
+                    uart_db.printf("\n\rMotor speed set to %.01f%%.\n\r", speed * 100.0);
+                    break;
+                
+                case '-':
+                    speed -= 0.1;
+                    if(speed < 0)
+                        speed = 0;
+                    Mot_Ctrl(dir, turn, speed);
+                    uart_db.printf("\n\rMotor speed set to %.01f%%.\n\r", speed * 100.0);
+                    break;
+                
+                case 'w':
+                    dir = 'f';
+                    turn = 'm';
+                    Mot_Ctrl(dir, turn, speed);
+                    uart_db.printf("\n\rMotor direction set to forward.\n\r");
+                    break;
+                
+                case 's':
+                    dir = 'b';
+                    turn = 'm';
+                    Mot_Ctrl(dir, turn, speed);
+                    uart_db.printf("\n\rMotor direction set to backward.\n\r");
+                    break;
+                
+                case 'a':
+                    turn = 'l';
+                    Mot_Ctrl(dir, turn, speed);
+                    uart_db.printf("\n\rMotor will turn left.\n\r");
+                    break;
+                
+                case 'd':
+                    turn = 'r';
+                    Mot_Ctrl(dir, turn, speed);
+                    uart_db.printf("\n\rMotor will turn right.\n\r");
+                    break;
+                
+                case 'p':
+                    dir = 's';
+                    turn = 'm';
+                    speed = 0;
+                    Mot_Ctrl(dir, turn, speed);
+                    uart_db.printf("\n\rMotor will stop.\n\r");
+                    break;
+                
+                case 'q':
+                    uart_db.printf("\n\rQuiting test...\n\r");
+                    Mot_StopVelocimeter();
+                    return;
+            }
+            wait_ms(1);
+        }
+        uart_db.printf("\n\r[Speed Info]\n\rMOT1 = %drpm, MOT2 = %drpm, MOT3 = %drpm, MOT4 = %drpm.\n\r", Mot_GetSpeed(1), Mot_GetSpeed(2), Mot_GetSpeed(3), Mot_GetSpeed(4));          
+    }
+}
+
+void DebugFunc_Servo(void)
+{
+    int cmdDebug;
+    int channel = 1;
+    float angle = 0;
+        
+    uart_db.printf("\n\r\n\r++++++++ Servo Test ++++++++\n\r\n\r");
+    uart_db.printf("Usage: 1~4.select servo channel, +.increase angle, -.decrease angle, \n\rd.set default angle, q.quit test\n\r");
+    uart_db.printf(" Input:");    
+    
+    while(1)
+    {    
+        cmdDebug = uart_db.getc();
+        switch(cmdDebug)
+        {
+            case '1':
+            case '2':
+            case '3':
+            case '4':
+                channel = cmdDebug - '0';
+                uart_db.printf("\n\rSelect servo channel <%d>.\n\r", channel);
+                break;
+                
+            case '+':
+                angle += 5;
+                if(angle > 90)
+                    angle = 90;
+                Servo_SetAngle(channel, angle);
+                uart_db.printf("\n\rServo position set to %.01f degrees.\n\r", angle);
+                break;
+
+            case '-':
+                angle -= 5;
+                if(angle < -90)
+                    angle = -90;
+                Servo_SetAngle(channel, angle);
+                uart_db.printf("\n\rServo position set to %.01f degrees.\n\r", angle);
+                break;
+
+            case 'd':
+                angle = 0;
+                Servo_SetAngle(channel, angle);
+                uart_db.printf("\n\rServo position set back to default.\n\r", angle);
+                break; 
+                
+            case 'q':
+                uart_db.printf("\n\rQuiting test...\n\r");
+                Mot_StopVelocimeter();
+                return;
+        }
+        wait_ms(1);
+    }
+}
+
+void DebugFunc_Us(void)
+{
+    uart_db.printf("Test function not implemented...\n\r");
+}
+
+void DebugFunc_Vmon(void)
+{
+    uart_db.printf("Test function not implemented...\n\r");
+}
+
+void DebugFunc_TestAll(void)
+{
+    SelfTest();
+}
+
+//Debug interface with text menu through serial port
+//This function is a loop with exit option
+void DebugCommander(void)
+{   
+    int quitFlag = 0, cmdDebug;
+    while(!quitFlag)
+    {             
+        uart_db.printf("\n\r\n\r============ RPi MOT HAT Debug Demo ============\n\r\n\r");
+        uart_db.printf(" 0. All function auto test.\n\r");        
+        uart_db.printf(" 1. Motor control test.\n\r");
+        uart_db.printf(" 2. Servo control test.\n\r");
+        uart_db.printf(" 3. Ultrasound rangefinder test.\n\r");
+        uart_db.printf(" 4. Power monitor test.\n\r");     
+        uart_db.printf(" q. Exit demo.\n\r\n\r");
+        uart_db.printf(" Input:");
+                              
+        while(!uart_db.readable());
+        cmdDebug = uart_db.getc();
+        
+        switch(cmdDebug)
+        {    
+            case '0':
+                DebugFunc_TestAll(); 
+                break;
+                
+            case '1':
+                DebugFunc_Motor();  
+                break;
+
+            case '2':
+                DebugFunc_Servo();
+                break;
+                
+            case '3':             
+                DebugFunc_Us();
+                break;
+                
+            case '4':
+                DebugFunc_Vmon();
+                break;
+                  
+            case 'q':
+                quitFlag = 1;
+                break;  
+            
+            default:
+                uart_db.printf("Incorrect input!\n\r");
+                break;
+        }        
+    }        
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AplicationLayer/DebugCommander.h	Thu Aug 20 11:36:01 2015 +0000
@@ -0,0 +1,8 @@
+#ifndef _APP_DB_CMD_
+#define _APP_DB_CMD_
+
+//Debug interface with text menu through serial port
+//This function is a loop with exit option
+extern void DebugCommander();
+
+#endif

--- a/AplicationLayer/SelfTest.cpp	Tue Aug 18 11:03:37 2015 +0000
+++ b/AplicationLayer/SelfTest.cpp	Thu Aug 20 11:36:01 2015 +0000
@@ -2,25 +2,25 @@
 
 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));  
+    uart_db.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)); 
+    uart_db.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");
+    uart_db.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");
+        uart_db.printf("\n\rMotor speed set to %d%%.\n\r", i);
+        uart_db.printf("-->Motor will move forward.\n\r");
         dir = 'f';
         
-        printf("---->Motor will move straight.\n\r");
+        uart_db.printf("---->Motor will move straight.\n\r");
         turn = 'm';
         Mot_Ctrl(dir, turn, (float)i / 100.0);
         SpeedTest();
@@ -28,7 +28,7 @@
         LedToggle(0);
         wait(3);
         
-        printf("---->Motor will turn left.\n\r");
+        uart_db.printf("---->Motor will turn left.\n\r");
         turn = 'l';
         Mot_Ctrl(dir, turn, (float)i / 100.0);
         SpeedTest();
@@ -36,7 +36,7 @@
         LedToggle(0);
         wait(3);
         
-        printf("---->Motor will turn right.\n\r");
+        uart_db.printf("---->Motor will turn right.\n\r");
         turn = 'r';
         Mot_Ctrl(dir, turn, (float)i / 100.0);
         SpeedTest();
@@ -44,10 +44,10 @@
         LedToggle(0);
         wait(3);
         
-        printf("-->Motor will move forward.\n\r");
+        uart_db.printf("-->Motor will move forward.\n\r");
         dir = 'b';
         
-        printf("---->Motor will move straight.\n\r");
+        uart_db.printf("---->Motor will move straight.\n\r");
         turn = 'm';
         Mot_Ctrl(dir, turn, (float)i / 100.0);
         SpeedTest();
@@ -55,7 +55,7 @@
         LedToggle(0);
         wait(3);
         
-        printf("---->Motor will turn left.\n\r");
+        uart_db.printf("---->Motor will turn left.\n\r");
         turn = 'l';
         Mot_Ctrl(dir, turn, (float)i / 100.0);
         SpeedTest();
@@ -63,7 +63,7 @@
         LedToggle(0);
         wait(3);
         
-        printf("---->Motor will turn right.\n\r");
+        uart_db.printf("---->Motor will turn right.\n\r");
         turn = 'r';
         Mot_Ctrl(dir, turn, (float)i / 100.0);
         SpeedTest();
@@ -76,12 +76,10 @@
 void SelfTest(void)
 #if 1
 {
-    printf("Starting motor velocimeter...\n\r");
+    uart_db.printf("Starting motor velocimeter...\n\r");
     Mot_StartVelocimeter();
-    while(1)
-    {
-        MotTest();
-    }   
+    MotTest();
+    Mot_StopVelocimeter();
 }
 #elif
 {
@@ -113,4 +111,4 @@
         wait_ms(200);
     }      
 }
-#endif
\ No newline at end of file
+#endif

--- a/AplicationLayer/SelfTest.h	Tue Aug 18 11:03:37 2015 +0000
+++ b/AplicationLayer/SelfTest.h	Thu Aug 20 11:36:01 2015 +0000
@@ -1,1 +1,7 @@
-void SelfTest(void);
\ No newline at end of file
+#ifndef _APP_SELF_TEST_
+#define _APP_SELF_TEST_
+
+//Test all functions
+void SelfTest(void);
+
+#endif

--- a/PeripheralLayer/MotHatLib.cpp	Tue Aug 18 11:03:37 2015 +0000
+++ b/PeripheralLayer/MotHatLib.cpp	Thu Aug 20 11:36:01 2015 +0000
@@ -16,6 +16,10 @@
 //DigitalOut led_mb(LED1, 0); //Nucleo
 DigitalOut led_mb(PB_12, 1); //MOT HAT
 
+//USART2 to PC for debug
+Serial uart_pc(SERIAL_TX, SERIAL_RX);
+SerialDummy uart_dummy;
+
 //GPIO for L293DD motor direction control
 //Forward = 2b'01, Backward = 2b'10, Stop = 2b'00
 BusOut mot1_dir(PB_3, PA_11), mot2_dir(PA_15, PA_12), mot3_dir(PD_2, PC_12), mot4_dir(PB_6, PB_7);
@@ -45,9 +49,9 @@
 PwmOut servo1_pwm(PB_14); //TIM15_CH1
 PwmOut servo2_pwm(PB_15); //TIM15_CH2
 //TIM16 PWM channels
-PwmOut servo3_pwm(PA_6); //TIM17_CH1
+PwmOut servo3_pwm(PB_8); //TIM16_CH1
 //TIM17 PWM channels
-PwmOut servo4_pwm(PA_7); //TIM17_CH1
+PwmOut servo4_pwm(PB_9); //TIM17_CH1
 
 //Position calibration for servos
 //ServoCal[x][0] is -90 degrees, ServoCal[x][1] is +90 degrees
@@ -64,7 +68,7 @@
     LedOnAll();
         
     //Init debug UART
-    //baud(115200);
+    uart_pc.baud(115200);
     
     //Init DC motors
     Mot_Init();
@@ -307,8 +311,8 @@
     spd4_in.disable_irq();
     
     timer_spd.stop();
-    for(int i=0; i<=MOT_NUM_MAX; i++)
-        MotSpdPulseTime[i] = 0;    
+    for(int i=1; i<=MOT_NUM_MAX; i++)
+        MotSpdPulseTime[i-1] = 0;    
 }
 
 int Mot_GetSpeed(int channel)
@@ -446,7 +450,7 @@
     switch(ch)
     {
         case 0:
-            led_mb = 1;
+            led_mb = 0;
             break;  
                         
         default:
@@ -459,7 +463,7 @@
     switch(ch)
     {
         case 0:
-            led_mb = 0;
+            led_mb = 1;
             break; 
             
         default:

--- a/PeripheralLayer/MotHatLib.h	Tue Aug 18 11:03:37 2015 +0000
+++ b/PeripheralLayer/MotHatLib.h	Thu Aug 20 11:36:01 2015 +0000
@@ -1,6 +1,17 @@
 #ifndef _PE_BD_LIB_
 #define _PE_BD_LIB_
 
+class SerialDummy
+{
+    public:
+        void baud(int baudrate) {}
+        int printf(const char *format, ...) {return 0;}        
+        int readable() {return 0;}
+        int writeable() {return 0;}
+        int putc(int ch) {return 0;}
+        int getc() {return 0;}        
+};
+
 //Number of on-board devices
 extern const int LED_NUM_MAX;
 extern const int MOT_NUM_MAX;
@@ -14,6 +25,11 @@
 //LEDs on mother board and daughter board
 extern DigitalOut led_mb;
 
+//USART2 to PC for debug
+
+extern Serial uart_pc;
+extern SerialDummy uart_dummy;
+
 //GPIO for L293DD motor direction control
 //Forward = 2b'01, Backward = 2b'10, Stop = 2b'00
 extern BusOut mot1_dir, mot2_dir, mot3_dir, mot4_dir;
@@ -73,4 +89,4 @@
 extern void LedOffAll(void);
 extern void LedOnAll(void);
 
-#endif
\ No newline at end of file
+#endif

--- a/SysConfig.h	Tue Aug 18 11:03:37 2015 +0000
+++ b/SysConfig.h	Thu Aug 20 11:36:01 2015 +0000
@@ -3,7 +3,17 @@
 #include "mbed.h"
 
 //Application Layer Modules
+#include "DebugCommander.h"
 #include "SelfTest.h"
 
 //Peripheral Layer Modules
-#include "MotHatLib.h"
\ No newline at end of file
+#include "MotHatLib.h"
+
+//Macro to enable debug serial port
+#define DEBUG_SERIAL
+
+#ifdef DEBUG_SERIAL
+    #define uart_db uart_pc
+#else
+    #define uart_db uart_dummy
+#endif

--- a/main.cpp	Tue Aug 18 11:03:37 2015 +0000
+++ b/main.cpp	Thu Aug 20 11:36:01 2015 +0000
@@ -2,12 +2,37 @@
 
 void SystemHalt(void)
 {   
-    printf("\n\rOops! System halted! O_o\n\r");
+    uart_db.printf("\n\rOops! System halted! O_o\n\r");
     LedOffAll();
     while(1)
     {
-        LedToggle(0);
-        wait_ms(1000);
+        //Breath LED
+        int i, j;
+        led_mb = 0;
+        while(1)
+        {
+            for(j=0;j<=100;j++)
+            {            
+                for(i=0;i<=100;i++)
+                {
+                    led_mb = 1;
+                    wait_us(j);
+                    led_mb = 0;
+                    wait_us(100-j);
+                }
+            }
+            for(j=100;j>=0;j--)
+            {            
+                for(i=0;i<=100;i++)
+                {
+                    led_mb = 1;
+                    wait_us(j);
+                    led_mb = 0;
+                    wait_us(100-j);
+                }
+            }
+            wait_ms(200);
+        } 
     }
 }
 
@@ -31,8 +56,8 @@
     if(AppLayerInit() != 0) 
         SystemHalt(); 
     
-    printf("System init done!\n\r");
-    SelfTest();  
+    uart_db.printf("System init done!\n\r");
+    DebugCommander();  
     
     //Program should never go to here
     SystemHalt();