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