v1
motor.cpp
- Committer:
- kangmingyo
- Date:
- 2019-06-25
- Revision:
- 5:15d0276d7e21
- Parent:
- 4:a629f46d6594
- Child:
- 6:366ec55e64fe
File content as of revision 5:15d0276d7e21:
#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(32); _pwm.write(0); _tick.attach_us(this,&MotorCtl::PIDControl,32000); kp=0.1; ki=0.2; 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 = 0; //initial target speed vel=0; DistancePosition =0; } 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 // printf("Target %d\r\n",TargetSpeed); } 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; DistancePosition +=1; break; case 1: if(PreviousEncode==0) CurrentPosition +=1; else if(PreviousEncode==3) CurrentPosition -=1; DistancePosition +=1; break; case 2: if(PreviousEncode==3) CurrentPosition +=1; else if(PreviousEncode==0) CurrentPosition -=1; DistancePosition +=1; break; case 3: if(PreviousEncode==1) CurrentPosition +=1; else if(PreviousEncode==2) CurrentPosition -=1; DistancePosition +=1; break; default: break; } PreviousEncode = CurrentEncode; // printf("CSP: %x\r\n",CurrentPosition); } void MotorCtl::PIDControl() { int DeltaCnt; DeltaCnt = CurrentPosition - PreviousPosition; PreviousPosition = CurrentPosition; CurrentSpeed = (int)CalculateRPM(DeltaCnt); // printf("DeltaCnt: %d CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed); Error = TargetSpeed - CurrentSpeed; integ += Error; derv = Error-PreviousError; control = (int)(kp*Error+ki*integ+kd*derv); if (control>40) control=40; else if(control <-40) control = -40; PreviousError = Error; if(TargetSpeed>0){ vel =(TargetSpeed+control)/200.0; _pwm.write(vel); _Dir=1; // printf("control: %d\r\n",control); }else{ vel =(-TargetSpeed-control)/200.0; _pwm.write(vel); _Dir=0; } // printf("Duty: %d Target:%d control:%d\r\n",duty,TargetSpeed, control); } float MotorCtl::CalculateRPM(int DeltaCnt) { return(DeltaCnt*60.0*1000.0)/(float)(DeltaT*CntPerRev); // Time measured in us } float MotorCtl::CalculateCumDis() //accumulate Distance { return (DistancePosition*3.14159265359*0.043/CntPerRev); // Time measured in us } float MotorCtl::CalculateRelaDis() //relative Distance { return (CurrentPosition*3.14159265359*0.043/CntPerRev); }