Diff: StepperDrive.cpp

Revision:

2:80c0b2a5adc0

Parent:

1:909572175aad

Child:

3:97bea13f40a9

diff -r 909572175aad -r 80c0b2a5adc0 StepperDrive.cpp
--- a/StepperDrive.cpp	Wed Jan 27 16:40:02 2016 +0000
+++ b/StepperDrive.cpp	Wed Feb 17 16:47:29 2016 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "StepperDrive.h"
+#include <cmath> // fabs, floor, signbit
 
 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)
 {
@@ -63,8 +64,6 @@
     }
     leftError=0;
     rightError=0;
-pc.printf("\n   Left Steps - %i", leftSteps);
-pc.printf("\n   Right Steps - %i", rightSteps);
     return 0;
 }
 
@@ -99,7 +98,7 @@
             {
                 stepRight(rightStepsPC>0);
             }
-            if(signbit(rightSteps)!=signbit(rightSteps-floor(fabs(rightStepsPC)+leftError)))
+            if(signbit(rightSteps)!=signbit(rightSteps-floor(fabs(rightStepsPC)+rightError)))
             {
                 rightSteps=0;
             }
@@ -120,7 +119,13 @@
     leftDir=(invertLeft^dir);
     leftStep=1;
     wait_us(3);
-    leftStep=0;  
+    leftStep=0;
+    
+    /* completed 1 step (increment or decrement */
+    if(leftSteps < 0)
+        leftSteps++;
+    else
+        leftSteps--; 
 }
 
 void StepperDrive::stepRight(bool dir)
@@ -128,7 +133,13 @@
     rightDir=(invertRight^dir);
     rightStep=1;
     wait_us(3);
-    rightStep=0;    
+    rightStep=0;
+    
+    /* completed 1 step (increment or decrement */
+    if(rightSteps < 0)
+        rightSteps++;
+    else
+        rightSteps--; 
 }
 
 bool StepperDrive::isMoveDone()