Diff: StepperDrive.cpp

Revision:

0:266a770a17e9

Child:

1:909572175aad

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StepperDrive.cpp	Sun Nov 22 19:27:47 2015 +0000
@@ -0,0 +1,134 @@
+#include "mbed.h"
+#include "StepperDrive.h"
+
+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)
+{
+    wheelCircum=in7;
+    wheelSepar=in8;
+    invertLeft=in3;
+    invertRight=in6;
+    pit.attach(this, &StepperDrive::pitCallback, in9);
+    moveComplete=true;
+}
+
+int StepperDrive::move(float distance, float angle)
+{
+    if(moveComplete==false) //if there is a move in progress
+    {                       //return
+        return -1;
+    }
+    
+    float stepDistance=wheelCircum/(200*Ustep);
+    float d, dl, dr;
+    
+    if(distance==0 && angle!=0)
+    {
+        d=angle*wheelSepar/2;
+        leftSteps=d/stepDistance;
+        rightSteps=-1*leftSteps;
+        leftStepsPC=(angle>0 ? -1:1);
+        rightStepsPC=-1*leftStepsPC;
+        moveComplete=false;
+    }
+    else if(angle==0 && distance!=0)
+    {
+        leftSteps=distance/stepDistance;
+        rightSteps=leftSteps;
+        leftStepsPC=-1;
+        rightStepsPC=-1;
+        moveComplete=false;
+    }
+    else if(angle>0 && distance!=0)
+    {
+        dl=angle*(distance/angle+wheelSepar/2);
+        dr=angle*(distance/angle-wheelSepar/2);
+        
+        leftSteps=dl/stepDistance;
+        rightSteps=dr/stepDistance;
+        leftStepsPC=-1;
+        rightStepsPC=-1*(dr/dl);
+        moveComplete=false;
+    }
+    else if(angle<0 && distance!=0)
+    {
+        dl=angle*(distance/angle-wheelSepar/2);
+        dr=angle*(distance/angle+wheelSepar/2);
+        
+        leftSteps=dl/stepDistance;
+        rightSteps=dr/stepDistance;
+        leftStepsPC=-1*(dl/dr);
+        rightStepsPC=-1;
+        moveComplete=false;
+    }
+    leftError=0;
+    rightError=0;
+    return 0;
+}
+
+void StepperDrive::pitCallback()
+{
+    if(moveComplete==true)
+    {
+        return;
+    }
+    else
+    {
+        if(leftSteps!=0)
+        {
+            leftError+=(fabs(leftStepsPC)-floor(fabs(leftStepsPC)));
+            for(int i=0; i<floor(fabs(leftStepsPC)+leftError); i++)
+            {
+                stepLeft(leftStepsPC>0);
+            }
+            if(signbit(leftSteps)!=signbit(leftSteps-floor(fabs(leftStepsPC)+leftError)))
+            {
+                leftSteps=0;
+            }
+            if(leftError>=1)
+            {
+                leftError-=1;
+            }
+        }
+        if(rightSteps!=0)
+        {
+            rightError+=(fabs(rightStepsPC)-floor(fabs(rightStepsPC)));
+            for(int i=0; i<floor(fabs(rightStepsPC)+rightError); i++)
+            {
+                stepRight(rightStepsPC>0);
+            }
+            if(signbit(rightSteps)!=signbit(rightSteps-floor(fabs(rightStepsPC)+leftError)))
+            {
+                rightSteps=0;
+            }
+            if(rightError>=1)
+            {
+                rightError-=1;
+            }
+        }
+        if(leftSteps==0 && rightSteps==0)
+        {
+            moveComplete=true;
+        }
+    }
+}
+
+void StepperDrive::stepLeft(bool dir)
+{
+    leftDir=(invertLeft^dir);
+    leftStep=1;
+    wait_us(3);
+    leftStep=0;  
+}
+
+void StepperDrive::stepRight(bool dir)
+{
+    rightDir=(invertRight^dir);
+    rightStep=1;
+    wait_us(3);
+    rightStep=0;    
+}
+
+bool StepperDrive::isMoveDone()
+{
+    return moveComplete;
+}
\ No newline at end of file