Diff: CURRENT_CONTROL.h

Revision:

8:fd6fb3cb12ec

Parent:

7:6794cfba3564

Child:

9:d8157fbfcd2a

--- a/CURRENT_CONTROL.h	Mon Dec 19 13:26:05 2016 +0000
+++ b/CURRENT_CONTROL.h	Mon Dec 19 15:27:13 2016 +0000
@@ -9,6 +9,7 @@
 
 #include "mbed.h"
 #include "PID.h"
+#include "QEI.h"
 
 
 
@@ -38,10 +39,17 @@
         PWM1,
         PWM2
     } PWMIndex;
-
-    CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, float Kp, float Ki, float Kd, float Ka, float samplingTime);
+    
+    //                      AD_pin,             PWM_p,               PWM_n,       pwmIndex  ,                   Ts
+    // CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, float samplingTime);
+    //                      AD_pin,             PWM_p,               PWM_n,                pwmIndex  ,       QEI_A        , QEI_B      , pulsesPerRev    , arraysize ,  Ts
+    CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, PinName QEI_A, PinName QEI_B, float pulsesPerRev, int arraysize, float samplingTime);
+    
+    //
+    void SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio);
+    void SetGain(float Kp, float Ki, float Kd, float Ka);
+    //
     void OffsetInit(void);
-    void SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio);
     float saturation(float input_value, float &delta, const float &limit_H, const float &limit_L);
     void Control(float curRef, float speed);
         
@@ -60,6 +68,7 @@
     PID pid;
     PwmOut MotorPlus;
     PwmOut MotorMinus;
+    QEI wheelSpeed; //(pin_A, pin_B, pin_Z, pulsesPerRev, arraysize, sampletime, pulses)
     
     //
     // float controlOutput;
@@ -88,8 +97,14 @@
     bool Flag_Init;
     int Init_count;
     float Accumulated_offset;
+    
+    // Speed
+    bool Flag_SpeedCal_Iterated;
+    float angularSpeed;
+    float Speed_IterateOnce(void);
+    float getAngularSpeed(void);
+    float getAngularSpeed_deg_s(void); 
 
-    
 private:
 
     float Ts;