Justin Jordan / Mbed 2 deprecated R5_StepperDrive

update 1/27/16

Dependencies:   mbed

Fork of R5_StepperDrive by Jaime Martinez

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?

UserRevisionLine numberNew 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 }