Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers CURRENT_CONTROL.cpp Source File

CURRENT_CONTROL.cpp

00001 #include "mbed.h"
00002 #include "CURRENT_CONTROL.h"
00003 
00004 /*
00005 //=====================LPF ====================//
00006 LPF::LPF(float samplingTime, float cutOff_freq_Hz_in)
00007 {
00008     Ts = samplingTime;
00009     cutOff_freq_Hz = cutOff_freq_Hz_in;
00010     alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts;
00011     One_alpha_Ts = 1.0 - alpha_Ts;
00012     output = 0.0;
00013 
00014     //
00015     Flag_Init = false;
00016 }
00017 
00018 float LPF::filter(float input)
00019 {
00020     // Initialization
00021     if (!Flag_Init){
00022         reset(input);
00023         Flag_Init = true;
00024         return output;
00025     }
00026 
00027     // output = One_alpha_Ts*output + alpha_Ts*input;
00028     output += alpha_Ts*(input - output);
00029 
00030     return output;
00031 }
00032 void LPF::reset(float input)
00033 {
00034     // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
00035     output = input;
00036     return;
00037 }
00038 */
00039 
00040 //================== CURRENT_CONTROL =================//
00041 CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
00042                                  PinName PwmChannel1,
00043                                  PinName PwmChannel2,
00044                                  PWMIndex pwmIndex,
00045                                  PinName QEI_A,
00046                                  PinName QEI_B,
00047                                  float pulsesPerRev,
00048                                  int arraysize,
00049                                  float samplingTime) :
00050     currentAnalogIn(curChannel),
00051     MotorPlus(PwmChannel1),
00052     MotorMinus(PwmChannel2),
00053     wheelSpeed(QEI_A, QEI_B, NC, pulsesPerRev, arraysize, samplingTime, QEI::X4_ENCODING), //(pin1, pin2, pin3, pulsesPerRev, arraysize, sampletime, pulses)
00054     pid(0.0,0.0,0.0,samplingTime),
00055     lpf_current(samplingTime, 400.0), // 1.5915 Hz = 10 rad/s
00056     lpf_wheelSpeed(samplingTime, 300.0, 2) // 2nd-order LPF, fc = 200.0 Hz
00057 
00058 
00059 {
00060     pwmIndex_ = pwmIndex;
00061 
00062     Ts = samplingTime;
00063 
00064     //setup motor PWM parameters
00065     MotorPlus.period_us(50); //set the pwm period = 50 us, where the frequency = 20kHz
00066     //
00067     SetPWMDuty(0.5);
00068 
00069     //Current-input limit
00070     current_limit_H = 1.3;
00071     current_limit_L = -1.3;
00072     //
00073     Flag_Init = false;
00074     Init_count = 0;
00075     Accumulated_offset = 0.0;
00076 
00077     //
00078     currentOffset = 0.0;
00079     //
00080     delta_output = 0.0;
00081     curFeedBack = 0.0;
00082     curFeedBack_filter = 0.0;
00083     curCommand = 0.0;
00084     voltage_out = 0.0;
00085 
00086     // Set PID's parameters
00087     /////////////////////
00088     pid.Kp = 0.0;
00089     pid.Ki = 0.0;
00090     pid.Kd = 0.0;
00091     pid.Ka = 0.0; // Gain for anti-windup // Ka = 0.0017
00092 
00093     // Speed
00094     angularSpeed = 0.0;
00095     Flag_SpeedCal_Iterated = false;
00096 
00097     // Reverse flage for each signal
00098     reverse_current = false;
00099     reverse_rotationalSpeed = false;
00100     reverse_voltage = false;
00101 }
00102 //
00103 void CURRENT_CONTROL::SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in)
00104 {
00105     analog2Cur = Analog2Cur_in;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
00106     Ke = angSpeed2BackEmf;
00107     Kt = Ke;
00108     Kt_inv = 1/Kt;
00109     voltage2Duty = voltage2Duty_in;
00110 }
00111 void CURRENT_CONTROL::SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in)
00112 {
00113     reverse_current = reverse_current_in;
00114     reverse_rotationalSpeed = reverse_rotationalSpeed_in;
00115     reverse_voltage = reverse_voltage_in;
00116 }
00117 //
00118 void CURRENT_CONTROL::SetGain(float Kp, float Ki, float Kd, float Ka)
00119 {
00120     // Set PID's parameters
00121     /////////////////////
00122     pid.Kp = Kp;
00123     pid.Ki = Ki;
00124     pid.Kd = Kd;
00125     pid.Ka = Ka; // Gain for anti-windup // Ka = 0.0017
00126 }
00127 //
00128 void CURRENT_CONTROL::setInputLimits(float current_limit_H_in, float current_limit_L_in){
00129     current_limit_H = current_limit_H_in;
00130     current_limit_L = current_limit_L_in;
00131 }
00132 //
00133 void CURRENT_CONTROL::OffsetInit(void){
00134     if (Flag_Init) return;
00135     //
00136 
00137     Init_count++;
00138     /*
00139     Accumulated_offset += GetAnalogIn();
00140 
00141     if (Init_count >= 500){ // Fixed time
00142         currentOffset = Accumulated_offset / float(Init_count);
00143         Flag_Init = true;
00144     }
00145     */
00146 
00147     //
00148     /*
00149     if (Init_count <= 10){
00150         Accumulated_offset = GetAnalogIn();
00151     }else{
00152         // Accumulated_offset +=  0.0063*(GetAnalogIn() - Accumulated_offset); // fc = 1 Hz
00153         Accumulated_offset = 0.9937*Accumulated_offset + 0.0063*GetAnalogIn(); // fc = 1 Hz
00154     }
00155 
00156     if (Init_count >= 300){ // Fixed time: 500 samples
00157         currentOffset = Accumulated_offset;
00158         Flag_Init = true;
00159     }
00160     */
00161 
00162 
00163     //
00164     if (Init_count >= 10){ // Fixed time: 10 samples
00165         currentOffset = GetAnalogIn();
00166         Flag_Init = true;
00167     }
00168 
00169 
00170 }
00171 
00172 // Without delta output
00173 float CURRENT_CONTROL::saturation(float input_value, const float &limit_H, const float &limit_L){
00174     if( input_value > limit_H ){
00175         input_value = limit_H;
00176     }else if( input_value < limit_L ){
00177         input_value = limit_L;
00178     }else{
00179         // Nothing
00180     }
00181     return input_value;
00182 }
00183 // With delta output
00184 float CURRENT_CONTROL::saturation(float input_value, float &delta, const float &limit_H, const float &limit_L){
00185     if( input_value > limit_H ){
00186         delta = limit_H - input_value;
00187         input_value = limit_H;
00188     }else if( input_value < limit_L ){
00189         delta = limit_L - input_value;
00190         input_value = limit_L;
00191     }else{
00192         delta = 0.0;
00193     }
00194     return input_value;
00195 }
00196 // Speed_IterateOnce
00197 ////////////////////////////////
00198 float CURRENT_CONTROL::Speed_IterateOnce(void){
00199     // The flag "Flag_SpeedCal_Iterated" will be resetted at the end of function Control()
00200     if(Flag_SpeedCal_Iterated) return;
00201 
00202     // Speed
00203     wheelSpeed.Calculate();
00204 
00205     /*
00206     //
00207     if (reverse_rotationalSpeed)
00208         angularSpeed = -wheelSpeed.getAngularSpeed();
00209     else
00210         angularSpeed = wheelSpeed.getAngularSpeed();
00211     //
00212     */
00213 
00214     //
00215     if (reverse_rotationalSpeed)
00216         angularSpeed = lpf_wheelSpeed.filter( -wheelSpeed.getAngularSpeed() );
00217     else
00218         angularSpeed = lpf_wheelSpeed.filter( wheelSpeed.getAngularSpeed() );
00219     //
00220 
00221     // Flag
00222     Flag_SpeedCal_Iterated = true;
00223 
00224     return angularSpeed;
00225 }
00226 float CURRENT_CONTROL::getAngularSpeed(void){
00227     // return wheelSpeed.getAngularSpeed();
00228     return angularSpeed;
00229 }
00230 float CURRENT_CONTROL::getAngularSpeed_deg_s(void){
00231 
00232     if (reverse_rotationalSpeed)
00233         return -wheelSpeed.getAngularSpeed_deg_s();
00234     else
00235         return wheelSpeed.getAngularSpeed_deg_s();
00236     //
00237     // return wheelSpeed.getAngularSpeed_deg_s();
00238 }
00239 //////////////////////////////// end Speed_IterateOnce
00240 
00241 
00242 // Control
00243 ///////////////////////////////////////////////////////////
00244 void CURRENT_CONTROL::TorqueControl(float TorqueRef, bool enable){
00245     Control(Kt_inv*TorqueRef, enable);
00246 }
00247 void CURRENT_CONTROL::Control(float curRef, bool enable)
00248 {
00249     // Input saturation
00250     curCommand = saturation(curRef,current_limit_H,current_limit_L);
00251 
00252     // Init check
00253     OffsetInit();
00254     if (!Flag_Init) return;
00255     //////////////////////////////////////
00256 
00257     // Get measurement
00258     // Speed
00259     Speed_IterateOnce();
00260     // Motor current
00261     GetCurrent();
00262 
00263     //--------------------------------//
00264 
00265     if (enable){
00266         // PID
00267         pid.Compute_noWindUP(curRef, curFeedBack_filter);
00268 
00269         // Voltage output with back-emf compensation
00270         voltage_out = pid.output + func_back_emf(angularSpeed);
00271         // voltage_out = 0.0 + func_back_emf(angularSpeed);
00272 
00273         // Controlller output
00274         SetVoltage(voltage_out);
00275     }else{
00276         pid.Reset();
00277         voltage_out = 0.0;
00278         SetVoltage(0.0);
00279     }
00280     //--------------------------------//
00281 
00282     // Anti-windup
00283     pid.Anti_windup(delta_output);
00284 
00285     ///////////////////////////////////////
00286     // Reset flag
00287     Flag_SpeedCal_Iterated = false;
00288 }
00289 /////////////////////////////////////////////////////////// end Control
00290 
00291 
00292 // Back emf as the function of rotational speed
00293 float CURRENT_CONTROL::func_back_emf(const float &W_in){
00294     return (Ke*W_in);
00295 }
00296 
00297 // Elementary function (building block)
00298 void CURRENT_CONTROL::SetPWMDuty(float ratio)
00299 {
00300     MotorPlus = ratio;
00301     if(pwmIndex_ == PWM1){
00302         TIM1->CCER |= 4;
00303     }else if(pwmIndex_ == PWM2){
00304         TIM1->CCER |= 64;
00305     }
00306 }
00307 void CURRENT_CONTROL::SetVoltage(float volt)
00308 {
00309     // Output saturation and unit changing
00310     if (reverse_voltage)
00311         SetPWMDuty( 0.5 - saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
00312     else
00313         SetPWMDuty( 0.5 + saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
00314 }
00315 //
00316 float CURRENT_CONTROL::GetAnalogIn(void)
00317 {
00318     analogInValue = currentAnalogIn.read();
00319     return analogInValue;
00320 }
00321 
00322 float CURRENT_CONTROL::GetCurrent(void)
00323 {
00324     // Init check
00325     if (!Flag_Init) return 0.0;
00326 
00327     //-----------------------------------------//
00328     GetAnalogIn();
00329 
00330     // Unit transformation
00331     if (reverse_current){
00332         curFeedBack = -1*(analogInValue - currentOffset)*analog2Cur;
00333     }else{
00334         curFeedBack = (analogInValue - currentOffset)*analog2Cur;
00335     }
00336     //
00337 
00338     // Low-pass filter
00339     curFeedBack_filter = lpf_current.filter(curFeedBack);
00340 
00341     return  curFeedBack_filter; //curFeedBack;
00342 }