Diff: threeAxis.cpp

Revision:

1:43d856fad23a

Parent:

0:603f46a29b61

Child:

2:5206484275e3

diff -r 603f46a29b61 -r 43d856fad23a threeAxis.cpp
--- a/threeAxis.cpp	Wed Dec 15 00:01:51 2021 +0000
+++ b/threeAxis.cpp	Wed Dec 15 01:17:55 2021 +0000
@@ -1,7 +1,9 @@
 #include "mbed.h"
 #include "threeAxis.h"
 #include "rtos.h"
+#include "uLCD_4DGL.h"
 
+extern uLCD_4DGL uLCD;
 threeAxis::threeAxis(PinName xStep, PinName yStep, PinName zStep, PinName xDir, PinName yDir, PinName zDir, PinName xEnable, PinName yEnable, PinName zEnable, float stepRatio, float xBound, float yBound, float zBound, PinName xLimit, PinName yLimit, PinName zLimit)
 : _xStep(xStep), _yStep(yStep), _zStep(zStep), _xDir(xDir), _yDir(yDir), _zDir(zDir), _xEnable(xEnable), _yEnable(yEnable), _zEnable(zEnable), _xLimit(xLimit), _yLimit(yLimit), _zLimit(zLimit)
 {
@@ -138,32 +140,42 @@
     limitsEnabled = val;
 }
 
-int threeAxis::goTo(float xVal, float yVal, float zVal){
+/*void threeAxis::goTo(float xVal, float yVal, float zVal){
     int xSteps = (int)xVal*_stepRatio;
     int ySteps = (int)yVal*_stepRatio;
     int zSteps = (int)zVal*_stepRatio;
     
-    return this->goToRaw(xSteps, ySteps, zSteps);
-}
+    this->goToRaw(xSteps, ySteps, zSteps);
+}*/
 
-int threeAxis::goToRaw(int xVal, int yVal, int zVal){
-    toX = xVal;
-    toY = yVal;
-    toZ = zVal;
+void threeAxis::goTo(float xVal, float yVal, float zVal){
+    
+    int xSteps = (int)xVal/_stepRatio;
+    int ySteps = (int)yVal/_stepRatio;
+    int zSteps = (int)zVal/_stepRatio;
+    
+    //uLCD.printf("\n%.2f, %.2f, %.2f, %d, %d, %d", xVal, yVal, zVal, xSteps, ySteps, zSteps);
+    
+    toX = xSteps;
+    toY = ySteps;
+    toZ = zSteps;
     
     
-    if (toX > _xMax * _stepRatio){
-        toX = _xMax * _stepRatio;
+    
+    if (toX > _xMax / _stepRatio){
+        toX = _xMax / _stepRatio;
     }
     
-    if (toY > _yMax * _stepRatio){
-        toY = _yMax * _stepRatio;
+    if (toY > _yMax / _stepRatio){
+        toY = _yMax / _stepRatio;
     }
     
-    if (toZ > _zMax * _stepRatio){
-        toZ = _zMax * _stepRatio;
+    if (toZ > _zMax / _stepRatio){
+        toZ = _zMax / _stepRatio;
     }
     
+    //uLCD.printf("\nTo: %d, %d, %d", toX, toY, toZ);
+    //uLCD.printf("\nCur: %d, %d, %d", currX, currY, currZ);
     int xStepVal = 1;
     int yStepVal = 1;
     int zStepVal = 1;
@@ -175,6 +187,8 @@
     int x = toX - currX;
     int y = toY - currY;
     int z = toZ - currZ;
+    
+    //uLCD.printf("\n%d,%d,%d", x,y,z);
         
         
         
@@ -237,5 +251,4 @@
      _yDir = defaultYdir;
      _zDir = defaultZdir;
      
-     return 0;
 }