DebugCommander.cpp

00001 #include "SysConfig.h"
00002 
00003 void LedFlasher(void)
00004 {
00005     int ledNumOn;
00006     for(ledNumOn = 1; ledNumOn < LED_NUM_MAX; ledNumOn++)
00007     {
00008         LedOffAll();
00009         LedOn(ledNumOn);
00010         wait_ms(300);
00011     }
00012 }
00013 
00014 void DebugFunc_Motor(void)
00015 {
00016     int cmdDebug;
00017     int dir = 's', turn = 'm';
00018     float speed = 0;
00019     
00020     Mot_StartVelocimeter();
00021     
00022     uart_db.printf("\n\r\n\r++++++++ Motor Test ++++++++\n\r\n\r");
00023     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");
00024     uart_db.printf(" Input:");    
00025     
00026     while(1)
00027     {             
00028         for(int i=0;i<3000;i++)
00029         {
00030             cmdDebug = uart_db.getc();
00031             switch(cmdDebug)
00032             {
00033                 case '+':
00034                     speed += 0.1;
00035                     if(speed > 1)
00036                         speed = 1;
00037                     Mot_Ctrl(dir, turn, speed);
00038                     uart_db.printf("\n\rMotor speed set to %.01f%%.\n\r", speed * 100.0);
00039                     break;
00040                 
00041                 case '-':
00042                     speed -= 0.1;
00043                     if(speed < 0)
00044                         speed = 0;
00045                     Mot_Ctrl(dir, turn, speed);
00046                     uart_db.printf("\n\rMotor speed set to %.01f%%.\n\r", speed * 100.0);
00047                     break;
00048                 
00049                 case 'w':
00050                     dir = 'f';
00051                     turn = 'm';
00052                     Mot_Ctrl(dir, turn, speed);
00053                     uart_db.printf("\n\rMotor direction set to forward.\n\r");
00054                     break;
00055                 
00056                 case 's':
00057                     dir = 'b';
00058                     turn = 'm';
00059                     Mot_Ctrl(dir, turn, speed);
00060                     uart_db.printf("\n\rMotor direction set to backward.\n\r");
00061                     break;
00062                 
00063                 case 'a':
00064                     turn = 'l';
00065                     Mot_Ctrl(dir, turn, speed);
00066                     uart_db.printf("\n\rMotor will turn left.\n\r");
00067                     break;
00068                 
00069                 case 'd':
00070                     turn = 'r';
00071                     Mot_Ctrl(dir, turn, speed);
00072                     uart_db.printf("\n\rMotor will turn right.\n\r");
00073                     break;
00074                 
00075                 case 'p':
00076                     dir = 's';
00077                     turn = 'm';
00078                     speed = 0;
00079                     Mot_Ctrl(dir, turn, speed);
00080                     uart_db.printf("\n\rMotor will stop.\n\r");
00081                     break;
00082                 
00083                 case 'q':
00084                     uart_db.printf("\n\rQuiting test...\n\r");
00085                     Mot_StopVelocimeter();
00086                     return;
00087             }
00088             wait_ms(1);
00089         }
00090         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));          
00091     }
00092 }
00093 
00094 void DebugFunc_Servo(void)
00095 {
00096     int cmdDebug;
00097     int channel = 1;
00098     float angle = 0;
00099         
00100     uart_db.printf("\n\r\n\r++++++++ Servo Test ++++++++\n\r\n\r");
00101     uart_db.printf("Usage: 1~4.select servo channel, +.increase angle, -.decrease angle, \n\rd.set default angle, q.quit test\n\r");
00102     uart_db.printf(" Input:");    
00103     
00104     while(1)
00105     {    
00106         cmdDebug = uart_db.getc();
00107         switch(cmdDebug)
00108         {
00109             case '1':
00110             case '2':
00111             case '3':
00112             case '4':
00113                 channel = cmdDebug - '0';
00114                 uart_db.printf("\n\rSelect servo channel <%d>.\n\r", channel);
00115                 break;
00116                 
00117             case '+':
00118                 angle += 5;
00119                 if(angle > 90)
00120                     angle = 90;
00121                 Servo_SetAngle(channel, angle);
00122                 uart_db.printf("\n\rServo position set to %.01f degrees.\n\r", angle);
00123                 break;
00124 
00125             case '-':
00126                 angle -= 5;
00127                 if(angle < -90)
00128                     angle = -90;
00129                 Servo_SetAngle(channel, angle);
00130                 uart_db.printf("\n\rServo position set to %.01f degrees.\n\r", angle);
00131                 break;
00132 
00133             case 'd':
00134                 angle = 0;
00135                 Servo_SetAngle(channel, angle);
00136                 uart_db.printf("\n\rServo position set back to default.\n\r", angle);
00137                 break; 
00138                 
00139             case 'q':
00140                 uart_db.printf("\n\rQuiting test...\n\r");
00141                 Mot_StopVelocimeter();
00142                 return;
00143         }
00144         wait_ms(1);
00145     }
00146 }
00147 
00148 void DebugFunc_Us(void)
00149 {
00150     uart_db.printf("Test function not implemented...\n\r");
00151 }
00152 
00153 void DebugFunc_Vmon(void)
00154 {
00155     uart_db.printf("Test function not implemented...\n\r");
00156 }
00157 
00158 void DebugFunc_TestAll(void)
00159 {
00160     SelfTest();
00161 }
00162 
00163 //Debug interface with text menu through serial port
00164 //This function is a loop with exit option
00165 void DebugCommander(void)
00166 {   
00167     int quitFlag = 0, cmdDebug;
00168     while(!quitFlag)
00169     {             
00170         uart_db.printf("\n\r\n\r============ RPi MOT HAT Debug Demo ============\n\r\n\r");
00171         uart_db.printf(" 0. All function auto test.\n\r");        
00172         uart_db.printf(" 1. Motor control test.\n\r");
00173         uart_db.printf(" 2. Servo control test.\n\r");
00174         uart_db.printf(" 3. Ultrasound rangefinder test.\n\r");
00175         uart_db.printf(" 4. Power monitor test.\n\r");     
00176         uart_db.printf(" q. Exit demo.\n\r\n\r");
00177         uart_db.printf(" Input:");
00178                               
00179         while(!uart_db.readable());
00180         cmdDebug = uart_db.getc();
00181         
00182         switch(cmdDebug)
00183         {    
00184             case '0':
00185                 DebugFunc_TestAll(); 
00186                 break;
00187                 
00188             case '1':
00189                 DebugFunc_Motor();  
00190                 break;
00191 
00192             case '2':
00193                 DebugFunc_Servo();
00194                 break;
00195                 
00196             case '3':             
00197                 DebugFunc_Us();
00198                 break;
00199                 
00200             case '4':
00201                 DebugFunc_Vmon();
00202                 break;
00203                   
00204             case 'q':
00205                 quitFlag = 1;
00206                 break;  
00207             
00208             default:
00209                 uart_db.printf("Incorrect input!\n\r");
00210                 break;
00211         }        
00212     }        
00213 }
00214