Diff: CURRENT_CONTROL.cpp

Revision:

7:6794cfba3564

Parent:

6:bae35ca64f10

Child:

8:fd6fb3cb12ec

--- a/CURRENT_CONTROL.cpp	Thu Dec 15 23:16:09 2016 +0000
+++ b/CURRENT_CONTROL.cpp	Mon Dec 19 13:26:05 2016 +0000
@@ -7,14 +7,31 @@
     Ts = samplingTime;
     cutOff_freq_Hz = cutOff_freq_Hz_in;
     alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts;
+    output = 0.0;
+    
+    //
+    Flag_Init = false;    
 }
 
 float LPF::filter(float input)
 {
+    // Initialization
+    if (!Flag_Init){
+        reset(input);
+        Flag_Init = true;
+        return output;
+    }
+    
     // output = (1.0 - 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 =================//
@@ -28,7 +45,10 @@
     MotorPlus(PwmChannel1),
     MotorMinus(PwmChannel2),
     pid(Kp,Ki,Kd,samplingTime),
-    lpFilter(samplingTime, 1.5915) // 10 rad/s
+    lpFilter(samplingTime, 100.0), // 1.5915 Hz = 10 rad/s
+    curFeedBack(0.0),
+    curFeedBack_filter(0.0),
+    voltage_out(0.0)
     
 {
     pwmIndex_ = pwmIndex;
@@ -36,14 +56,19 @@
     Ts = samplingTime;
     
     //setup motor PWM parameters
-    MotorPlus.period_us(15);//default period equals to 25 microseconds
+    MotorPlus.period_us(50); //set the pwm period = 50 us, where the frequency = 20kHz 
     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
     
+    //
+    Flag_Init = false;
+    Init_count = 0;
+    Accumulated_offset = 0.0;
     
     //
     currentOffset = 0.0;
+    //
     delta_output = 0.0;
     
     // Set PID's parameters                        
@@ -53,6 +78,17 @@
     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;
+    }
+}
 //
 void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
 {
@@ -77,26 +113,32 @@
 //
 void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
 {
-    analogInValue = currentAnalogIn.read();
+    // Init check
+    OffsetInit();
+    if (!Flag_Init) return;   
+    //////////////////////////////////////
     
-    // Filter
-    curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur);
+    // Get measurement
+    GetCurrent();
+     
+    //--------------------------------//
     
     // PID
-    pid.Compute_noWindUP(curRef, curFeedBack);
+    pid.Compute_noWindUP(curRef, curFeedBack_filter);
+    
+    // Voltage output with back-emf compensation
+    voltage_out = -pid.output + func_back_emf(angularSpeed); 
     
     // Output saturation and unit changing
-    MotorPlus = 0.5 + saturation((-pid.output + func_back_emf(angularSpeed) )*voltage2Duty, delta_output, 0.5, -0.5) ;
+    SetPWMDuty( 0.5 + saturation( voltage_out*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){
@@ -107,18 +149,34 @@
 void CURRENT_CONTROL::SetPWMDuty(float ratio)
 {
     MotorPlus = ratio;
-    if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
-    else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
+    if(pwmIndex_ == PWM1){
+        TIM1->CCER |= 4;
+    }else if(pwmIndex_ == PWM2){
+        TIM1->CCER |= 64;
+    }
 }
 
 float CURRENT_CONTROL::GetAnalogIn(void)
 {
-    return analogInValue = currentAnalogIn.read();   
+    analogInValue = currentAnalogIn.read();
+    return analogInValue;
 }
 
 float CURRENT_CONTROL::GetCurrent(void)
 {
-    return  curFeedBack;   
+    // Init check
+    if (!Flag_Init) return 0.0; 
+    
+    //-----------------------------------------//
+    analogInValue = currentAnalogIn.read();
+    
+    // Unit transformation
+    curFeedBack = (analogInValue - currentOffset)*analog2Cur;
+    
+    // Low-pass filter
+    curFeedBack_filter = lpFilter.filter(curFeedBack);
+    
+    return  curFeedBack; //curFeedBack_filter;   
 }