File content as of revision 1:a6bcf44b90df:

#include "SysConfig.h"

//Number of on-board devices
const int LED_NUM_MAX = 1;
const int MOT_NUM_MAX = 4;
const int SRV_NUM_MAX = 4;
const int VMON_NUM_MAX = 4;

//The speed difference rate of left/right side motors when the vehicle turns head
const int MOT_TURN_RATE = 4;

//Fixed PWM period of servos
const int SRV_PERIOD_US = 20000;

//LEDs on mother board and daughter board
//DigitalOut led_mb(LED1, 0); //Nucleo
DigitalOut led_mb(PB_12, 1); //MOT HAT

//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);

//GPIO for motor velocimeter
InterruptIn spd1_in(PF_6), spd2_in(PF_7), spd3_in(PF_5), spd4_in(PC_4);
//GPIO for ultrasound rangefinder
InterruptIn us1_echo(PC_10), us2_echo(PC_5);
DigitalOut us1_trig(PC_11), us2_trig(PF_4);

//Timers for speed and distance counter
Timer timer_spd, timer_us;

//Analog inputs for power voltage monitoring
AnalogIn vin_bat(PC_0); //ADC1_CH10
AnalogIn vin_12v(PC_1); //ADC1_CH11
AnalogIn vin_5v(PC_2);  //ADC1_CH12
AnalogIn vin_3v3(PC_3);  //ADC1_CH13

//TIM3 PWM channels
PwmOut mot1_pwm(PB_5); //TIM3_CH2
PwmOut mot2_pwm(PB_4); //TIM3_CH1
PwmOut mot3_pwm(PB_0); //TIM3_CH3
PwmOut mot4_pwm(PC_9); //TIM3_CH4

//TIM15 PWM channels
PwmOut servo1_pwm(PB_14); //TIM15_CH1
PwmOut servo2_pwm(PB_15); //TIM15_CH2
//TIM16 PWM channels
PwmOut servo3_pwm(PA_6); //TIM17_CH1
//TIM17 PWM channels
PwmOut servo4_pwm(PA_7); //TIM17_CH1

//Position calibration for servos
//ServoCal[x][0] is -90 degrees, ServoCal[x][1] is +90 degrees
const int ServoCal[][2] = {{1000, 2000}, {1000, 2000}, {1000, 2000}, {1000, 2000}};

//Voltage division ratios
const float VmonDivision[] = {0.1, 0.1, 0.5, 0.5};

//Init board library
//--> Success return 0
int BoardLibInit(void)
{    
    //Init LED status
    LedOnAll();
        
    //Init debug UART
    //baud(115200);
    
    //Init DC motors
    Mot_Init();
    
    //Init servos
    Servo_Init();
    
    //Return 0 if success
    return 0; 
}

/*********************************************/
/*         Voltage Monitor Functions         */
/*********************************************/
float Vmon_ReadVolt(int channel)
{    
    switch(channel)
    {
        case 1:
            return vin_bat.read() * 3.3 / VmonDivision[channel - 1];
            
        case 2:
            return vin_12v.read() * 3.3 / VmonDivision[channel - 1];
            
        case 3:
            return vin_5v.read() * 3.3 / VmonDivision[channel - 1];
            
        case 4:
            return vin_3v3.read() * 3.3 / VmonDivision[channel - 1];
            
        default:
            return 0;
    }
}


/*********************************************/
/*         DC Motor Control Functions        */
/*********************************************/
//Init the PWM and direction of motors to STOP state
void Mot_Init(void)
{
    for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
    {
        Mot_PwmSetup(motNum, 1000, 0); //Freq = 1KHz, Speed = 0%
        Mot_SetDirection(motNum, 's'); //Direction = STOP
    }
}

