File content as of revision 1:681973914ddd:

#include "mtrctl.h"

/**********************************************************************************
* Object declaration
***********************************************************************************/
PwmOut MTRCTL__xPwm[nNoOfMotors] = { D5, D7 };
DigitalOut MTRCTL__xEnable[nNoOfMotors] = { D4, D6 };

/**********************************************************************************
* Local variable declaration
***********************************************************************************/
static uint16_t MTRCTL__MaxSpeed = MAX_POWER;

void MTRCTL_vInit(void)
{
    uint8_t u8MotorIndex = 0;
    /* Initial pwm frequency */
    for( u8MotorIndex = 0; u8MotorIndex < (uint8_t)(nNoOfMotors); u8MotorIndex++ )
    {
        MTRCTL__xPwm[u8MotorIndex].period_us(OP_PERIOD);
        MTRCTL__xEnable[u8MotorIndex] = 1;
    } 
}

void MTRCTL_vSetMaxSpeed( uint16_t u16MaxSpeed )
{
    MTRCTL__MaxSpeed = u16MaxSpeed;
}

void MTRCTL_vSetMotorPower( uint16_t u16InPwr, MTRCTL_tenMotors enMotor )
{
    uint8_t u8MotorIndex = 0;
    for( u8MotorIndex = 0; u8MotorIndex < (uint8_t)(nNoOfMotors); u8MotorIndex++ )
    {
        if( u8MotorIndex == (uint8_t)(enMotor) )
        {
            if(  u16InPwr < MAX_POWER )
            {
                MTRCTL__xPwm[u8MotorIndex].write(((float)u16InPwr/MAX_POWER)*((float)MTRCTL__MaxSpeed/MAX_POWER));
            }
            else
            {
                 MTRCTL__xPwm[u8MotorIndex].write(1.0);    
            }
        }
        else
        {
            /* Motor not found, do nothing */    
        }   
    }    
}

void MTRCTL_vSetSystemSpeed( uint16_t u16Speed, uint8_t u8Direction )
{
    uint16_t u16RightCompensation = 0;
    uint16_t u16LeftCompensation = 0;
    uint16_t u16CompError = 0;
    uint16_t u16FinalSpeed = 0;
    
    u16RightCompensation = (uint16_t)( SCALE_DIR2SPEED * u8Direction );
    u16LeftCompensation = (uint16_t)( SCALE_DIR2SPEED * (MAX_DIRECTION_VAL - u8Direction) );
    
    if( u16RightCompensation >  u16LeftCompensation )
    {
        u16CompError = SCALE_DIR2SPEED * ( u16RightCompensation - u16LeftCompensation );
    }
    else
    {
        u16CompError = SCALE_DIR2SPEED * ( u16LeftCompensation - u16RightCompensation );
    }
    
    if( u16Speed > u16CompError )
    {
        u16FinalSpeed = u16Speed - u16CompError;
    }
    else
    {
        u16FinalSpeed = MIN_POWER;
    }
    
    MTRCTL_vSetMaxSpeed(u16FinalSpeed);
    MTRCTL_vSetMotorPower( u16RightCompensation , nRightMotor );
    MTRCTL_vSetMotorPower( u16LeftCompensation , nLeftMotor );
}