Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers CURRENT_CONTROL.h Source File

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