File content as of revision 2:cff0fa966e58:

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