![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
3-26-2015 DSV
Fork of R5_StepperDrive by
StepperDrive.cpp@0:266a770a17e9, 2015-11-22 (annotated)
- Committer:
- jmar11
- Date:
- Sun Nov 22 19:27:47 2015 +0000
- Revision:
- 0:266a770a17e9
- Child:
- 1:909572175aad
First. Library to control drive stepper motors for TX state IEEE team 1.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jmar11 | 0:266a770a17e9 | 1 | #include "mbed.h" |
jmar11 | 0:266a770a17e9 | 2 | #include "StepperDrive.h" |
jmar11 | 0:266a770a17e9 | 3 | |
jmar11 | 0:266a770a17e9 | 4 | 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) |
jmar11 | 0:266a770a17e9 | 5 | { |
jmar11 | 0:266a770a17e9 | 6 | wheelCircum=in7; |
jmar11 | 0:266a770a17e9 | 7 | wheelSepar=in8; |
jmar11 | 0:266a770a17e9 | 8 | invertLeft=in3; |
jmar11 | 0:266a770a17e9 | 9 | invertRight=in6; |
jmar11 | 0:266a770a17e9 | 10 | pit.attach(this, &StepperDrive::pitCallback, in9); |
jmar11 | 0:266a770a17e9 | 11 | moveComplete=true; |
jmar11 | 0:266a770a17e9 | 12 | } |
jmar11 | 0:266a770a17e9 | 13 | |
jmar11 | 0:266a770a17e9 | 14 | int StepperDrive::move(float distance, float angle) |
jmar11 | 0:266a770a17e9 | 15 | { |
jmar11 | 0:266a770a17e9 | 16 | if(moveComplete==false) //if there is a move in progress |
jmar11 | 0:266a770a17e9 | 17 | { //return |
jmar11 | 0:266a770a17e9 | 18 | return -1; |
jmar11 | 0:266a770a17e9 | 19 | } |
jmar11 | 0:266a770a17e9 | 20 | |
jmar11 | 0:266a770a17e9 | 21 | float stepDistance=wheelCircum/(200*Ustep); |
jmar11 | 0:266a770a17e9 | 22 | float d, dl, dr; |
jmar11 | 0:266a770a17e9 | 23 | |
jmar11 | 0:266a770a17e9 | 24 | if(distance==0 && angle!=0) |
jmar11 | 0:266a770a17e9 | 25 | { |
jmar11 | 0:266a770a17e9 | 26 | d=angle*wheelSepar/2; |
jmar11 | 0:266a770a17e9 | 27 | leftSteps=d/stepDistance; |
jmar11 | 0:266a770a17e9 | 28 | rightSteps=-1*leftSteps; |
jmar11 | 0:266a770a17e9 | 29 | leftStepsPC=(angle>0 ? -1:1); |
jmar11 | 0:266a770a17e9 | 30 | rightStepsPC=-1*leftStepsPC; |
jmar11 | 0:266a770a17e9 | 31 | moveComplete=false; |
jmar11 | 0:266a770a17e9 | 32 | } |
jmar11 | 0:266a770a17e9 | 33 | else if(angle==0 && distance!=0) |
jmar11 | 0:266a770a17e9 | 34 | { |
jmar11 | 0:266a770a17e9 | 35 | leftSteps=distance/stepDistance; |
jmar11 | 0:266a770a17e9 | 36 | rightSteps=leftSteps; |
jmar11 | 0:266a770a17e9 | 37 | leftStepsPC=-1; |
jmar11 | 0:266a770a17e9 | 38 | rightStepsPC=-1; |
jmar11 | 0:266a770a17e9 | 39 | moveComplete=false; |
jmar11 | 0:266a770a17e9 | 40 | } |
jmar11 | 0:266a770a17e9 | 41 | else if(angle>0 && distance!=0) |
jmar11 | 0:266a770a17e9 | 42 | { |
jmar11 | 0:266a770a17e9 | 43 | dl=angle*(distance/angle+wheelSepar/2); |
jmar11 | 0:266a770a17e9 | 44 | dr=angle*(distance/angle-wheelSepar/2); |
jmar11 | 0:266a770a17e9 | 45 | |
jmar11 | 0:266a770a17e9 | 46 | leftSteps=dl/stepDistance; |
jmar11 | 0:266a770a17e9 | 47 | rightSteps=dr/stepDistance; |
jmar11 | 0:266a770a17e9 | 48 | leftStepsPC=-1; |
jmar11 | 0:266a770a17e9 | 49 | rightStepsPC=-1*(dr/dl); |
jmar11 | 0:266a770a17e9 | 50 | moveComplete=false; |
jmar11 | 0:266a770a17e9 | 51 | } |
jmar11 | 0:266a770a17e9 | 52 | else if(angle<0 && distance!=0) |
jmar11 | 0:266a770a17e9 | 53 | { |
jmar11 | 0:266a770a17e9 | 54 | dl=angle*(distance/angle-wheelSepar/2); |
jmar11 | 0:266a770a17e9 | 55 | dr=angle*(distance/angle+wheelSepar/2); |
jmar11 | 0:266a770a17e9 | 56 | |
jmar11 | 0:266a770a17e9 | 57 | leftSteps=dl/stepDistance; |
jmar11 | 0:266a770a17e9 | 58 | rightSteps=dr/stepDistance; |
jmar11 | 0:266a770a17e9 | 59 | leftStepsPC=-1*(dl/dr); |
jmar11 | 0:266a770a17e9 | 60 | rightStepsPC=-1; |
jmar11 | 0:266a770a17e9 | 61 | moveComplete=false; |
jmar11 | 0:266a770a17e9 | 62 | } |
jmar11 | 0:266a770a17e9 | 63 | leftError=0; |
jmar11 | 0:266a770a17e9 | 64 | rightError=0; |
jmar11 | 0:266a770a17e9 | 65 | return 0; |
jmar11 | 0:266a770a17e9 | 66 | } |
jmar11 | 0:266a770a17e9 | 67 | |
jmar11 | 0:266a770a17e9 | 68 | void StepperDrive::pitCallback() |
jmar11 | 0:266a770a17e9 | 69 | { |
jmar11 | 0:266a770a17e9 | 70 | if(moveComplete==true) |
jmar11 | 0:266a770a17e9 | 71 | { |
jmar11 | 0:266a770a17e9 | 72 | return; |
jmar11 | 0:266a770a17e9 | 73 | } |
jmar11 | 0:266a770a17e9 | 74 | else |
jmar11 | 0:266a770a17e9 | 75 | { |
jmar11 | 0:266a770a17e9 | 76 | if(leftSteps!=0) |
jmar11 | 0:266a770a17e9 | 77 | { |
jmar11 | 0:266a770a17e9 | 78 | leftError+=(fabs(leftStepsPC)-floor(fabs(leftStepsPC))); |
jmar11 | 0:266a770a17e9 | 79 | for(int i=0; i<floor(fabs(leftStepsPC)+leftError); i++) |
jmar11 | 0:266a770a17e9 | 80 | { |
jmar11 | 0:266a770a17e9 | 81 | stepLeft(leftStepsPC>0); |
jmar11 | 0:266a770a17e9 | 82 | } |
jmar11 | 0:266a770a17e9 | 83 | if(signbit(leftSteps)!=signbit(leftSteps-floor(fabs(leftStepsPC)+leftError))) |
jmar11 | 0:266a770a17e9 | 84 | { |
jmar11 | 0:266a770a17e9 | 85 | leftSteps=0; |
jmar11 | 0:266a770a17e9 | 86 | } |
jmar11 | 0:266a770a17e9 | 87 | if(leftError>=1) |
jmar11 | 0:266a770a17e9 | 88 | { |
jmar11 | 0:266a770a17e9 | 89 | leftError-=1; |
jmar11 | 0:266a770a17e9 | 90 | } |
jmar11 | 0:266a770a17e9 | 91 | } |
jmar11 | 0:266a770a17e9 | 92 | if(rightSteps!=0) |
jmar11 | 0:266a770a17e9 | 93 | { |
jmar11 | 0:266a770a17e9 | 94 | rightError+=(fabs(rightStepsPC)-floor(fabs(rightStepsPC))); |
jmar11 | 0:266a770a17e9 | 95 | for(int i=0; i<floor(fabs(rightStepsPC)+rightError); i++) |
jmar11 | 0:266a770a17e9 | 96 | { |
jmar11 | 0:266a770a17e9 | 97 | stepRight(rightStepsPC>0); |
jmar11 | 0:266a770a17e9 | 98 | } |
jmar11 | 0:266a770a17e9 | 99 | if(signbit(rightSteps)!=signbit(rightSteps-floor(fabs(rightStepsPC)+leftError))) |
jmar11 | 0:266a770a17e9 | 100 | { |
jmar11 | 0:266a770a17e9 | 101 | rightSteps=0; |
jmar11 | 0:266a770a17e9 | 102 | } |
jmar11 | 0:266a770a17e9 | 103 | if(rightError>=1) |
jmar11 | 0:266a770a17e9 | 104 | { |
jmar11 | 0:266a770a17e9 | 105 | rightError-=1; |
jmar11 | 0:266a770a17e9 | 106 | } |
jmar11 | 0:266a770a17e9 | 107 | } |
jmar11 | 0:266a770a17e9 | 108 | if(leftSteps==0 && rightSteps==0) |
jmar11 | 0:266a770a17e9 | 109 | { |
jmar11 | 0:266a770a17e9 | 110 | moveComplete=true; |
jmar11 | 0:266a770a17e9 | 111 | } |
jmar11 | 0:266a770a17e9 | 112 | } |
jmar11 | 0:266a770a17e9 | 113 | } |
jmar11 | 0:266a770a17e9 | 114 | |
jmar11 | 0:266a770a17e9 | 115 | void StepperDrive::stepLeft(bool dir) |
jmar11 | 0:266a770a17e9 | 116 | { |
jmar11 | 0:266a770a17e9 | 117 | leftDir=(invertLeft^dir); |
jmar11 | 0:266a770a17e9 | 118 | leftStep=1; |
jmar11 | 0:266a770a17e9 | 119 | wait_us(3); |
jmar11 | 0:266a770a17e9 | 120 | leftStep=0; |
jmar11 | 0:266a770a17e9 | 121 | } |
jmar11 | 0:266a770a17e9 | 122 | |
jmar11 | 0:266a770a17e9 | 123 | void StepperDrive::stepRight(bool dir) |
jmar11 | 0:266a770a17e9 | 124 | { |
jmar11 | 0:266a770a17e9 | 125 | rightDir=(invertRight^dir); |
jmar11 | 0:266a770a17e9 | 126 | rightStep=1; |
jmar11 | 0:266a770a17e9 | 127 | wait_us(3); |
jmar11 | 0:266a770a17e9 | 128 | rightStep=0; |
jmar11 | 0:266a770a17e9 | 129 | } |
jmar11 | 0:266a770a17e9 | 130 | |
jmar11 | 0:266a770a17e9 | 131 | bool StepperDrive::isMoveDone() |
jmar11 | 0:266a770a17e9 | 132 | { |
jmar11 | 0:266a770a17e9 | 133 | return moveComplete; |
jmar11 | 0:266a770a17e9 | 134 | } |