File content as of revision 14:67fc256efeb7:

#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;
    One_alpha_Ts = 1.0 - alpha_Ts;
    output = 0.0;

    //
    Flag_Init = false;
}

float LPF::filter(float input)
{
    // Initialization
    if (!Flag_Init){
        reset(input);
        Flag_Init = true;
        return output;
    }

    // output = One_alpha_Ts*output + alpha_Ts*input;
    output += alpha_Ts*(input - output);

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


//================== CURRENT_CONTROL =================//
CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
                                 PinName PwmChannel1,
                                 PinName PwmChannel2,
                                 PWMIndex pwmIndex,
                                 PinName QEI_A,
                                 PinName QEI_B,
                                 float pulsesPerRev,
                                 int arraysize,
                                 float samplingTime) :
    currentAnalogIn(curChannel),
    MotorPlus(PwmChannel1),
    MotorMinus(PwmChannel2),
    wheelSpeed(QEI_A, QEI_B, NC, pulsesPerRev, arraysize, samplingTime, QEI::X4_ENCODING), //(pin1, pin2, pin3, pulsesPerRev, arraysize, sampletime, pulses)
    pid(0.0,0.0,0.0,samplingTime),
    lpFilter(samplingTime, 70.0) // 1.5915 Hz = 10 rad/s


{
    pwmIndex_ = pwmIndex;

    Ts = samplingTime;

    //setup motor PWM parameters
    MotorPlus.period_us(50); //set the pwm period = 50 us, where the frequency = 20kHz
    //
    SetPWMDuty(0.5);

    //
    Flag_Init = false;
    Init_count = 0;
    Accumulated_offset = 0.0;

    //
    currentOffset = 0.0;
    //
    delta_output = 0.0;
    curFeedBack = 0.0;
    curFeedBack_filter = 0.0;
    curCommand = 0.0;
    voltage_out = 0.0;

    // Set PID's parameters
    /////////////////////
    pid.Kp = 0.0;
    pid.Ki = 0.0;
    pid.Kd = 0.0;
    pid.Ka = 0.0; // Gain for anti-windup // Ka = 0.0017

    // Speed
    angularSpeed = 0.0;
    Flag_SpeedCal_Iterated = false;

    // Reverse flage for each signal
    reverse_current = false;
    reverse_rotationalSpeed = false;
    reverse_voltage = false;
}
//
void CURRENT_CONTROL::SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in)
{
    analog2Cur = Analog2Cur_in;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
    Ke = angSpeed2BackEmf;
    Kt = Ke;
    Kt_inv = 1/Kt;
    voltage2Duty = voltage2Duty_in;
}
void CURRENT_CONTROL::SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in)
{
    reverse_current = reverse_current_in;
    reverse_rotationalSpeed = reverse_rotationalSpeed_in;
    reverse_voltage = reverse_voltage_in;
}
//
void CURRENT_CONTROL::SetGain(float Kp, float Ki, float Kd, float Ka)
{
    // 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::OffsetInit(void){
    if (Flag_Init) return;
    //

    Init_count++;
    /*
    Accumulated_offset += GetAnalogIn();

    if (Init_count >= 500){ // Fixed time
        currentOffset = Accumulated_offset / float(Init_count);
        Flag_Init = true;
    }
    */

    //
    /*
    if (Init_count <= 10){
        Accumulated_offset = GetAnalogIn();
    }else{
        // Accumulated_offset +=  0.0063*(GetAnalogIn() - Accumulated_offset); // fc = 1 Hz
        Accumulated_offset = 0.9937*Accumulated_offset + 0.0063*GetAnalogIn(); // fc = 1 Hz
    }

    if (Init_count >= 300){ // Fixed time: 500 samples
        currentOffset = Accumulated_offset;
        Flag_Init = true;
    }
    */


    //
    if (Init_count >= 10){ // Fixed time: 10 samples
        currentOffset = GetAnalogIn();
        Flag_Init = true;
    }


}

//
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;
}
// Speed_IterateOnce
////////////////////////////////
float CURRENT_CONTROL::Speed_IterateOnce(void){
    // The flag "Flag_SpeedCal_Iterated" will be resetted at the end of function Control()
    if(Flag_SpeedCal_Iterated) return;

    // Speed
    wheelSpeed.Calculate();

    //
    if (reverse_rotationalSpeed)
        angularSpeed = -wheelSpeed.getAngularSpeed();
    else
        angularSpeed = wheelSpeed.getAngularSpeed();
    //

    // Flag
    Flag_SpeedCal_Iterated = true;

    return angularSpeed;
}
float CURRENT_CONTROL::getAngularSpeed(void){
    // return wheelSpeed.getAngularSpeed();
    return angularSpeed;
}
float CURRENT_CONTROL::getAngularSpeed_deg_s(void){

    if (reverse_rotationalSpeed)
        return -wheelSpeed.getAngularSpeed_deg_s();
    else
        return wheelSpeed.getAngularSpeed_deg_s();
    //
    // return wheelSpeed.getAngularSpeed_deg_s();
}
//////////////////////////////// end Speed_IterateOnce


// Control
///////////////////////////////////////////////////////////
void CURRENT_CONTROL::TorqueControl(float TorqueRef, bool enable){
    Control(Kt_inv*TorqueRef, enable);
}
void CURRENT_CONTROL::Control(float curRef, bool enable)
{
    curCommand = curRef;
    // Init check
    OffsetInit();
    if (!Flag_Init) return;
    //////////////////////////////////////

    // Get measurement
    // Speed
    Speed_IterateOnce();
    // Motor current
    GetCurrent();

    //--------------------------------//

    if (enable){
        // PID
        pid.Compute_noWindUP(curRef, curFeedBack_filter);

        // Voltage output with back-emf compensation
        voltage_out = pid.output + func_back_emf(angularSpeed);
        // voltage_out = 0.0 + func_back_emf(angularSpeed);

        // Controlller output
        SetVoltage(voltage_out);
    }else{
        pid.Reset();
        voltage_out = 0.0;
        SetVoltage(0.0);
    }
    //--------------------------------//

    // Anti-windup
    pid.Anti_windup(delta_output);

    ///////////////////////////////////////
    // Reset flag
    Flag_SpeedCal_Iterated = false;
}
/////////////////////////////////////////////////////////// end Control


// 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;
    }
}
void CURRENT_CONTROL::SetVoltage(float volt)
{
    // Output saturation and unit changing
    if (reverse_voltage)
        SetPWMDuty( 0.5 - saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
    else
        SetPWMDuty( 0.5 + saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
}
//
float CURRENT_CONTROL::GetAnalogIn(void)
{
    analogInValue = currentAnalogIn.read();
    return analogInValue;
}

float CURRENT_CONTROL::GetCurrent(void)
{
    // Init check
    if (!Flag_Init) return 0.0;

    //-----------------------------------------//
    GetAnalogIn();

    // Unit transformation
    if (reverse_current){
        curFeedBack = -1*(analogInValue - currentOffset)*analog2Cur;
    }else{
        curFeedBack = (analogInValue - currentOffset)*analog2Cur;
    }
    //

    // Low-pass filter
    curFeedBack_filter = lpFilter.filter(curFeedBack);

    return  curFeedBack_filter; //curFeedBack;
}