Diff: threeAxis.cpp

Revision:

0:603f46a29b61

Child:

1:43d856fad23a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/threeAxis.cpp	Wed Dec 15 00:01:51 2021 +0000
@@ -0,0 +1,241 @@
+#include "mbed.h"
+#include "threeAxis.h"
+#include "rtos.h"
+
+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)
+{
+    _stepRatio = stepRatio;
+    _xMax = xBound;
+    _yMax = yBound;
+    _zMax = zBound;
+
+
+    _xEnable = 1;
+    _yEnable = 1;
+    _zEnable = 1;
+
+    wait = 5;
+
+    defaultXdir = 0;
+    defaultYdir = 0;
+    defaultZdir = 0;
+
+    _xDir = defaultXdir;
+    _yDir = defaultYdir;
+    _zDir = defaultZdir;
+
+    limitsEnabled = false;
+}
+
+void threeAxis::invertX(){
+    defaultXdir = !defaultXdir;
+    _xDir = defaultXdir;
+}
+
+void threeAxis::invertY(){
+    defaultYdir = !defaultYdir;
+    _yDir = defaultYdir;
+}
+
+void threeAxis::invertZ(){
+    defaultZdir = !defaultZdir;
+    _zDir = defaultZdir;
+}
+
+void threeAxis::setZeroX(){
+    toX = 0;
+    currX = 0;
+}
+
+void threeAxis::setZeroY(){
+    toY = 0;
+    currY = 0;
+}
+
+void threeAxis::setZeroZ(){
+    toZ = 0;
+    currZ = 0;
+}
+
+void threeAxis::setZero(){
+    this->setZeroX();
+    this->setZeroY();
+    this->setZeroZ();
+}
+
+void threeAxis::setWait(int waitPer){
+    wait = waitPer;
+}
+
+void threeAxis::setXdir(bool val){
+    defaultXdir = val;
+    _xDir = val;
+}
+
+void threeAxis::setYdir(bool val){
+    defaultYdir = val;
+    _yDir = val;
+}
+
+void threeAxis::setZdir(bool val){
+    defaultZdir = val;
+    _zDir = val;
+}
+
+
+void threeAxis::zeroX(){
+    _xEnable = 0;
+    _xDir = !defaultXdir;
+    
+    while(!_xLimit){
+        _xStep = 1;
+        Thread::wait(wait);
+        _xStep = 0;
+        Thread::wait(wait);
+    }
+    
+    this->setZeroX();
+    _xEnable = 1;
+
+}
+
+
+
+void threeAxis::zeroY(){
+    _yEnable = 0;
+    _yDir = !defaultYdir;
+    
+    while(!_yLimit){
+        _yStep = 1;
+        Thread::wait(wait);
+        _yStep = 0;
+        Thread::wait(wait);
+    }
+    
+    this->setZeroY();
+    _yEnable = 1;
+
+}
+
+void threeAxis::zeroZ(){
+    _zEnable = 0;
+    _zDir = !defaultZdir;
+    
+    while(!_zLimit){
+        _zStep = 1;
+        Thread::wait(wait);
+        _zStep = 0;
+        Thread::wait(wait);
+    }
+    
+    this->setZeroX();
+    _zEnable = 1;
+
+}
+
+void threeAxis::setLimits(bool val){
+    limitsEnabled = val;
+}
+
+int 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);
+}
+
+int threeAxis::goToRaw(int xVal, int yVal, int zVal){
+    toX = xVal;
+    toY = yVal;
+    toZ = zVal;
+    
+    
+    if (toX > _xMax * _stepRatio){
+        toX = _xMax * _stepRatio;
+    }
+    
+    if (toY > _yMax * _stepRatio){
+        toY = _yMax * _stepRatio;
+    }
+    
+    if (toZ > _zMax * _stepRatio){
+        toZ = _zMax * _stepRatio;
+    }
+    
+    int xStepVal = 1;
+    int yStepVal = 1;
+    int zStepVal = 1;
+    
+    _xDir = defaultXdir;
+    _yDir = defaultYdir;
+    _zDir = defaultZdir;
+    
+    int x = toX - currX;
+    int y = toY - currY;
+    int z = toZ - currZ;
+        
+        
+        
+    if (x != 0 || y != 0 || z != 0){
+         if (x != 0){
+             _xEnable = 0;
+             if (x < 0){
+                 _xDir = !defaultXdir;
+                 x = x* -1;
+                 xStepVal = xStepVal * -1;
+             }
+         }
+            
+         if (y != 0){
+             _yEnable = 0;
+             if (y < 0){
+                 _yDir = !defaultYdir;
+                 y = y* -1;
+                 yStepVal = yStepVal * -1;
+             }
+         }
+            
+         if (z != 0){
+             _zEnable = 0;
+             if (z < 0){
+                 _zDir = !defaultZdir;
+                 z = z* -1;
+                 zStepVal = zStepVal * -1;
+             }
+         }
+            
+         while (x > 0 || y > 0 || z > 0){
+             if (x>0){
+                 _xStep = 1;
+                 x=x-1;
+                 currX += xStepVal;
+             }
+             if (y>0){
+                 _yStep = 1;
+                 y=y-1;
+                 currY += yStepVal;
+             }
+             if (z>0){
+                 _zStep = 1;
+                 z=z-1;
+                 currZ += zStepVal;
+             }
+             Thread::wait(wait);
+             _xStep = 0;
+             _yStep = 0;
+             _zStep = 0;
+             Thread::wait(wait);
+         }
+     }
+
+     _xEnable = 1;
+     _yEnable = 1;
+     _zEnable = 1;
+     _xDir = defaultXdir;
+     _yDir = defaultYdir;
+     _zDir = defaultZdir;
+     
+     return 0;
+}