Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.h@15:d9ccd6c92a21, 2016-12-26 (annotated)

Committer:
benson516
Date:
Mon Dec 26 08:53:09 2016 +0000
Revision:
15:d9ccd6c92a21
Parent:
14:67fc256efeb7
Child:
16:6e3bcd373f9d
Add input saturztion

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