Justin Jordan / Mbed 2 deprecated R5_StepperDrive

update 1/27/16

Dependencies:   mbed

Fork of R5_StepperDrive by Jaime Martinez

StepperDrive.cpp@2:80c0b2a5adc0, 2016-02-17 (annotated)

Committer:
j_j205
Date:
Wed Feb 17 16:47:29 2016 +0000
Revision:
2:80c0b2a5adc0
Parent:
1:909572175aad
Child:
3:97bea13f40a9
2/17/16 JJ

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 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 1:909572175aad 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 }
jmar11 0:266a770a17e9 22
jmar11 0:266a770a17e9 23 float stepDistance=wheelCircum/(200*Ustep);
jmar11 0:266a770a17e9 24 float d, dl, dr;
jmar11 0:266a770a17e9 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);
jmar11 0:266a770a17e9 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);
jmar11 0:266a770a17e9 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 2:80c0b2a5adc0 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 2:80c0b2a5adc0 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 2:80c0b2a5adc0 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 2:80c0b2a5adc0 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 }