v1
Diff: motor.cpp
- Revision:
- 0:20e026e254d6
- Child:
- 1:911f5a86c105
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor.cpp Thu May 09 22:20:02 2019 +0000 @@ -0,0 +1,171 @@ +#include "motor.h" + +MotorCtl::MotorCtl(PinName Pwm, PinName Dir, PinName tachoA, PinName tachoB): + _pwm(Pwm), _Dir(Dir), _tachoA(tachoA), _tachoB(tachoB) +{ + _Dir=1; + dr = 10; + _pwm.period_ms(50); + _pwm.write(0); + _tick.attach_us(this,&MotorCtl::PIDControl,32000); + kp=7.0; + ki=0.05; + kd=0.0; + integ=0; + derv=0; + cpidx =0; + + DeltaT = 32; // Timer Interrupt period = 32ms = 32,000 us + CntPerRev = 2400; // 100 shft/rev x 6 pls/shft x 4 evnt/pls + duty = 50; + PreviousEncode = 0; + TargetSpeed = 50; //initial target speed + +} + +MotorCtl::~MotorCtl() +{ +} + +int MotorCtl::getRPM() +{ + return CurrentSpeed; +} +// Rturn target speed +int MotorCtl::getTarget() +{ + return TargetSpeed; +} +int MotorCtl::getError() +{ + return Error; +} +float MotorCtl::getKP() +{ + return kp; +} +float MotorCtl::getKI() +{ + return ki; +} +float MotorCtl::getKD() +{ + return kd; +} +int *MotorCtl::getHistory() +{ + return cw0; +} +int MotorCtl::getCurrentPosition() +{ + return CurrentPosition; +} + +//set Method + +void MotorCtl::SetPeriod(long pwmPeriod) +{ + _pwm.period_ms(pwmPeriod); +} + +void MotorCtl::setTarget(int spd) +{ + if(spd<0) dr=0; + else dr=10; + setDirection(); + TargetSpeed = spd; + integ = 0; + derv=0; + cpidx=0; // clear +} +void MotorCtl:: setPID(float p, float i, float d) +{ + kp = p; + ki = i; + kd = d; + integ =0; + derv=0; + cpidx=0; +} + +void MotorCtl::Reset(void) +{ + integ=0; + derv=0; + cpidx=0; +} + +void MotorCtl::setDirection() +{ + if (dr>5) { + _Dir=1; + } else { + _Dir=0; + } +} + +void MotorCtl::UpdateCurrentPosition() +{ + unsigned char CurrentEncode; + CurrentEncode = (_tachoA << 1 | _tachoB); + switch(CurrentEncode) { + case 0: + if (PreviousEncode==2) CurrentPosition +=1; + else if(PreviousEncode==1) CurrentPosition -=1; + break; + case 1: + if(PreviousEncode==0) CurrentPosition +=1; + else if(PreviousEncode==3) CurrentPosition -=1; + break; + case 2: + if(PreviousEncode==3) CurrentPosition +=1; + else if(PreviousEncode==0) CurrentPosition -=1; + break; + case 3: + if(PreviousEncode==1) CurrentPosition +=1; + else if(PreviousEncode==2) CurrentPosition -=1; + break; + default: + break; + } + PreviousEncode = CurrentEncode; +} + + +void MotorCtl::PIDControl() +{ + int DeltaCnt; + DeltaCnt = CurrentPosition - PreviousPosition; + PreviousPosition = CurrentPosition; + CurrentSpeed = (int)CalculateRPM(DeltaCnt); + if (cpidx<MaxBuf) cw0[cpidx++]=CurrentSpeed; + + Error = TargetSpeed - CurrentSpeed; + + integ += Error; + derv = Error-PreviousError; + control = (int)(kp*Error+ki*integ+kd*derv); + if (control>510) control=510; + else if(control <-510) control = -510; + PreviousError = Error; + if(dr>5){ + duty+= control; + _Dir=1; + }else{ + duty += -control; + _Dir=0; + } + + + + + +} + +float MotorCtl::CalculateRPM(int DeltaCnt) +{ + return DeltaCnt*60*(1000/DeltaT)/CntPerRev; // Time measured in us +} + + +