Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.h@11:31cd02611cd0, 2016-12-22 (annotated)

Committer:
benson516
Date:
Thu Dec 22 19:22:59 2016 +0000
Revision:
11:31cd02611cd0
Parent:
9:d8157fbfcd2a
Child:
12:085f35babe21
Current control - done

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;
adam_z 3:c787d1c5ad6a 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);
adam_z 3:c787d1c5ad6a 24
adam_z 3:c787d1c5ad6a 25 private:
adam_z 3:c787d1c5ad6a 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 7:6794cfba3564 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 8:fd6fb3cb12ec 43
benson516 8:fd6fb3cb12ec 44 // AD_pin, PWM_p, PWM_n, pwmIndex , Ts
benson516 8:fd6fb3cb12ec 45 // CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, float samplingTime);
benson516 8:fd6fb3cb12ec 46 // AD_pin, PWM_p, PWM_n, pwmIndex , QEI_A , QEI_B , pulsesPerRev , arraysize , Ts
benson516 8:fd6fb3cb12ec 47 CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, PinName QEI_A, PinName QEI_B, float pulsesPerRev, int arraysize, float samplingTime);
benson516 8:fd6fb3cb12ec 48
benson516 8:fd6fb3cb12ec 49 //
benson516 9:d8157fbfcd2a 50 void SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in);
benson516 9:d8157fbfcd2a 51 void SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in);
benson516 8:fd6fb3cb12ec 52 void SetGain(float Kp, float Ki, float Kd, float Ka);
benson516 8:fd6fb3cb12ec 53 //
benson516 7:6794cfba3564 54 void OffsetInit(void);
benson516 5:7ccd2fb7ce7e 55 float saturation(float input_value, float &delta, const float &limit_H, const float &limit_L);
benson516 11:31cd02611cd0 56 void Control(float curRef, bool enable);
benson516 5:7ccd2fb7ce7e 57
benson516 5:7ccd2fb7ce7e 58 // Back emf as the function of rotational speed
benson516 5:7ccd2fb7ce7e 59 float func_back_emf(const float &W_in);
adam_z 3:c787d1c5ad6a 60
adam_z 2:562bd14dfd3a 61 //functions for test////////
adam_z 0:955aa05c968a 62 void ChangePwmPeriod(float microSeconds);
adam_z 1:c5973a56d474 63 void SetPWMDuty(float ratio);
benson516 9:d8157fbfcd2a 64 void SetVoltage(float volt);
adam_z 2:562bd14dfd3a 65 float GetAnalogIn(void);
adam_z 2:562bd14dfd3a 66 float GetCurrent(void);
benson516 5:7ccd2fb7ce7e 67
benson516 5:7ccd2fb7ce7e 68
adam_z 2:562bd14dfd3a 69 //////////////////////////
adam_z 1:c5973a56d474 70 PWMIndex pwmIndex_;
adam_z 0:955aa05c968a 71 PID pid;
adam_z 0:955aa05c968a 72 PwmOut MotorPlus;
adam_z 0:955aa05c968a 73 PwmOut MotorMinus;
benson516 8:fd6fb3cb12ec 74 QEI wheelSpeed; //(pin_A, pin_B, pin_Z, pulsesPerRev, arraysize, sampletime, pulses)
benson516 5:7ccd2fb7ce7e 75
benson516 5:7ccd2fb7ce7e 76 //
benson516 7:6794cfba3564 77 // float controlOutput;
adam_z 0:955aa05c968a 78 float currentOffset;
adam_z 1:c5973a56d474 79 float curFeedBack;
benson516 7:6794cfba3564 80 float curFeedBack_filter;
benson516 7:6794cfba3564 81 //
benson516 7:6794cfba3564 82 float voltage_out;
benson516 7:6794cfba3564 83 float delta_output;
adam_z 4:1a6ba05e7736 84
benson516 5:7ccd2fb7ce7e 85 //
adam_z 1:c5973a56d474 86 AnalogIn currentAnalogIn;
adam_z 2:562bd14dfd3a 87 float analogInValue;
benson516 7:6794cfba3564 88
benson516 7:6794cfba3564 89 // Ratio for units transformation
adam_z 4:1a6ba05e7736 90 float analog2Cur;
adam_z 4:1a6ba05e7736 91 float voltage2Duty;
benson516 5:7ccd2fb7ce7e 92
benson516 5:7ccd2fb7ce7e 93 //
benson516 5:7ccd2fb7ce7e 94 float Ke;
benson516 5:7ccd2fb7ce7e 95
benson516 5:7ccd2fb7ce7e 96 //
adam_z 3:c787d1c5ad6a 97 LPF lpFilter;
adam_z 3:c787d1c5ad6a 98
benson516 7:6794cfba3564 99 // Initialization
benson516 7:6794cfba3564 100 bool Flag_Init;
benson516 7:6794cfba3564 101 int Init_count;
benson516 7:6794cfba3564 102 float Accumulated_offset;
benson516 8:fd6fb3cb12ec 103
benson516 8:fd6fb3cb12ec 104 // Speed
benson516 8:fd6fb3cb12ec 105 bool Flag_SpeedCal_Iterated;
benson516 8:fd6fb3cb12ec 106 float angularSpeed;
benson516 8:fd6fb3cb12ec 107 float Speed_IterateOnce(void);
benson516 8:fd6fb3cb12ec 108 float getAngularSpeed(void);
benson516 8:fd6fb3cb12ec 109 float getAngularSpeed_deg_s(void);
benson516 5:7ccd2fb7ce7e 110
adam_z 0:955aa05c968a 111 private:
benson516 9:d8157fbfcd2a 112
adam_z 0:955aa05c968a 113 float Ts;
benson516 9:d8157fbfcd2a 114 bool reverse_current;
benson516 9:d8157fbfcd2a 115 bool reverse_rotationalSpeed;
benson516 9:d8157fbfcd2a 116 bool reverse_voltage;
adam_z 0:955aa05c968a 117
adam_z 0:955aa05c968a 118 };
adam_z 0:955aa05c968a 119
adam_z 0:955aa05c968a 120
adam_z 0:955aa05c968a 121 #endif