Diff: CURRENT_CONTROL.cpp

Revision:

12:085f35babe21

Parent:

11:31cd02611cd0

Child:

13:b5f85f926f22

diff -r 31cd02611cd0 -r 085f35babe21 CURRENT_CONTROL.cpp
--- a/CURRENT_CONTROL.cpp	Thu Dec 22 19:22:59 2016 +0000
+++ b/CURRENT_CONTROL.cpp	Sat Dec 24 09:49:47 2016 +0000
@@ -9,9 +9,9 @@
     alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts;
     One_alpha_Ts = 1.0 - alpha_Ts;
     output = 0.0;
-    
+
     //
-    Flag_Init = false;    
+    Flag_Init = false;
 }
 
 float LPF::filter(float input)
@@ -22,10 +22,10 @@
         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)
@@ -45,7 +45,7 @@
                                  PinName QEI_B,
                                  float pulsesPerRev,
                                  int arraysize,
-                                 float samplingTime) : 
+                                 float samplingTime) :
     currentAnalogIn(curChannel),
     MotorPlus(PwmChannel1),
     MotorMinus(PwmChannel2),
@@ -53,22 +53,22 @@
     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 
+    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;
     //
@@ -76,18 +76,18 @@
     curFeedBack = 0.0;
     curFeedBack_filter = 0.0;
     voltage_out = 0.0;
-    
-    // Set PID's parameters                        
+
+    // 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;
@@ -98,7 +98,7 @@
 {
     analog2Cur = Analog2Cur_in;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
     Ke = angSpeed2BackEmf;
-    voltage2Duty = voltage2Duty_in; 
+    voltage2Duty = voltage2Duty_in;
 }
 void CURRENT_CONTROL::SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in)
 {
@@ -109,7 +109,7 @@
 //
 void CURRENT_CONTROL::SetGain(float Kp, float Ki, float Kd, float Ka)
 {
-    // Set PID's parameters                        
+    // Set PID's parameters
     /////////////////////
     pid.Kp = Kp;
     pid.Ki = Ki;
@@ -120,17 +120,17 @@
 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){
@@ -139,21 +139,21 @@
         // 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;
     }
-    
-    
+
+
 }
 
 //
@@ -161,9 +161,9 @@
     if( input_value > limit_H ){
         delta = limit_H - input_value;
         input_value = limit_H;
-    }else if( input_value < limit_L ){  
+    }else if( input_value < limit_L ){
         delta = limit_L - input_value;
-        input_value = limit_L; 
+        input_value = limit_L;
     }else{
         delta = 0.0;
     }
@@ -173,21 +173,21 @@
 ////////////////////////////////
 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; 
-    
+    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){
@@ -195,7 +195,7 @@
     return angularSpeed;
 }
 float CURRENT_CONTROL::getAngularSpeed_deg_s(void){
-    
+
     if (reverse_rotationalSpeed)
         return -wheelSpeed.getAngularSpeed_deg_s();
     else
@@ -212,36 +212,37 @@
 {
     // Init check
     OffsetInit();
-    if (!Flag_Init) return;   
+    if (!Flag_Init) return;
     //////////////////////////////////////
-    
+
     // Get measurement
     // Speed
     Speed_IterateOnce();
     // Motor current
     GetCurrent();
-     
+
     //--------------------------------//
-    
-    // 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); 
-    
-    // Output saturation and unit changing
-    
-    if (enable)
+
+    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
+    }else{
+        pid.Reset();
+        voltage_out = 0.0
         SetVoltage(0.0);
-    
+    }
     //--------------------------------//
-    
+
     // Anti-windup
     pid.Anti_windup(delta_output);
-    
+
     ///////////////////////////////////////
     // Reset flag
     Flag_SpeedCal_Iterated = false;
@@ -282,11 +283,11 @@
 float CURRENT_CONTROL::GetCurrent(void)
 {
     // Init check
-    if (!Flag_Init) return 0.0; 
-    
+    if (!Flag_Init) return 0.0;
+
     //-----------------------------------------//
     GetAnalogIn();
-    
+
     // Unit transformation
     if (reverse_current){
         curFeedBack = -1*(analogInValue - currentOffset)*analog2Cur;
@@ -294,11 +295,9 @@
         curFeedBack = (analogInValue - currentOffset)*analog2Cur;
     }
     //
-    
+
     // Low-pass filter
     curFeedBack_filter = lpFilter.filter(curFeedBack);
-    
-    return  curFeedBack_filter; //curFeedBack;   
+
+    return  curFeedBack_filter; //curFeedBack;
 }
-
-