//Control the movement of vehicle
//dir: forward = 'f', backward = 'b', stop  = 's'
//turn: left = 'l', right = 'r', fixed turning rate
//speed: 0.0~1.0 (0~100%, 0.245 = 24.5%)
void Mot_Ctrl(char dir, char turn, float speed)
{
    if(speed < 0)
        speed = 0;
    if(speed > 1)
        speed = 1;
    
    switch(turn)
    {
        case 'm':
            Mot_PwmWrite(1, speed);
            Mot_SetDirection(1, dir);
            Mot_PwmWrite(2, speed);
            Mot_SetDirection(2, dir);
            
            if(MOT_NUM_MAX > 2)
            {
                Mot_PwmWrite(3, speed);
                Mot_SetDirection(3, dir);
                Mot_PwmWrite(4, speed);
                Mot_SetDirection(4, dir); 
            }            
            break;
            
        case 'l':
            Mot_PwmWrite(1, speed / MOT_TURN_RATE);
            Mot_SetDirection(1, dir);
            Mot_PwmWrite(2, speed);
            Mot_SetDirection(2, dir);
            
            if(MOT_NUM_MAX > 2)
            {
                Mot_PwmWrite(3, speed / MOT_TURN_RATE);
                Mot_SetDirection(3, dir);
                Mot_PwmWrite(4, speed);
                Mot_SetDirection(4, dir); 
            }            
            break;
            
        case 'r':
            Mot_PwmWrite(1, speed);
            Mot_SetDirection(1, dir);
            Mot_PwmWrite(2, speed / MOT_TURN_RATE);
            Mot_SetDirection(2, dir);
            
            if(MOT_NUM_MAX > 2)
            {
                Mot_PwmWrite(3, speed);
                Mot_SetDirection(3, dir);
                Mot_PwmWrite(4, speed / MOT_TURN_RATE);
                Mot_SetDirection(4, dir); 
            }
            break;
            
        case 's':
            for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
            {
                Mot_PwmWrite(motNum, 0);
                Mot_SetDirection(motNum, 's');
            }
            break;
    }    
}

//channel: 1~4, period: us, duty: 0.0~1.0 (0.245 = 24.5%)
void Mot_PwmSetup(int channel, int period, float duty)
{
    if(duty < 0)
        duty = 0;
    else if(duty > 1)
        duty = 1;
    
    switch(channel)
    {
        case 1:
            mot1_pwm.period_us(period);
            mot1_pwm = duty;
            break;
        case 2:   
            mot2_pwm.period_us(period);  
            mot2_pwm = duty;
            break;  
        case 3:
            mot3_pwm.period_us(period);
            mot3_pwm = duty;
            break;
        case 4:
            mot4_pwm.period_us(period);
            mot4_pwm = duty;
            break;
            
        default:
            break;
    }
}

//channel: 1~4, duty: 0.0~1.0 (0.245 = 24.5%)
void Mot_PwmWrite(int channel, float duty)
{
    if(duty < 0)
        duty = 0;
    else if(duty > 1)
        duty = 1;
    
    switch(channel)
    {
        //All channels in TIM3 share the same period
        case 1:
            mot1_pwm = duty;
            break;
        case 2:            
            mot2_pwm = duty;           
            break;
        case 3:
            mot3_pwm = duty;
            break;
        case 4:
            mot4_pwm = duty;
            break;    
            
        default:
            break;
    }
}

//channel: 1~4, dir: forward = 'f', backward = 'b', stop  = 's'
void Mot_SetDirection(int channel, char dir)
{
    switch(channel)    
    {
        case 1:
            if(dir == 'f')
                mot1_dir = 0x1;
            else if(dir == 'b')
                mot1_dir = 0x2;
            else
                mot1_dir = 0x0;
            break;
        case 2:            
            if(dir == 'f')
                mot2_dir = 0x1;
            else if(dir == 'b')
                mot2_dir = 0x2;
            else
                mot2_dir = 0x0;        
            break;
        case 3:
            if(dir == 'f')
                mot3_dir = 0x1;
            else if(dir == 'b')
                mot3_dir = 0x2;
            else
                mot3_dir = 0x0;
            break;
        case 4:
            if(dir == 'f')
                mot4_dir = 0x1;
            else if(dir == 'b')
                mot4_dir = 0x2;
            else
                mot4_dir = 0x0;
            break;      
    }
}

/*********************************************/
/*        Motor Velocimetoer Functions       */
/*********************************************/
int MotSpdPulseTime[] = {0, 0, 0, 0};
int MotSpeed[] = {0, 0, 0, 0};

void Mot_StartVelocimeter(void)
{
    timer_spd.start(); 
    
    spd1_in.rise(&Mot_SpdPluseIntHandler1);
    spd2_in.rise(&Mot_SpdPluseIntHandler2);
    spd3_in.rise(&Mot_SpdPluseIntHandler3);
    spd4_in.rise(&Mot_SpdPluseIntHandler4);    
}

void Mot_StopVelocimeter(void)
{
    spd1_in.disable_irq();
    spd2_in.disable_irq();
    spd3_in.disable_irq();
    spd4_in.disable_irq();
    
    timer_spd.stop();
    for(int i=0; i<=MOT_NUM_MAX; i++)
        MotSpdPulseTime[i] = 0;    
}

