v1
Diff: motor.cpp
- Revision:
- 5:15d0276d7e21
- Parent:
- 4:a629f46d6594
- Child:
- 6:366ec55e64fe
--- a/motor.cpp Thu May 23 23:02:14 2019 +0000 +++ b/motor.cpp Tue Jun 25 11:01:39 2019 +0000 @@ -20,7 +20,8 @@ duty = 50; PreviousEncode = 0; TargetSpeed = 0; //initial target speed - + vel=0; + DistancePosition =0; } MotorCtl::~MotorCtl() @@ -73,12 +74,14 @@ 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) +void MotorCtl::setPID(float p, float i, float d) { kp = p; ki = i; @@ -112,24 +115,28 @@ 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("PreviousEncode: %x CSP: %x\r\n",PreviousEncode,CurrentPosition); +// printf("CSP: %x\r\n",CurrentPosition); } @@ -139,7 +146,7 @@ DeltaCnt = CurrentPosition - PreviousPosition; PreviousPosition = CurrentPosition; CurrentSpeed = (int)CalculateRPM(DeltaCnt); -// printf("DeltaCnt: %d CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed); +// printf("DeltaCnt: %d CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed); Error = TargetSpeed - CurrentSpeed; integ += Error; @@ -149,25 +156,16 @@ else if(control <-40) control = -40; PreviousError = Error; if(TargetSpeed>0){ - duty =(TargetSpeed+control)/200.0; - _pwm.write(duty); + vel =(TargetSpeed+control)/200.0; + _pwm.write(vel); _Dir=1; // printf("control: %d\r\n",control); }else{ - duty =(-TargetSpeed-control)/200.0; - _pwm.write(duty); + vel =(-TargetSpeed-control)/200.0; + _pwm.write(vel); _Dir=0; - } - - - -// printf("Duty: %d vel: %f\r\n",duty,vel); - - - - - - + } + // printf("Duty: %d Target:%d control:%d\r\n",duty,TargetSpeed, control); } @@ -177,5 +175,16 @@ 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); +} +