David Vasquez
/
R5_StepperDrive
3-26-2015 DSV
Fork of R5_StepperDrive by
Diff: StepperDrive.cpp
- Revision:
- 0:266a770a17e9
- Child:
- 1:909572175aad
diff -r 000000000000 -r 266a770a17e9 StepperDrive.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/StepperDrive.cpp Sun Nov 22 19:27:47 2015 +0000 @@ -0,0 +1,134 @@ +#include "mbed.h" +#include "StepperDrive.h" + +StepperDrive::StepperDrive(PinName in1, PinName in2, bool in3, PinName in4, PinName in5, bool in6, float in7, float in8, float in9):leftStep(in1), leftDir(in2), rightStep(in4), rightDir(in5) +{ + wheelCircum=in7; + wheelSepar=in8; + invertLeft=in3; + invertRight=in6; + pit.attach(this, &StepperDrive::pitCallback, in9); + moveComplete=true; +} + +int StepperDrive::move(float distance, float angle) +{ + if(moveComplete==false) //if there is a move in progress + { //return + return -1; + } + + float stepDistance=wheelCircum/(200*Ustep); + float d, dl, dr; + + if(distance==0 && angle!=0) + { + d=angle*wheelSepar/2; + leftSteps=d/stepDistance; + rightSteps=-1*leftSteps; + leftStepsPC=(angle>0 ? -1:1); + rightStepsPC=-1*leftStepsPC; + moveComplete=false; + } + else if(angle==0 && distance!=0) + { + leftSteps=distance/stepDistance; + rightSteps=leftSteps; + leftStepsPC=-1; + rightStepsPC=-1; + moveComplete=false; + } + else if(angle>0 && distance!=0) + { + dl=angle*(distance/angle+wheelSepar/2); + dr=angle*(distance/angle-wheelSepar/2); + + leftSteps=dl/stepDistance; + rightSteps=dr/stepDistance; + leftStepsPC=-1; + rightStepsPC=-1*(dr/dl); + moveComplete=false; + } + else if(angle<0 && distance!=0) + { + dl=angle*(distance/angle-wheelSepar/2); + dr=angle*(distance/angle+wheelSepar/2); + + leftSteps=dl/stepDistance; + rightSteps=dr/stepDistance; + leftStepsPC=-1*(dl/dr); + rightStepsPC=-1; + moveComplete=false; + } + leftError=0; + rightError=0; + return 0; +} + +void StepperDrive::pitCallback() +{ + if(moveComplete==true) + { + return; + } + else + { + if(leftSteps!=0) + { + leftError+=(fabs(leftStepsPC)-floor(fabs(leftStepsPC))); + for(int i=0; i<floor(fabs(leftStepsPC)+leftError); i++) + { + stepLeft(leftStepsPC>0); + } + if(signbit(leftSteps)!=signbit(leftSteps-floor(fabs(leftStepsPC)+leftError))) + { + leftSteps=0; + } + if(leftError>=1) + { + leftError-=1; + } + } + if(rightSteps!=0) + { + rightError+=(fabs(rightStepsPC)-floor(fabs(rightStepsPC))); + for(int i=0; i<floor(fabs(rightStepsPC)+rightError); i++) + { + stepRight(rightStepsPC>0); + } + if(signbit(rightSteps)!=signbit(rightSteps-floor(fabs(rightStepsPC)+leftError))) + { + rightSteps=0; + } + if(rightError>=1) + { + rightError-=1; + } + } + if(leftSteps==0 && rightSteps==0) + { + moveComplete=true; + } + } +} + +void StepperDrive::stepLeft(bool dir) +{ + leftDir=(invertLeft^dir); + leftStep=1; + wait_us(3); + leftStep=0; +} + +void StepperDrive::stepRight(bool dir) +{ + rightDir=(invertRight^dir); + rightStep=1; + wait_us(3); + rightStep=0; +} + +bool StepperDrive::isMoveDone() +{ + return moveComplete; +} \ No newline at end of file