Motor current controller
Fork of CURRENT_CONTROL by
Embed:
(wiki syntax)
Show/hide line numbers
CURRENT_CONTROL.h
00001 /* 00002 #ifndef PI 00003 #define PI = 3.1415926 00004 #endif 00005 */ 00006 00007 #ifndef __CURRENT_CONTROL_H__ 00008 #define __CURRENT_CONTROL_H__ 00009 00010 #include "mbed.h" 00011 #include "PID.h" 00012 #include "QEI.h" 00013 // 00014 #include "FILTER_LIB.h" // Filter library 00015 00016 00017 00018 /* 00019 class LPF 00020 {public: 00021 float output; 00022 00023 LPF(float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz" 00024 float filter(float input); 00025 void reset(float input); 00026 00027 private: 00028 float Ts; 00029 float cutOff_freq_Hz; // Hz 00030 float alpha_Ts; 00031 float One_alpha_Ts; 00032 00033 // Flag 00034 bool Flag_Init; 00035 }; 00036 */ 00037 00038 class CURRENT_CONTROL 00039 { 00040 public: 00041 typedef enum { 00042 PWM1, 00043 PWM2 00044 } PWMIndex; 00045 00046 //Current-input limit 00047 float current_limit_H; // +1.3 00048 float current_limit_L; // -1.3 00049 00050 // AD_pin, PWM_p, PWM_n, pwmIndex , Ts 00051 // CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, float samplingTime); 00052 // AD_pin, PWM_p, PWM_n, pwmIndex , QEI_A , QEI_B , pulsesPerRev , arraysize , Ts 00053 CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, PinName QEI_A, PinName QEI_B, float pulsesPerRev, int arraysize, float samplingTime); 00054 00055 // Setting parameters 00056 void SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in); 00057 void SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in); 00058 void SetGain(float Kp, float Ki, float Kd, float Ka); 00059 void setInputLimits(float current_limit_H_in, float current_limit_L_in); 00060 00061 // Initialization 00062 void OffsetInit(void); 00063 // Utilities 00064 float saturation(float input_value, const float &limit_H, const float &limit_L); 00065 float saturation(float input_value, float &delta, const float &limit_H, const float &limit_L); 00066 // Control 00067 void TorqueControl(float TorqueRef, bool enable); 00068 void Control(float curRef, bool enable); 00069 00070 // Back emf as the function of rotational speed 00071 float func_back_emf(const float &W_in); 00072 00073 // Elementary function (building block) 00074 void ChangePwmPeriod(float microSeconds); 00075 void SetPWMDuty(float ratio); 00076 void SetVoltage(float volt); 00077 float GetAnalogIn(void); 00078 float GetCurrent(void); 00079 00080 00081 ////////////////////////// 00082 PWMIndex pwmIndex_; 00083 PID pid; 00084 PwmOut MotorPlus; 00085 PwmOut MotorMinus; 00086 QEI wheelSpeed; //(pin_A, pin_B, pin_Z, pulsesPerRev, arraysize, sampletime, pulses) 00087 00088 // 00089 // float controlOutput; 00090 float currentOffset; 00091 float curFeedBack; 00092 float curFeedBack_filter; 00093 float curCommand; 00094 // 00095 float voltage_out; 00096 float delta_output; 00097 00098 // 00099 AnalogIn currentAnalogIn; 00100 float analogInValue; 00101 00102 // Ratio for units transformation 00103 float analog2Cur; 00104 float voltage2Duty; 00105 00106 // 00107 float Ke; 00108 float Kt; 00109 float Kt_inv; 00110 00111 00112 00113 // Initialization 00114 bool Flag_Init; 00115 int Init_count; 00116 float Accumulated_offset; 00117 00118 // Speed 00119 bool Flag_SpeedCal_Iterated; 00120 float angularSpeed; 00121 float Speed_IterateOnce(void); 00122 float getAngularSpeed(void); 00123 float getAngularSpeed_deg_s(void); 00124 00125 private: 00126 00127 float Ts; 00128 bool reverse_current; 00129 bool reverse_rotationalSpeed; 00130 bool reverse_voltage; 00131 00132 // Low-pass filters 00133 LPF lpf_current; // For motor current 00134 LPF_nthOrderCritical lpf_wheelSpeed; // For wheel rotational speed 00135 00136 }; 00137 00138 00139 #endif
Generated on Wed Jul 13 2022 05:32:48 by
1.7.2
