Diff: CURRENT_CONTROL.h

Revision:

14:67fc256efeb7

Parent:

12:085f35babe21

Child:

15:d9ccd6c92a21

--- a/CURRENT_CONTROL.h	Sat Dec 24 09:55:00 2016 +0000
+++ b/CURRENT_CONTROL.h	Mon Dec 26 07:52:43 2016 +0000
@@ -17,17 +17,17 @@
 class LPF
 {public:
     float output;
-    
+
     LPF(float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
     float filter(float input);
     void reset(float input);
-    
+
 private:
-    float Ts;    
+    float Ts;
     float cutOff_freq_Hz; // Hz
     float alpha_Ts;
     float One_alpha_Ts;
-    
+
     // Flag
     bool Flag_Init;
 };
@@ -40,12 +40,12 @@
         PWM1,
         PWM2
     } PWMIndex;
-    
+
     //                      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_in, float angSpeed2BackEmf, float voltage2Duty_in);
     void SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in);
@@ -53,63 +53,67 @@
     //
     void OffsetInit(void);
     float saturation(float input_value, float &delta, const float &limit_H, const float &limit_L);
+    void TorqueControl(float TorqueRef, bool enable);
     void Control(float curRef, bool enable);
-        
+
     // Back emf as the function of rotational speed
     float func_back_emf(const float &W_in);
-    
+
     //functions for test////////
     void ChangePwmPeriod(float microSeconds);
     void SetPWMDuty(float ratio);
     void SetVoltage(float volt);
     float GetAnalogIn(void);
     float GetCurrent(void);
-    
-    
+
+
     //////////////////////////
     PWMIndex pwmIndex_;
     PID pid;
     PwmOut MotorPlus;
     PwmOut MotorMinus;
     QEI wheelSpeed; //(pin_A, pin_B, pin_Z, pulsesPerRev, arraysize, sampletime, pulses)
-    
+
     //
     // float controlOutput;
     float currentOffset;
     float curFeedBack;
     float curFeedBack_filter;
+    float curCommand;
     //
     float voltage_out;
     float delta_output;
-    
+
     //
     AnalogIn currentAnalogIn;
     float analogInValue;
-    
+
     // Ratio for units transformation
     float analog2Cur;
     float voltage2Duty;
-    
+
     //
-    float Ke;    
-    
+    float Ke;
+    float Kt;
+    float Kt_inv;
+
     //
     LPF lpFilter;
-    
+
     // Initialization
     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); 
+    float getAngularSpeed_deg_s(void);
 
 private:
-    
+
     float Ts;
     bool reverse_current;
     bool reverse_rotationalSpeed;