190711
motor.cpp
- Committer:
- kangmingyo
- Date:
- 2019-05-23
- Revision:
- 3:df8469e71728
- Parent:
- 2:01227a4bd3d7
- Child:
- 4:a629f46d6594
File content as of revision 3:df8469e71728:
#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.2; 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; // printf("PreviousEncode: %x CSP: %x\r\n",PreviousEncode,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>35) control=35; else if(control <-35) control = -35; PreviousError = Error; if(TargetSpeed>0){ duty =TargetSpeed+control; _Dir=1; //printf("control: %d\r\n",control); }else{ duty =TargetSpeed-control; _Dir=0; } //if(duty<=0){duty=-duty;} vel=duty/180.0; printf("Duty: %d vel: %f\r\n",duty,vel); _pwm.write(vel); } float MotorCtl::CalculateRPM(int DeltaCnt) { return(DeltaCnt*60*1000)/(DeltaT*CntPerRev); // Time measured in us }