int Mot_GetSpeed(int channel)
{
    if(channel < 1)
        channel = 1;
    else if(channel > MOT_NUM_MAX)
        channel = MOT_NUM_MAX;
        
    return MotSpeed[channel - 1];
}

void Mot_SpdPluseIntHandler1(void)
{
    int time = timer_spd.read_ms();
    MotSpeed[0] = 3000 / (time - MotSpdPulseTime[0]);
    MotSpdPulseTime[0] = time;
}

void Mot_SpdPluseIntHandler2(void)
{
    int time = timer_spd.read_ms();
    MotSpeed[1] = 3000 / (time - MotSpdPulseTime[1]);
    MotSpdPulseTime[1] = time;
}

void Mot_SpdPluseIntHandler3(void)
{
    int time = timer_spd.read_ms();
    MotSpeed[2] = 3000 / (time - MotSpdPulseTime[2]);
    MotSpdPulseTime[2] = time;
}

void Mot_SpdPluseIntHandler4(void)
{
    int time = timer_spd.read_ms();
    MotSpeed[3] = 3000 / (time - MotSpdPulseTime[3]);
    MotSpdPulseTime[3] = time;
}

/*********************************************/
/*          Servo Control Functions          */
/*********************************************/
//Init the PWM and direction of motors to STOP state
void Servo_Init(void)
{
    for(int servoNum = 1; servoNum <= SRV_NUM_MAX; servoNum++)
    {
        Servo_SetAngle(servoNum, 0);
    }
}

//channel: 1~4, angle: -90.0~+90.0
void Servo_SetAngle(int channel, float angle)
{
    if(channel > 0 && channel <= SRV_NUM_MAX)
    {
        int pulse = (int)((angle + 90.0) * (ServoCal[channel - 1][1] - ServoCal[channel-1][0]) / 180.0 + ServoCal[channel-1][0]);
        Servo_PwmWrite(channel, pulse);
    }
}

//channel: 1~4, pulse_us: 0~20000us
void Servo_PwmSetup(int channel, int pulse_us)
{  
    if(pulse_us < 0)
        pulse_us = 0;
    else if(pulse_us > 20000)
        pulse_us = 20000;
          
    switch(channel)
    {
        case 1:
            servo1_pwm.period_us(SRV_PERIOD_US);
            servo1_pwm.pulsewidth_us(pulse_us);
            break;
        case 2:   
            servo2_pwm.period_us(SRV_PERIOD_US);  
            servo2_pwm.pulsewidth_us(pulse_us);
            break;  
        case 3:
            servo3_pwm.period_us(SRV_PERIOD_US);
            servo3_pwm.pulsewidth_us(pulse_us);
            break;
        case 4:
            servo4_pwm.period_us(SRV_PERIOD_US);
            servo4_pwm.pulsewidth_us(pulse_us);
            break;
            
        default:
            break;
    }
}

//channel: 1~4, pulse_us: 0~20000us
void Servo_PwmWrite(int channel, int pulse_us)
{
    if(pulse_us < 0)
        pulse_us = 0;
    else if(pulse_us > 20000)
        pulse_us = 20000;
    
    switch(channel)
    {
        //All channels in TIM3 share the same period
        case 1:
            servo1_pwm.pulsewidth_us(pulse_us);
            break;
        case 2:            
            servo2_pwm.pulsewidth_us(pulse_us);
            break;
        case 3:
            servo3_pwm.pulsewidth_us(pulse_us);
            break;
        case 4:
            servo4_pwm.pulsewidth_us(pulse_us);
            break;    
            
        default:
            break;
    }
}

/*********************************************/
/*      Ultrasound Rangefinder Functions     */
/*********************************************/
//xxx


/*********************************************/
/*          LED Control Functions            */
/*********************************************/
void LedOn(int ch)
{
    switch(ch)
    {
        case 0:
            led_mb = 1;
            break;  
                        
        default:
            break;   
    }           
}

void LedOff(int ch)
{
    switch(ch)
    {
        case 0:
            led_mb = 0;
            break; 
            
        default:
            break;   
    }           
}

void LedToggle(int ch)
{
    switch(ch)
    {
        case 0:
            led_mb = !led_mb;
            break;  
                        
        default:
            break;   
    }           
}

void LedOnAll(void)
{
    for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
        LedOn(ledNum);    
}

void LedOffAll(void)
{
    for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
        LedOff(ledNum);    
}