Texas State IEEE / R5_StepperDrive

debugged pauseMove and resumeMove JJ

Dependents:   steppertest GrabTest R5 2016 Robotics Team 1

Fork of R5_StepperDrive by David Vasquez

StepperDrive.cpp@10:1a79af981c7b, 2016-04-09 (annotated)

Committer:
j_j205
Date:
Sat Apr 09 04:58:38 2016 +0000
Revision:
10:1a79af981c7b
Parent:
9:fb26142680b9 
set value of enable to 1 (disable steppers) in constructor

Who changed what in which revision?

UserRevisionLine numberNew 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 9:fb26142680b9 5 StepperDrive::StepperDrive(Serial &pc1, PinName in1, PinName in2, bool in3, PinName in4, PinName in5, PinName enablePin, bool in6, float in7, float in8, float in9) : pc(pc1), leftStep(in1), leftDir(in2), rightStep(in4), rightDir(in5), enable(enablePin)
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 10:1a79af981c7b 13 enable = 1; // disables steppers
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 {
j_j205 10:1a79af981c7b 74 enable = 1; // disables steppers
jmar11 0:266a770a17e9 75 return;
jmar11 0:266a770a17e9 76 }
jmar11 0:266a770a17e9 77 else
jmar11 0:266a770a17e9 78 {
j_j205 10:1a79af981c7b 79 enable = 0; // enable Steppers
jmar11 0:266a770a17e9 80 if(leftSteps!=0)
jmar11 0:266a770a17e9 81 {
jmar11 0:266a770a17e9 82 leftError+=(fabs(leftStepsPC)-floor(fabs(leftStepsPC)));
jmar11 0:266a770a17e9 83 for(int i=0; i<floor(fabs(leftStepsPC)+leftError); i++)
jmar11 0:266a770a17e9 84 {
jmar11 0:266a770a17e9 85 stepLeft(leftStepsPC>0);
jmar11 0:266a770a17e9 86 }
jmar11 0:266a770a17e9 87 if(signbit(leftSteps)!=signbit(leftSteps-floor(fabs(leftStepsPC)+leftError)))
jmar11 0:266a770a17e9 88 {
jmar11 0:266a770a17e9 89 leftSteps=0;
jmar11 0:266a770a17e9 90 }
jmar11 0:266a770a17e9 91 if(leftError>=1)
jmar11 0:266a770a17e9 92 {
jmar11 0:266a770a17e9 93 leftError-=1;
jmar11 0:266a770a17e9 94 }
jmar11 0:266a770a17e9 95 }
jmar11 0:266a770a17e9 96 if(rightSteps!=0)
jmar11 0:266a770a17e9 97 {
jmar11 0:266a770a17e9 98 rightError+=(fabs(rightStepsPC)-floor(fabs(rightStepsPC)));
jmar11 0:266a770a17e9 99 for(int i=0; i<floor(fabs(rightStepsPC)+rightError); i++)
jmar11 0:266a770a17e9 100 {
jmar11 0:266a770a17e9 101 stepRight(rightStepsPC>0);
jmar11 0:266a770a17e9 102 }
j_j205 2:80c0b2a5adc0 103 if(signbit(rightSteps)!=signbit(rightSteps-floor(fabs(rightStepsPC)+rightError)))
jmar11 0:266a770a17e9 104 {
jmar11 0:266a770a17e9 105 rightSteps=0;
jmar11 0:266a770a17e9 106 }
jmar11 0:266a770a17e9 107 if(rightError>=1)
jmar11 0:266a770a17e9 108 {
jmar11 0:266a770a17e9 109 rightError-=1;
jmar11 0:266a770a17e9 110 }
jmar11 0:266a770a17e9 111 }
jmar11 0:266a770a17e9 112 if(leftSteps==0 && rightSteps==0)
jmar11 0:266a770a17e9 113 {
jmar11 0:266a770a17e9 114 moveComplete=true;
jmar11 0:266a770a17e9 115 }
jmar11 0:266a770a17e9 116 }
jmar11 0:266a770a17e9 117 }
jmar11 0:266a770a17e9 118
jmar11 0:266a770a17e9 119 void StepperDrive::stepLeft(bool dir)
jmar11 0:266a770a17e9 120 {
jmar11 0:266a770a17e9 121 leftDir=(invertLeft^dir);
jmar11 0:266a770a17e9 122 leftStep=1;
jmar11 0:266a770a17e9 123 wait_us(3);
j_j205 2:80c0b2a5adc0 124 leftStep=0;
j_j205 3:97bea13f40a9 125
j_j205 2:80c0b2a5adc0 126 /* completed 1 step (increment or decrement */
j_j205 2:80c0b2a5adc0 127 if(leftSteps < 0)
j_j205 2:80c0b2a5adc0 128 leftSteps++;
j_j205 2:80c0b2a5adc0 129 else
j_j205 3:97bea13f40a9 130 leftSteps--;
jmar11 0:266a770a17e9 131 }
jmar11 0:266a770a17e9 132
jmar11 0:266a770a17e9 133 void StepperDrive::stepRight(bool dir)
jmar11 0:266a770a17e9 134 {
jmar11 0:266a770a17e9 135 rightDir=(invertRight^dir);
jmar11 0:266a770a17e9 136 rightStep=1;
jmar11 0:266a770a17e9 137 wait_us(3);
j_j205 2:80c0b2a5adc0 138 rightStep=0;
j_j205 3:97bea13f40a9 139
j_j205 2:80c0b2a5adc0 140 /* completed 1 step (increment or decrement */
j_j205 2:80c0b2a5adc0 141 if(rightSteps < 0)
j_j205 2:80c0b2a5adc0 142 rightSteps++;
j_j205 2:80c0b2a5adc0 143 else
j_j205 3:97bea13f40a9 144 rightSteps--;
jmar11 0:266a770a17e9 145 }
jmar11 0:266a770a17e9 146
jmar11 0:266a770a17e9 147 bool StepperDrive::isMoveDone()
jmar11 0:266a770a17e9 148 {
jmar11 0:266a770a17e9 149 return moveComplete;
j_j205 4:754c74beef20 150 }
j_j205 4:754c74beef20 151
j_j205 4:754c74beef20 152 // FUNCTION:
j_j205 4:754c74beef20 153 // void pauseMove()
j_j205 4:754c74beef20 154 // IN-PARAMETERS:
j_j205 4:754c74beef20 155 // None
j_j205 4:754c74beef20 156 // OUT-PARAMETERS:
j_j205 4:754c74beef20 157 // None
j_j205 4:754c74beef20 158 // DESCRIPTION:
j_j205 4:754c74beef20 159 // Stops current move. Saves remaining steps in leftStepsPause
j_j205 4:754c74beef20 160 // and rightStepsPause.
Hellkite85 5:f611950fdeba 161 void StepperDrive::pauseMove()
j_j205 4:754c74beef20 162 {
j_j205 6:2657751be34b 163 moveComplete = true;
j_j205 6:2657751be34b 164
Hellkite85 5:f611950fdeba 165 leftStepsPause = leftSteps;
Hellkite85 5:f611950fdeba 166 rightStepsPause = rightSteps;
j_j205 6:2657751be34b 167 leftStepsPCPause = leftStepsPC;
j_j205 6:2657751be34b 168 rightStepsPCPause = rightStepsPC;
j_j205 6:2657751be34b 169 leftErrorPause = leftError;
j_j205 6:2657751be34b 170 rightErrorPause = rightError;
j_j205 6:2657751be34b 171 invertLeftPause = invertLeft;
j_j205 6:2657751be34b 172 invertRightPause = invertRight;
Hellkite85 5:f611950fdeba 173
Hellkite85 5:f611950fdeba 174 leftSteps = 0;
Hellkite85 5:f611950fdeba 175 rightSteps = 0;
j_j205 4:754c74beef20 176 }
j_j205 4:754c74beef20 177
j_j205 4:754c74beef20 178 // FUNCTION:
j_j205 4:754c74beef20 179 // void resumeMove()
j_j205 4:754c74beef20 180 // IN-PARAMETERS:
j_j205 4:754c74beef20 181 // None
j_j205 4:754c74beef20 182 // OUT-PARAMETERS:
j_j205 4:754c74beef20 183 // None
j_j205 4:754c74beef20 184 // DESCRIPTION:
j_j205 4:754c74beef20 185 // Restores remaining steps from leftStepsPause
j_j205 4:754c74beef20 186 // and rightStepsPause to leftSteps and rightSteps. Resumes
j_j205 4:754c74beef20 187 // move.
Hellkite85 5:f611950fdeba 188 void StepperDrive::resumeMove()
j_j205 4:754c74beef20 189 {
Hellkite85 5:f611950fdeba 190 leftSteps = leftStepsPause;
Hellkite85 5:f611950fdeba 191 rightSteps = rightStepsPause;
j_j205 6:2657751be34b 192 leftStepsPC = leftStepsPCPause;
j_j205 6:2657751be34b 193 rightStepsPC = rightStepsPCPause;
j_j205 6:2657751be34b 194 leftError = leftErrorPause;
j_j205 6:2657751be34b 195 rightError = rightErrorPause;
j_j205 6:2657751be34b 196 invertLeft = invertLeftPause;
j_j205 6:2657751be34b 197 invertRight = invertRightPause;
Hellkite85 5:f611950fdeba 198
Hellkite85 5:f611950fdeba 199 leftStepsPause = 0;
Hellkite85 5:f611950fdeba 200 rightStepsPause = 0;
Hellkite85 5:f611950fdeba 201
j_j205 6:2657751be34b 202 if ((leftSteps == 0) && (rightSteps == 0))
j_j205 6:2657751be34b 203 moveComplete = true;
j_j205 6:2657751be34b 204 else
j_j205 6:2657751be34b 205 moveComplete = false;
j_j205 6:2657751be34b 206
j_j205 6:2657751be34b 207 /* may need to wait for move to complete here */
jmar11 0:266a770a17e9 208 }