Diff: CURRENT_CONTROL.cpp

Revision:

8:fd6fb3cb12ec

Parent:

7:6794cfba3564

Child:

9:d8157fbfcd2a

--- a/CURRENT_CONTROL.cpp	Mon Dec 19 13:26:05 2016 +0000
+++ b/CURRENT_CONTROL.cpp	Mon Dec 19 15:27:13 2016 +0000
@@ -39,16 +39,18 @@
                                  PinName PwmChannel1,
                                  PinName PwmChannel2,
                                  PWMIndex pwmIndex,
-                                 float Kp, float Ki, float Kd, float Ka,
+                                 PinName QEI_A,
+                                 PinName QEI_B,
+                                 float pulsesPerRev,
+                                 int arraysize,
                                  float samplingTime) : 
     currentAnalogIn(curChannel),
     MotorPlus(PwmChannel1),
     MotorMinus(PwmChannel2),
-    pid(Kp,Ki,Kd,samplingTime),
-    lpFilter(samplingTime, 100.0), // 1.5915 Hz = 10 rad/s
-    curFeedBack(0.0),
-    curFeedBack_filter(0.0),
-    voltage_out(0.0)
+    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, 100.0) // 1.5915 Hz = 10 rad/s
+
     
 {
     pwmIndex_ = pwmIndex;
@@ -57,9 +59,8 @@
     
     //setup motor PWM parameters
     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
+    //
+    SetPWMDuty(0.5);
     
     //
     Flag_Init = false;
@@ -70,14 +71,39 @@
     currentOffset = 0.0;
     //
     delta_output = 0.0;
+    curFeedBack = 0.0;
+    curFeedBack_filter = 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;
+}
+//
+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; 
+}
+//
+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;
     //
@@ -85,18 +111,11 @@
     Accumulated_offset += GetAnalogIn();
     
     if (Init_count >= 500){ // Fixed time
-        currentOffset = Accumulated_offset/float(Init_count);
+        currentOffset = Accumulated_offset / float(Init_count);
         Flag_Init = true;
     }
 }
-//
-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 ){
@@ -111,7 +130,29 @@
     return input_value;
 }
 //
-void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
+
+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();
+    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){
+    return wheelSpeed.getAngularSpeed_deg_s();
+}
+//
+void CURRENT_CONTROL::Control(float curRef, float angularSpeed_in)
 {
     // Init check
     OffsetInit();
@@ -119,6 +160,9 @@
     //////////////////////////////////////
     
     // Get measurement
+    // Speed
+    Speed_IterateOnce();
+    // Motor current
     GetCurrent();
      
     //--------------------------------//
@@ -138,7 +182,9 @@
     // Anti-windup
     pid.Anti_windup(delta_output);
     
-
+    ///////////////////////////////////////
+    // Reset flag
+    Flag_SpeedCal_Iterated = false;
 }
 // Back emf as the function of rotational speed
 float CURRENT_CONTROL::func_back_emf(const float &W_in){