Diff: CURRENT_CONTROL.cpp

Revision:

3:c787d1c5ad6a

Parent:

2:562bd14dfd3a

Child:

4:1a6ba05e7736

--- a/CURRENT_CONTROL.cpp	Fri Apr 22 15:21:31 2016 +0000
+++ b/CURRENT_CONTROL.cpp	Fri Apr 22 16:29:35 2016 +0000
@@ -10,7 +10,9 @@
                                  float samplingTime) : currentAnalogIn(curChannel),
     MotorPlus(PwmChannel1),
     MotorMinus(PwmChannel2),
-    pid(Kp,Ki,Kd,samplingTime)
+    pid(Kp,Ki,Kd,samplingTime),
+    lpFilter(samplingTime)
+    
 {
     pwmIndex_ = pwmIndex;
 
@@ -20,13 +22,15 @@
     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
-
+    
+    
+    
 }
 
 void CURRENT_CONTROL::Control(float curRef)
 {
     analogInValue = currentAnalogIn.read();
-    curFeedBack = (analogInValue - currentOffset)*analog2Cur;
+    curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur, 10*3.14159);
     pid.Compute(curRef, curFeedBack);
     MotorPlus = 0.5 - pid.output;
     if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
@@ -37,6 +41,8 @@
 {
     analog2Cur = ratio;
 }
+
+
 //****************for test************************//
 void CURRENT_CONTROL::SetPWMDuty(float ratio)
 {
@@ -52,5 +58,18 @@
 
 float CURRENT_CONTROL::GetCurrent(void)
 {
-    return  ((currentAnalogIn.read()-currentOffset)*analog2Cur);   
+    return  curFeedBack;   
+}
+
+//=====================LPF ====================//
+LPF::LPF(float samplingTime)
+{
+    Ts = samplingTime;
+}
+
+float LPF::filter(float input, float cutOff)
+{
+    output = (outputLast + cutOff*Ts)/(1 + cutOff*Ts);
+    outputLast = output;
+    return output;
 }
\ No newline at end of file