Revision 4:1a6ba05e7736, committed 2016-04-28

Comitter:

adam_z

Date:

Thu Apr 28 09:07:32 2016 +0000

Parent:

3:c787d1c5ad6a

Commit message:

add back emf compensation

Changed in this revision

diff -r c787d1c5ad6a -r 1a6ba05e7736 CURRENT_CONTROL.cpp
--- 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

diff -r c787d1c5ad6a -r 1a6ba05e7736 CURRENT_CONTROL.h
--- a/CURRENT_CONTROL.h	Fri Apr 22 16:29:35 2016 +0000
+++ b/CURRENT_CONTROL.h	Thu Apr 28 09:07:32 2016 +0000
@@ -27,8 +27,9 @@
     } PWMIndex;
 
     CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, float Kp, float Ki, float Kd, float samplingTime);
-    void Control(float curRef);
-    void SetAnalog2Cur(float ratio);
+    float saturation(float input, float limit_H, float limit_L);
+    void Control(float curRef, float speed);
+    void SetParams(float Analog2Cur, float angSpeed2Backemf, float voltage2DutyRatio);
     
     //functions for test////////
     void ChangePwmPeriod(float microSeconds);
@@ -44,10 +45,13 @@
 
     float currentOffset;
     float curFeedBack;
-    float analog2Cur;
+    
     AnalogIn currentAnalogIn;
     float analogInValue;
     
+    float analog2Cur;
+    float Kw;
+    float voltage2Duty;
     LPF lpFilter;