Developers_of_anti_slip_compensator / CURRENT_CONTROL

20160814

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@2:562bd14dfd3a, 2016-04-22 (annotated)

Committer:
adam_z
Date:
Fri Apr 22 15:21:31 2016 +0000
Revision:
2:562bd14dfd3a
Parent:
1:c5973a56d474
Child:
3:c787d1c5ad6a
no problem till now

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 0:955aa05c968a 13 pid(Kp,Ki,Kd,samplingTime)
adam_z 0:955aa05c968a 14 {
adam_z 1:c5973a56d474 15 pwmIndex_ = pwmIndex;
adam_z 1:c5973a56d474 16
adam_z 0:955aa05c968a 17 Ts = samplingTime;
adam_z 0:955aa05c968a 18 //setup motor PWM parameters
adam_z 1:c5973a56d474 19 MotorPlus.period_us(50);//default period equals to 50 microseconds
adam_z 1:c5973a56d474 20 MotorPlus.write(0.5); //duty ratio = 0.5 in complementary output mode -> static
adam_z 1:c5973a56d474 21 if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
adam_z 1:c5973a56d474 22 else if(pwmIndex_ == PWM2)TIM1->CCER |= 64; //enable complimentary output
adam_z 0:955aa05c968a 23
adam_z 0:955aa05c968a 24 }
adam_z 0:955aa05c968a 25
adam_z 1:c5973a56d474 26 void CURRENT_CONTROL::Control(float curRef)
adam_z 0:955aa05c968a 27 {
adam_z 2:562bd14dfd3a 28 analogInValue = currentAnalogIn.read();
adam_z 2:562bd14dfd3a 29 curFeedBack = (analogInValue - currentOffset)*analog2Cur;
adam_z 0:955aa05c968a 30 pid.Compute(curRef, curFeedBack);
adam_z 0:955aa05c968a 31 MotorPlus = 0.5 - pid.output;
adam_z 1:c5973a56d474 32 if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
adam_z 1:c5973a56d474 33 else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
adam_z 1:c5973a56d474 34 }
adam_z 1:c5973a56d474 35
adam_z 1:c5973a56d474 36 void CURRENT_CONTROL::SetAnalog2Cur(float ratio)//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
adam_z 1:c5973a56d474 37 {
adam_z 1:c5973a56d474 38 analog2Cur = ratio;
adam_z 1:c5973a56d474 39 }
adam_z 2:562bd14dfd3a 40 //****************for test************************//
adam_z 1:c5973a56d474 41 void CURRENT_CONTROL::SetPWMDuty(float ratio)
adam_z 1:c5973a56d474 42 {
adam_z 1:c5973a56d474 43 MotorPlus = ratio;
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 2:562bd14dfd3a 46 }
adam_z 2:562bd14dfd3a 47
adam_z 2:562bd14dfd3a 48 float CURRENT_CONTROL::GetAnalogIn(void)
adam_z 2:562bd14dfd3a 49 {
adam_z 2:562bd14dfd3a 50 return analogInValue = currentAnalogIn.read();
adam_z 2:562bd14dfd3a 51 }
adam_z 2:562bd14dfd3a 52
adam_z 2:562bd14dfd3a 53 float CURRENT_CONTROL::GetCurrent(void)
adam_z 2:562bd14dfd3a 54 {
adam_z 2:562bd14dfd3a 55 return ((currentAnalogIn.read()-currentOffset)*analog2Cur);
adam_z 0:955aa05c968a 56 }