Chun Feng Huang / CURRENT_CONTROL

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

Files at this revision

API Documentation at this revision

Revision 3:c787d1c5ad6a, committed 2016-04-22

Comitter:
adam_z
Date:
Fri Apr 22 16:29:35 2016 +0000
Parent:
2:562bd14dfd3a
Child:
4:1a6ba05e7736
Commit message:
basically works

Changed in this revision

CURRENT_CONTROL.cpp Show annotated file Show diff for this revision Revisions of this file
CURRENT_CONTROL.h Show annotated file Show diff for this revision Revisions of this file 
--- 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

--- a/CURRENT_CONTROL.h	Fri Apr 22 15:21:31 2016 +0000
+++ b/CURRENT_CONTROL.h	Fri Apr 22 16:29:35 2016 +0000
@@ -5,6 +5,19 @@
 #include "PID.h"
 
 
+
+class LPF
+{public:
+    float output, outputLast;
+    
+    LPF(float samplingTime);
+    float filter(float input, float cutOff);
+    
+private:
+    float Ts;    
+};
+
+
 class CURRENT_CONTROL
 {
 public:
@@ -16,6 +29,7 @@
     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);
+    
     //functions for test////////
     void ChangePwmPeriod(float microSeconds);
     void SetPWMDuty(float ratio);
@@ -34,6 +48,9 @@
     AnalogIn currentAnalogIn;
     float analogInValue;
     
+    LPF lpFilter;
+    
+    
 private:
 
     float Ts;
@@ -71,4 +88,6 @@
 
 
 
+
+
 #endif
\ No newline at end of file