File content as of revision 4:1a6ba05e7736:

#include "mbed.h"
#include "CURRENT_CONTROL.h"


CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
                                 PinName PwmChannel1,
                                 PinName PwmChannel2,
                                 PWMIndex pwmIndex,
                                 float Kp, float Ki, float Kd,
                                 float samplingTime) : currentAnalogIn(curChannel),
    MotorPlus(PwmChannel1),
    MotorMinus(PwmChannel2),
    pid(Kp,Ki,Kd,samplingTime),
    lpFilter(samplingTime)
    
{
    pwmIndex_ = pwmIndex;

    Ts = samplingTime;
    //setup motor PWM parameters
    MotorPlus.period_us(15);//default period equals to 25 microseconds
    MotorPlus.write(0.5);   //duty ratio = 0.5 in complementary output mode -> static
    if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
    else if(pwmIndex_ == PWM2)TIM1->CCER |= 64; //enable complimentary output
    
    
    
}

float CURRENT_CONTROL::saturation(float input, float limit_H, float limit_L)
{
    float output;
    if(input < limit_H && input > limit_L)output = input;
    else output = input >=limit_H?limit_H:limit_L;
    return output;
}

void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
{
    analogInValue = currentAnalogIn.read();
    curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur, 10);
    pid.Compute(curRef, curFeedBack);
    MotorPlus = 0.5 + saturation((-pid.output + Kw*angularSpeed)*voltage2Duty, 0.5, -0.5) ;
    if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
    else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
}

void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
{
    analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
    Kw = angSpeed2BackEmf;
    voltage2Duty = voltage2DutyRatio;
    
}


//****************for test************************//
void CURRENT_CONTROL::SetPWMDuty(float ratio)
{
    MotorPlus = ratio;
    if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
    else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
}

float CURRENT_CONTROL::GetAnalogIn(void)
{
    return analogInValue = currentAnalogIn.read();   
}

float CURRENT_CONTROL::GetCurrent(void)
{
    return  curFeedBack;   
}

//=====================LPF ====================//
LPF::LPF(float samplingTime)
{
    Ts = samplingTime;
}

float LPF::filter(float input, float cutOff)
{
    output = (1.0 - cutOff*Ts)*outputLast + cutOff*Ts*input;
    outputLast = output;
    return output;
}