v1
motor.cpp
- Committer:
- kangmingyo
- Date:
- 2019-05-09
- Revision:
- 2:01227a4bd3d7
- Parent:
- 1:911f5a86c105
- Child:
- 3:df8469e71728
File content as of revision 2:01227a4bd3d7:
#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=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(TargetSpeed>0){ duty += control; _Dir=1; }else{ duty +=- control; _Dir=0; } vel=duty/1000000; _pwm.write(vel); printf("duty: %d Error: %d\r\n",duty,Error); } float MotorCtl::CalculateRPM(int DeltaCnt) { return DeltaCnt*60/(DeltaT)/CntPerRev; // Time measured in us }