CURRENT_CONTROL.cpp

00001 #include "mbed.h"
00002 #include "CURRENT_CONTROL.h"
00003 
00004 
00005 CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
00006                                  PinName PwmChannel1,
00007                                  PinName PwmChannel2,
00008                                  PWMIndex pwmIndex,
00009                                  float Kp, float Ki, float Kd,
00010                                  float samplingTime) : currentAnalogIn(curChannel),
00011     MotorPlus(PwmChannel1),
00012     MotorMinus(PwmChannel2),
00013     pid(Kp,Ki,Kd,samplingTime),
00014     lpFilter(samplingTime)
00015     
00016 {
00017     pwmIndex_ = pwmIndex;
00018 
00019     Ts = samplingTime;
00020     //setup motor PWM parameters
00021     MotorPlus.period_us(15);//default period equals to 25 microseconds
00022     MotorPlus.write(0.5);   //duty ratio = 0.5 in complementary output mode -> static
00023     if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
00024     else if(pwmIndex_ == PWM2)TIM1->CCER |= 64; //enable complimentary output
00025     
00026     
00027     
00028 }
00029 
00030 float CURRENT_CONTROL::saturation(float input, float limit_H, float limit_L)
00031 {
00032     float output;
00033     if(input < limit_H && input > limit_L)output = input;
00034     else output = input >=limit_H?limit_H:limit_L;
00035     return output;
00036 }
00037 
00038 void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
00039 {
00040     analogInValue = currentAnalogIn.read();
00041     curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur, 10);
00042     pid.Compute(curRef, curFeedBack);
00043     MotorPlus = 0.5 + saturation((-pid.output + Kw*angularSpeed)*voltage2Duty, 0.5, -0.5) ;
00044     if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
00045     else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
00046 }
00047 
00048 void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
00049 {
00050     analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
00051     Kw = angSpeed2BackEmf;
00052     voltage2Duty = voltage2DutyRatio;
00053     
00054 }
00055 
00056 
00057 //****************for test************************//
00058 void CURRENT_CONTROL::SetPWMDuty(float ratio)
00059 {
00060     MotorPlus = ratio;
00061     if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
00062     else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
00063 }
00064 
00065 float CURRENT_CONTROL::GetAnalogIn(void)
00066 {
00067     return analogInValue = currentAnalogIn.read();   
00068 }
00069 
00070 float CURRENT_CONTROL::GetCurrent(void)
00071 {
00072     return  curFeedBack;   
00073 }
00074 
00075 //=====================LPF ====================//
00076 LPF::LPF(float samplingTime)
00077 {
00078     Ts = samplingTime;
00079 }
00080 
00081 float LPF::filter(float input, float cutOff)
00082 {
00083     output = (1.0 - cutOff*Ts)*outputLast + cutOff*Ts*input;
00084     outputLast = output;
00085     return output;
00086 }