Developers_of_anti_slip_compensator / CURRENT_CONTROL

20160814

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@4:1a6ba05e7736, 2016-04-28 (annotated)

Committer:
adam_z
Date:
Thu Apr 28 09:07:32 2016 +0000
Revision:
4:1a6ba05e7736
Parent:
3:c787d1c5ad6a 
add back emf compensation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adam_z 0:955aa05c968a 1 #include "mbed.h"
adam_z 0:955aa05c968a 2 #include "CURRENT_CONTROL.h"
adam_z 0:955aa05c968a 3
adam_z 0:955aa05c968a 4
adam_z 1:c5973a56d474 5 CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
adam_z 1:c5973a56d474 6 PinName PwmChannel1,
adam_z 1:c5973a56d474 7 PinName PwmChannel2,
adam_z 1:c5973a56d474 8 PWMIndex pwmIndex,
adam_z 1:c5973a56d474 9 float Kp, float Ki, float Kd,
adam_z 1:c5973a56d474 10 float samplingTime) : currentAnalogIn(curChannel),
adam_z 0:955aa05c968a 11 MotorPlus(PwmChannel1),
adam_z 0:955aa05c968a 12 MotorMinus(PwmChannel2),
adam_z 3:c787d1c5ad6a 13 pid(Kp,Ki,Kd,samplingTime),
adam_z 3:c787d1c5ad6a 14 lpFilter(samplingTime)
adam_z 3:c787d1c5ad6a 15
adam_z 0:955aa05c968a 16 {
adam_z 1:c5973a56d474 17 pwmIndex_ = pwmIndex;
adam_z 1:c5973a56d474 18
adam_z 0:955aa05c968a 19 Ts = samplingTime;
adam_z 0:955aa05c968a 20 //setup motor PWM parameters
adam_z 4:1a6ba05e7736 21 MotorPlus.period_us(15);//default period equals to 25 microseconds
adam_z 1:c5973a56d474 22 MotorPlus.write(0.5); //duty ratio = 0.5 in complementary output mode -> static
adam_z 1:c5973a56d474 23 if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
adam_z 1:c5973a56d474 24 else if(pwmIndex_ == PWM2)TIM1->CCER |= 64; //enable complimentary output
adam_z 3:c787d1c5ad6a 25
adam_z 3:c787d1c5ad6a 26
adam_z 3:c787d1c5ad6a 27
adam_z 0:955aa05c968a 28 }
adam_z 0:955aa05c968a 29
adam_z 4:1a6ba05e7736 30 float CURRENT_CONTROL::saturation(float input, float limit_H, float limit_L)
adam_z 4:1a6ba05e7736 31 {
adam_z 4:1a6ba05e7736 32 float output;
adam_z 4:1a6ba05e7736 33 if(input < limit_H && input > limit_L)output = input;
adam_z 4:1a6ba05e7736 34 else output = input >=limit_H?limit_H:limit_L;
adam_z 4:1a6ba05e7736 35 return output;
adam_z 4:1a6ba05e7736 36 }
adam_z 4:1a6ba05e7736 37
adam_z 4:1a6ba05e7736 38 void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
adam_z 0:955aa05c968a 39 {
adam_z 2:562bd14dfd3a 40 analogInValue = currentAnalogIn.read();
adam_z 4:1a6ba05e7736 41 curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur, 10);
adam_z 0:955aa05c968a 42 pid.Compute(curRef, curFeedBack);
adam_z 4:1a6ba05e7736 43 MotorPlus = 0.5 + saturation((-pid.output + Kw*angularSpeed)*voltage2Duty, 0.5, -0.5) ;
adam_z 1:c5973a56d474 44 if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
adam_z 1:c5973a56d474 45 else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
adam_z 1:c5973a56d474 46 }
adam_z 1:c5973a56d474 47
adam_z 4:1a6ba05e7736 48 void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
adam_z 1:c5973a56d474 49 {
adam_z 4:1a6ba05e7736 50 analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
adam_z 4:1a6ba05e7736 51 Kw = angSpeed2BackEmf;
adam_z 4:1a6ba05e7736 52 voltage2Duty = voltage2DutyRatio;
adam_z 4:1a6ba05e7736 53
adam_z 1:c5973a56d474 54 }
adam_z 3:c787d1c5ad6a 55
adam_z 3:c787d1c5ad6a 56
adam_z 2:562bd14dfd3a 57 //****************for test************************//
adam_z 1:c5973a56d474 58 void CURRENT_CONTROL::SetPWMDuty(float ratio)
adam_z 1:c5973a56d474 59 {
adam_z 1:c5973a56d474 60 MotorPlus = ratio;
adam_z 1:c5973a56d474 61 if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
adam_z 1:c5973a56d474 62 else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
adam_z 2:562bd14dfd3a 63 }
adam_z 2:562bd14dfd3a 64
adam_z 2:562bd14dfd3a 65 float CURRENT_CONTROL::GetAnalogIn(void)
adam_z 2:562bd14dfd3a 66 {
adam_z 2:562bd14dfd3a 67 return analogInValue = currentAnalogIn.read();
adam_z 2:562bd14dfd3a 68 }
adam_z 2:562bd14dfd3a 69
adam_z 2:562bd14dfd3a 70 float CURRENT_CONTROL::GetCurrent(void)
adam_z 2:562bd14dfd3a 71 {
adam_z 3:c787d1c5ad6a 72 return curFeedBack;
adam_z 3:c787d1c5ad6a 73 }
adam_z 3:c787d1c5ad6a 74
adam_z 3:c787d1c5ad6a 75 //=====================LPF ====================//
adam_z 3:c787d1c5ad6a 76 LPF::LPF(float samplingTime)
adam_z 3:c787d1c5ad6a 77 {
adam_z 3:c787d1c5ad6a 78 Ts = samplingTime;
adam_z 3:c787d1c5ad6a 79 }
adam_z 3:c787d1c5ad6a 80
adam_z 3:c787d1c5ad6a 81 float LPF::filter(float input, float cutOff)
adam_z 3:c787d1c5ad6a 82 {
adam_z 4:1a6ba05e7736 83 output = (1.0 - cutOff*Ts)*outputLast + cutOff*Ts*input;
adam_z 3:c787d1c5ad6a 84 outputLast = output;
adam_z 3:c787d1c5ad6a 85 return output;
adam_z 0:955aa05c968a 86 }