Chun Feng Huang / CURRENT_CONTROL

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.h@8:fd6fb3cb12ec, 2016-12-19 (annotated)

Committer:
benson516
Date:
Mon Dec 19 15:27:13 2016 +0000
Revision:
8:fd6fb3cb12ec
Parent:
7:6794cfba3564
Child:
9:d8157fbfcd2a
Combine "QEI" into "CURRENT_CONTROL"

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