Diff: CURRENT_CONTROL.cpp

Revision:

4:1a6ba05e7736

Parent:

3:c787d1c5ad6a

Child:

5:7ccd2fb7ce7e

--- a/CURRENT_CONTROL.cpp	Fri Apr 22 16:29:35 2016 +0000
+++ b/CURRENT_CONTROL.cpp	Thu Apr 28 09:07:32 2016 +0000
@@ -18,7 +18,7 @@
 
     Ts = samplingTime;
     //setup motor PWM parameters
-    MotorPlus.period_us(50);//default period equals to 50 microseconds
+    MotorPlus.period_us(15);//default period equals to 25 microseconds
     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
@@ -27,19 +27,30 @@
     
 }
 
-void CURRENT_CONTROL::Control(float curRef)
+float CURRENT_CONTROL::saturation(float input, float limit_H, float limit_L)
+{
+    float output;
+    if(input < limit_H && input > limit_L)output = input;
+    else output = input >=limit_H?limit_H:limit_L;
+    return output;
+}
+
+void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
 {
     analogInValue = currentAnalogIn.read();
-    curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur, 10*3.14159);
+    curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur, 10);
     pid.Compute(curRef, curFeedBack);
-    MotorPlus = 0.5 - pid.output;
+    MotorPlus = 0.5 + saturation((-pid.output + Kw*angularSpeed)*voltage2Duty, 0.5, -0.5) ;
     if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
     else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
 }
 
-void CURRENT_CONTROL::SetAnalog2Cur(float ratio)//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
+void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
 {
-    analog2Cur = ratio;
+    analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
+    Kw = angSpeed2BackEmf;
+    voltage2Duty = voltage2DutyRatio;
+    
 }
 
 
@@ -69,7 +80,7 @@
 
 float LPF::filter(float input, float cutOff)
 {
-    output = (outputLast + cutOff*Ts)/(1 + cutOff*Ts);
+    output = (1.0 - cutOff*Ts)*outputLast + cutOff*Ts*input;
     outputLast = output;
     return output;
 }
\ No newline at end of file