File content as of revision 3:171f4d0ca77b:

#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;
        }        
    }        
}