debugged pauseMove and resumeMove JJ
Dependents: steppertest GrabTest R5 2016 Robotics Team 1
Fork of R5_StepperDrive by
StepperDrive.cpp
- Committer:
- Hellkite85
- Date:
- 2016-03-26
- Revision:
- 5:f611950fdeba
- Parent:
- 4:754c74beef20
- Child:
- 6:2657751be34b
File content as of revision 5:f611950fdeba:
#include "mbed.h" #include "StepperDrive.h" #include <cmath> // fabs, floor, signbit StepperDrive::StepperDrive(Serial &pc1, PinName in1, PinName in2, bool in3, PinName in4, PinName in5, bool in6, float in7, float in8, float in9):pc(pc1), leftStep(in1), leftDir(in2), rightStep(in4), rightDir(in5) { wheelCircum=in7; wheelSepar=in8; invertLeft=in3; invertRight=in6; pit.attach_us(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)+rightError))) { 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; /* completed 1 step (increment or decrement */ if(leftSteps < 0) leftSteps++; else leftSteps--; } void StepperDrive::stepRight(bool dir) { rightDir=(invertRight^dir); rightStep=1; wait_us(3); rightStep=0; /* completed 1 step (increment or decrement */ if(rightSteps < 0) rightSteps++; else rightSteps--; } bool StepperDrive::isMoveDone() { return moveComplete; } // FUNCTION: // void pauseMove() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Stops current move. Saves remaining steps in leftStepsPause // and rightStepsPause. void StepperDrive::pauseMove() { leftStepsPause = leftSteps; rightStepsPause = rightSteps; leftSteps = 0; rightSteps = 0; moveComplete = true; } // FUNCTION: // void resumeMove() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Restores remaining steps from leftStepsPause // and rightStepsPause to leftSteps and rightSteps. Resumes // move. void StepperDrive::resumeMove() { leftSteps = leftStepsPause; rightSteps = rightStepsPause; leftStepsPause = 0; rightStepsPause = 0; moveComplete = false; }