Justin Jordan / Mbed 2 deprecated R5_StepperDrive

update 1/27/16

Dependencies:   mbed

Fork of R5_StepperDrive by Jaime Martinez

StepperDrive.cpp@1:909572175aad, 2016-01-27 (annotated)

Committer:
j_j205
Date:
Wed Jan 27 16:40:02 2016 +0000
Revision:
1:909572175aad
Parent:
0:266a770a17e9
Child:
2:80c0b2a5adc0
update 1/27/16

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
j_j205 1:909572175aad 4 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 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;
j_j205 1:909572175aad 10 pit.attach_us(this, &StepperDrive::pitCallback, in9);
jmar11 0:266a770a17e9 11 moveComplete=true;
j_j205 1:909572175aad 12
jmar11 0:266a770a17e9 13 }
jmar11 0:266a770a17e9 14
jmar11 0:266a770a17e9 15 int StepperDrive::move(float distance, float angle)
jmar11 0:266a770a17e9 16 {
jmar11 0:266a770a17e9 17 if(moveComplete==false) //if there is a move in progress
jmar11 0:266a770a17e9 18 { //return
jmar11 0:266a770a17e9 19 return -1;
jmar11 0:266a770a17e9 20 }
jmar11 0:266a770a17e9 21
jmar11 0:266a770a17e9 22 float stepDistance=wheelCircum/(200*Ustep);
jmar11 0:266a770a17e9 23 float d, dl, dr;
jmar11 0:266a770a17e9 24
jmar11 0:266a770a17e9 25 if(distance==0 && angle!=0)
jmar11 0:266a770a17e9 26 {
jmar11 0:266a770a17e9 27 d=angle*wheelSepar/2;
jmar11 0:266a770a17e9 28 leftSteps=d/stepDistance;
jmar11 0:266a770a17e9 29 rightSteps=-1*leftSteps;
jmar11 0:266a770a17e9 30 leftStepsPC=(angle>0 ? -1:1);
jmar11 0:266a770a17e9 31 rightStepsPC=-1*leftStepsPC;
jmar11 0:266a770a17e9 32 moveComplete=false;
jmar11 0:266a770a17e9 33 }
jmar11 0:266a770a17e9 34 else if(angle==0 && distance!=0)
jmar11 0:266a770a17e9 35 {
jmar11 0:266a770a17e9 36 leftSteps=distance/stepDistance;
jmar11 0:266a770a17e9 37 rightSteps=leftSteps;
jmar11 0:266a770a17e9 38 leftStepsPC=-1;
jmar11 0:266a770a17e9 39 rightStepsPC=-1;
jmar11 0:266a770a17e9 40 moveComplete=false;
jmar11 0:266a770a17e9 41 }
jmar11 0:266a770a17e9 42 else if(angle>0 && distance!=0)
jmar11 0:266a770a17e9 43 {
jmar11 0:266a770a17e9 44 dl=angle*(distance/angle+wheelSepar/2);
jmar11 0:266a770a17e9 45 dr=angle*(distance/angle-wheelSepar/2);
jmar11 0:266a770a17e9 46
jmar11 0:266a770a17e9 47 leftSteps=dl/stepDistance;
jmar11 0:266a770a17e9 48 rightSteps=dr/stepDistance;
jmar11 0:266a770a17e9 49 leftStepsPC=-1;
jmar11 0:266a770a17e9 50 rightStepsPC=-1*(dr/dl);
jmar11 0:266a770a17e9 51 moveComplete=false;
jmar11 0:266a770a17e9 52 }
jmar11 0:266a770a17e9 53 else if(angle<0 && distance!=0)
jmar11 0:266a770a17e9 54 {
jmar11 0:266a770a17e9 55 dl=angle*(distance/angle-wheelSepar/2);
jmar11 0:266a770a17e9 56 dr=angle*(distance/angle+wheelSepar/2);
jmar11 0:266a770a17e9 57
jmar11 0:266a770a17e9 58 leftSteps=dl/stepDistance;
jmar11 0:266a770a17e9 59 rightSteps=dr/stepDistance;
jmar11 0:266a770a17e9 60 leftStepsPC=-1*(dl/dr);
jmar11 0:266a770a17e9 61 rightStepsPC=-1;
jmar11 0:266a770a17e9 62 moveComplete=false;
jmar11 0:266a770a17e9 63 }
jmar11 0:266a770a17e9 64 leftError=0;
jmar11 0:266a770a17e9 65 rightError=0;
j_j205 1:909572175aad 66 pc.printf("\n Left Steps - %i", leftSteps);
j_j205 1:909572175aad 67 pc.printf("\n Right Steps - %i", rightSteps);
jmar11 0:266a770a17e9 68 return 0;
jmar11 0:266a770a17e9 69 }
jmar11 0:266a770a17e9 70
jmar11 0:266a770a17e9 71 void StepperDrive::pitCallback()
jmar11 0:266a770a17e9 72 {
jmar11 0:266a770a17e9 73 if(moveComplete==true)
jmar11 0:266a770a17e9 74 {
jmar11 0:266a770a17e9 75 return;
jmar11 0:266a770a17e9 76 }
jmar11 0:266a770a17e9 77 else
jmar11 0:266a770a17e9 78 {
jmar11 0:266a770a17e9 79 if(leftSteps!=0)
jmar11 0:266a770a17e9 80 {
jmar11 0:266a770a17e9 81 leftError+=(fabs(leftStepsPC)-floor(fabs(leftStepsPC)));
jmar11 0:266a770a17e9 82 for(int i=0; i<floor(fabs(leftStepsPC)+leftError); i++)
jmar11 0:266a770a17e9 83 {
jmar11 0:266a770a17e9 84 stepLeft(leftStepsPC>0);
jmar11 0:266a770a17e9 85 }
jmar11 0:266a770a17e9 86 if(signbit(leftSteps)!=signbit(leftSteps-floor(fabs(leftStepsPC)+leftError)))
jmar11 0:266a770a17e9 87 {
jmar11 0:266a770a17e9 88 leftSteps=0;
jmar11 0:266a770a17e9 89 }
jmar11 0:266a770a17e9 90 if(leftError>=1)
jmar11 0:266a770a17e9 91 {
jmar11 0:266a770a17e9 92 leftError-=1;
jmar11 0:266a770a17e9 93 }
jmar11 0:266a770a17e9 94 }
jmar11 0:266a770a17e9 95 if(rightSteps!=0)
jmar11 0:266a770a17e9 96 {
jmar11 0:266a770a17e9 97 rightError+=(fabs(rightStepsPC)-floor(fabs(rightStepsPC)));
jmar11 0:266a770a17e9 98 for(int i=0; i<floor(fabs(rightStepsPC)+rightError); i++)
jmar11 0:266a770a17e9 99 {
jmar11 0:266a770a17e9 100 stepRight(rightStepsPC>0);
jmar11 0:266a770a17e9 101 }
jmar11 0:266a770a17e9 102 if(signbit(rightSteps)!=signbit(rightSteps-floor(fabs(rightStepsPC)+leftError)))
jmar11 0:266a770a17e9 103 {
jmar11 0:266a770a17e9 104 rightSteps=0;
jmar11 0:266a770a17e9 105 }
jmar11 0:266a770a17e9 106 if(rightError>=1)
jmar11 0:266a770a17e9 107 {
jmar11 0:266a770a17e9 108 rightError-=1;
jmar11 0:266a770a17e9 109 }
jmar11 0:266a770a17e9 110 }
jmar11 0:266a770a17e9 111 if(leftSteps==0 && rightSteps==0)
jmar11 0:266a770a17e9 112 {
jmar11 0:266a770a17e9 113 moveComplete=true;
jmar11 0:266a770a17e9 114 }
jmar11 0:266a770a17e9 115 }
jmar11 0:266a770a17e9 116 }
jmar11 0:266a770a17e9 117
jmar11 0:266a770a17e9 118 void StepperDrive::stepLeft(bool dir)
jmar11 0:266a770a17e9 119 {
jmar11 0:266a770a17e9 120 leftDir=(invertLeft^dir);
jmar11 0:266a770a17e9 121 leftStep=1;
jmar11 0:266a770a17e9 122 wait_us(3);
jmar11 0:266a770a17e9 123 leftStep=0;
jmar11 0:266a770a17e9 124 }
jmar11 0:266a770a17e9 125
jmar11 0:266a770a17e9 126 void StepperDrive::stepRight(bool dir)
jmar11 0:266a770a17e9 127 {
jmar11 0:266a770a17e9 128 rightDir=(invertRight^dir);
jmar11 0:266a770a17e9 129 rightStep=1;
jmar11 0:266a770a17e9 130 wait_us(3);
jmar11 0:266a770a17e9 131 rightStep=0;
jmar11 0:266a770a17e9 132 }
jmar11 0:266a770a17e9 133
jmar11 0:266a770a17e9 134 bool StepperDrive::isMoveDone()
jmar11 0:266a770a17e9 135 {
jmar11 0:266a770a17e9 136 return moveComplete;
jmar11 0:266a770a17e9 137 }