Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.h@20:559db13dc85a, 2017-02-10 (annotated)

Committer:
benson516
Date:
Fri Feb 10 18:25:10 2017 +0000
Revision:
20:559db13dc85a
Parent:
19:24605ba48462 
Add a LPF to wheel-speed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 5:7ccd2fb7ce7e 1 /*
benson516 5:7ccd2fb7ce7e 2 #ifndef PI
benson516 5:7ccd2fb7ce7e 3 #define PI = 3.1415926
benson516 5:7ccd2fb7ce7e 4 #endif
benson516 5:7ccd2fb7ce7e 5 */
benson516 5:7ccd2fb7ce7e 6
adam_z 0:955aa05c968a 7 #ifndef __CURRENT_CONTROL_H__
adam_z 0:955aa05c968a 8 #define __CURRENT_CONTROL_H__
adam_z 0:955aa05c968a 9
adam_z 0:955aa05c968a 10 #include "mbed.h"
adam_z 0:955aa05c968a 11 #include "PID.h"
benson516 8:fd6fb3cb12ec 12 #include "QEI.h"
benson516 16:6e3bcd373f9d 13 //
benson516 16:6e3bcd373f9d 14 #include "FILTER_LIB.h" // Filter library
adam_z 0:955aa05c968a 15
adam_z 0:955aa05c968a 16
adam_z 3:c787d1c5ad6a 17
benson516 16:6e3bcd373f9d 18 /*
adam_z 3:c787d1c5ad6a 19 class LPF
adam_z 3:c787d1c5ad6a 20 {public:
benson516 5:7ccd2fb7ce7e 21 float output;
benson516 14:67fc256efeb7 22
benson516 5:7ccd2fb7ce7e 23 LPF(float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
benson516 5:7ccd2fb7ce7e 24 float filter(float input);
benson516 7:6794cfba3564 25 void reset(float input);
benson516 14:67fc256efeb7 26
adam_z 3:c787d1c5ad6a 27 private:
benson516 14:67fc256efeb7 28 float Ts;
benson516 5:7ccd2fb7ce7e 29 float cutOff_freq_Hz; // Hz
benson516 5:7ccd2fb7ce7e 30 float alpha_Ts;
benson516 9:d8157fbfcd2a 31 float One_alpha_Ts;
benson516 14:67fc256efeb7 32
benson516 7:6794cfba3564 33 // Flag
benson516 7:6794cfba3564 34 bool Flag_Init;
adam_z 3:c787d1c5ad6a 35 };
benson516 16:6e3bcd373f9d 36 */
adam_z 3:c787d1c5ad6a 37
adam_z 0:955aa05c968a 38 class CURRENT_CONTROL
adam_z 0:955aa05c968a 39 {
adam_z 0:955aa05c968a 40 public:
adam_z 1:c5973a56d474 41 typedef enum {
adam_z 1:c5973a56d474 42 PWM1,
adam_z 1:c5973a56d474 43 PWM2
adam_z 1:c5973a56d474 44 } PWMIndex;
benson516 14:67fc256efeb7 45
benson516 15:d9ccd6c92a21 46 //Current-input limit
benson516 15:d9ccd6c92a21 47 float current_limit_H; // +1.3
benson516 15:d9ccd6c92a21 48 float current_limit_L; // -1.3
benson516 15:d9ccd6c92a21 49
benson516 8:fd6fb3cb12ec 50 // AD_pin, PWM_p, PWM_n, pwmIndex , Ts
benson516 8:fd6fb3cb12ec 51 // CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, float samplingTime);
benson516 8:fd6fb3cb12ec 52 // AD_pin, PWM_p, PWM_n, pwmIndex , QEI_A , QEI_B , pulsesPerRev , arraysize , Ts
benson516 8:fd6fb3cb12ec 53 CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, PinName QEI_A, PinName QEI_B, float pulsesPerRev, int arraysize, float samplingTime);
benson516 14:67fc256efeb7 54
benson516 15:d9ccd6c92a21 55 // Setting parameters
benson516 9:d8157fbfcd2a 56 void SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in);
benson516 9:d8157fbfcd2a 57 void SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in);
benson516 8:fd6fb3cb12ec 58 void SetGain(float Kp, float Ki, float Kd, float Ka);
benson516 15:d9ccd6c92a21 59 void setInputLimits(float current_limit_H_in, float current_limit_L_in);
benson516 15:d9ccd6c92a21 60
benson516 15:d9ccd6c92a21 61 // Initialization
benson516 7:6794cfba3564 62 void OffsetInit(void);
benson516 15:d9ccd6c92a21 63 // Utilities
benson516 15:d9ccd6c92a21 64 float saturation(float input_value, const float &limit_H, const float &limit_L);
benson516 5:7ccd2fb7ce7e 65 float saturation(float input_value, float &delta, const float &limit_H, const float &limit_L);
benson516 15:d9ccd6c92a21 66 // Control
benson516 14:67fc256efeb7 67 void TorqueControl(float TorqueRef, bool enable);
benson516 11:31cd02611cd0 68 void Control(float curRef, bool enable);
benson516 14:67fc256efeb7 69
benson516 5:7ccd2fb7ce7e 70 // Back emf as the function of rotational speed
benson516 5:7ccd2fb7ce7e 71 float func_back_emf(const float &W_in);
benson516 14:67fc256efeb7 72
benson516 15:d9ccd6c92a21 73 // Elementary function (building block)
adam_z 0:955aa05c968a 74 void ChangePwmPeriod(float microSeconds);
adam_z 1:c5973a56d474 75 void SetPWMDuty(float ratio);
benson516 9:d8157fbfcd2a 76 void SetVoltage(float volt);
adam_z 2:562bd14dfd3a 77 float GetAnalogIn(void);
adam_z 2:562bd14dfd3a 78 float GetCurrent(void);
benson516 14:67fc256efeb7 79
benson516 14:67fc256efeb7 80
adam_z 2:562bd14dfd3a 81 //////////////////////////
adam_z 1:c5973a56d474 82 PWMIndex pwmIndex_;
adam_z 0:955aa05c968a 83 PID pid;
adam_z 0:955aa05c968a 84 PwmOut MotorPlus;
adam_z 0:955aa05c968a 85 PwmOut MotorMinus;
benson516 8:fd6fb3cb12ec 86 QEI wheelSpeed; //(pin_A, pin_B, pin_Z, pulsesPerRev, arraysize, sampletime, pulses)
benson516 14:67fc256efeb7 87
benson516 5:7ccd2fb7ce7e 88 //
benson516 7:6794cfba3564 89 // float controlOutput;
adam_z 0:955aa05c968a 90 float currentOffset;
adam_z 1:c5973a56d474 91 float curFeedBack;
benson516 7:6794cfba3564 92 float curFeedBack_filter;
benson516 14:67fc256efeb7 93 float curCommand;
benson516 7:6794cfba3564 94 //
benson516 7:6794cfba3564 95 float voltage_out;
benson516 7:6794cfba3564 96 float delta_output;
benson516 14:67fc256efeb7 97
benson516 5:7ccd2fb7ce7e 98 //
adam_z 1:c5973a56d474 99 AnalogIn currentAnalogIn;
adam_z 2:562bd14dfd3a 100 float analogInValue;
benson516 14:67fc256efeb7 101
benson516 7:6794cfba3564 102 // Ratio for units transformation
adam_z 4:1a6ba05e7736 103 float analog2Cur;
adam_z 4:1a6ba05e7736 104 float voltage2Duty;
benson516 14:67fc256efeb7 105
benson516 5:7ccd2fb7ce7e 106 //
benson516 14:67fc256efeb7 107 float Ke;
benson516 14:67fc256efeb7 108 float Kt;
benson516 14:67fc256efeb7 109 float Kt_inv;
benson516 14:67fc256efeb7 110
benson516 19:24605ba48462 111
benson516 14:67fc256efeb7 112
benson516 7:6794cfba3564 113 // Initialization
benson516 7:6794cfba3564 114 bool Flag_Init;
benson516 7:6794cfba3564 115 int Init_count;
benson516 7:6794cfba3564 116 float Accumulated_offset;
benson516 14:67fc256efeb7 117
benson516 8:fd6fb3cb12ec 118 // Speed
benson516 8:fd6fb3cb12ec 119 bool Flag_SpeedCal_Iterated;
benson516 8:fd6fb3cb12ec 120 float angularSpeed;
benson516 8:fd6fb3cb12ec 121 float Speed_IterateOnce(void);
benson516 8:fd6fb3cb12ec 122 float getAngularSpeed(void);
benson516 14:67fc256efeb7 123 float getAngularSpeed_deg_s(void);
benson516 5:7ccd2fb7ce7e 124
adam_z 0:955aa05c968a 125 private:
benson516 14:67fc256efeb7 126
adam_z 0:955aa05c968a 127 float Ts;
benson516 9:d8157fbfcd2a 128 bool reverse_current;
benson516 9:d8157fbfcd2a 129 bool reverse_rotationalSpeed;
benson516 9:d8157fbfcd2a 130 bool reverse_voltage;
adam_z 0:955aa05c968a 131
benson516 19:24605ba48462 132 // Low-pass filters
benson516 19:24605ba48462 133 LPF lpf_current; // For motor current
benson516 19:24605ba48462 134 LPF_nthOrderCritical lpf_wheelSpeed; // For wheel rotational speed
benson516 19:24605ba48462 135
adam_z 0:955aa05c968a 136 };
adam_z 0:955aa05c968a 137
adam_z 0:955aa05c968a 138
benson516 12:085f35babe21 139 #endif