File content as of revision 6:bae35ca64f10:

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

//=====================LPF ====================//
LPF::LPF(float samplingTime, float cutOff_freq_Hz_in)
{
    Ts = samplingTime;
    cutOff_freq_Hz = cutOff_freq_Hz_in;
    alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts;
}

float LPF::filter(float input)
{
    // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
    output += alpha_Ts*(input - output);
    return output;
}


//================== CURRENT_CONTROL =================//
CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
                                 PinName PwmChannel1,
                                 PinName PwmChannel2,
                                 PWMIndex pwmIndex,
                                 float Kp, float Ki, float Kd, float Ka,
                                 float samplingTime) : 
    currentAnalogIn(curChannel),
    MotorPlus(PwmChannel1),
    MotorMinus(PwmChannel2),
    pid(Kp,Ki,Kd,samplingTime),
    lpFilter(samplingTime, 1.5915) // 10 rad/s
    
{
    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
    
    
    //
    currentOffset = 0.0;
    delta_output = 0.0;
    
    // Set PID's parameters                        
    /////////////////////
    pid.Kp = Kp;
    pid.Ki = Ki;
    pid.Kd = Kd;
    pid.Ka = Ka; // Gain for anti-windup // Ka = 0.0017
}
//
void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
{
    analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
    Ke = angSpeed2BackEmf;
    voltage2Duty = voltage2DutyRatio;
    
}
//
float CURRENT_CONTROL::saturation(float input_value, float &delta, const float &limit_H, const float &limit_L){
    if( input_value > limit_H ){
        delta = limit_H - input_value;
        input_value = limit_H;
    }else if( input_value < limit_L ){  
        delta = limit_L - input_value;
        input_value = limit_L; 
    }else{
        delta = 0.0;
    }
    return input_value;
}
//
void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
{
    analogInValue = currentAnalogIn.read();
    
    // Filter
    curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur);
    
    // PID
    pid.Compute_noWindUP(curRef, curFeedBack);
    
    // Output saturation and unit changing
    MotorPlus = 0.5 + saturation((-pid.output + func_back_emf(angularSpeed) )*voltage2Duty, delta_output, 0.5, -0.5) ;
    
    // Anti-windup
    pid.Anti_windup(delta_output);
    
    // Setting up complementary PWM
    if(pwmIndex_ == PWM1){
        TIM1->CCER |= 4;
    }else if(pwmIndex_ == PWM2){
        TIM1->CCER |= 64;
    }
}
// Back emf as the function of rotational speed
float CURRENT_CONTROL::func_back_emf(const float &W_in){
    return (Ke*W_in);
}

//****************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;   
}