Alan Simms / threeAxis

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

threeAxis.cpp@1:43d856fad23a, 2021-12-15 (annotated)

Committer:
magiwarriorx
Date:
Wed Dec 15 01:17:55 2021 +0000
Revision:
1:43d856fad23a
Parent:
0:603f46a29b61
Child:
2:5206484275e3
fixed stepRatio error;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
magiwarriorx 0:603f46a29b61 1 #include "mbed.h"
magiwarriorx 0:603f46a29b61 2 #include "threeAxis.h"
magiwarriorx 0:603f46a29b61 3 #include "rtos.h"
magiwarriorx 1:43d856fad23a 4 #include "uLCD_4DGL.h"
magiwarriorx 0:603f46a29b61 5
magiwarriorx 1:43d856fad23a 6 extern uLCD_4DGL uLCD;
magiwarriorx 0:603f46a29b61 7 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)
magiwarriorx 0:603f46a29b61 8 : _xStep(xStep), _yStep(yStep), _zStep(zStep), _xDir(xDir), _yDir(yDir), _zDir(zDir), _xEnable(xEnable), _yEnable(yEnable), _zEnable(zEnable), _xLimit(xLimit), _yLimit(yLimit), _zLimit(zLimit)
magiwarriorx 0:603f46a29b61 9 {
magiwarriorx 0:603f46a29b61 10 _stepRatio = stepRatio;
magiwarriorx 0:603f46a29b61 11 _xMax = xBound;
magiwarriorx 0:603f46a29b61 12 _yMax = yBound;
magiwarriorx 0:603f46a29b61 13 _zMax = zBound;
magiwarriorx 0:603f46a29b61 14
magiwarriorx 0:603f46a29b61 15
magiwarriorx 0:603f46a29b61 16 _xEnable = 1;
magiwarriorx 0:603f46a29b61 17 _yEnable = 1;
magiwarriorx 0:603f46a29b61 18 _zEnable = 1;
magiwarriorx 0:603f46a29b61 19
magiwarriorx 0:603f46a29b61 20 wait = 5;
magiwarriorx 0:603f46a29b61 21
magiwarriorx 0:603f46a29b61 22 defaultXdir = 0;
magiwarriorx 0:603f46a29b61 23 defaultYdir = 0;
magiwarriorx 0:603f46a29b61 24 defaultZdir = 0;
magiwarriorx 0:603f46a29b61 25
magiwarriorx 0:603f46a29b61 26 _xDir = defaultXdir;
magiwarriorx 0:603f46a29b61 27 _yDir = defaultYdir;
magiwarriorx 0:603f46a29b61 28 _zDir = defaultZdir;
magiwarriorx 0:603f46a29b61 29
magiwarriorx 0:603f46a29b61 30 limitsEnabled = false;
magiwarriorx 0:603f46a29b61 31 }
magiwarriorx 0:603f46a29b61 32
magiwarriorx 0:603f46a29b61 33 void threeAxis::invertX(){
magiwarriorx 0:603f46a29b61 34 defaultXdir = !defaultXdir;
magiwarriorx 0:603f46a29b61 35 _xDir = defaultXdir;
magiwarriorx 0:603f46a29b61 36 }
magiwarriorx 0:603f46a29b61 37
magiwarriorx 0:603f46a29b61 38 void threeAxis::invertY(){
magiwarriorx 0:603f46a29b61 39 defaultYdir = !defaultYdir;
magiwarriorx 0:603f46a29b61 40 _yDir = defaultYdir;
magiwarriorx 0:603f46a29b61 41 }
magiwarriorx 0:603f46a29b61 42
magiwarriorx 0:603f46a29b61 43 void threeAxis::invertZ(){
magiwarriorx 0:603f46a29b61 44 defaultZdir = !defaultZdir;
magiwarriorx 0:603f46a29b61 45 _zDir = defaultZdir;
magiwarriorx 0:603f46a29b61 46 }
magiwarriorx 0:603f46a29b61 47
magiwarriorx 0:603f46a29b61 48 void threeAxis::setZeroX(){
magiwarriorx 0:603f46a29b61 49 toX = 0;
magiwarriorx 0:603f46a29b61 50 currX = 0;
magiwarriorx 0:603f46a29b61 51 }
magiwarriorx 0:603f46a29b61 52
magiwarriorx 0:603f46a29b61 53 void threeAxis::setZeroY(){
magiwarriorx 0:603f46a29b61 54 toY = 0;
magiwarriorx 0:603f46a29b61 55 currY = 0;
magiwarriorx 0:603f46a29b61 56 }
magiwarriorx 0:603f46a29b61 57
magiwarriorx 0:603f46a29b61 58 void threeAxis::setZeroZ(){
magiwarriorx 0:603f46a29b61 59 toZ = 0;
magiwarriorx 0:603f46a29b61 60 currZ = 0;
magiwarriorx 0:603f46a29b61 61 }
magiwarriorx 0:603f46a29b61 62
magiwarriorx 0:603f46a29b61 63 void threeAxis::setZero(){
magiwarriorx 0:603f46a29b61 64 this->setZeroX();
magiwarriorx 0:603f46a29b61 65 this->setZeroY();
magiwarriorx 0:603f46a29b61 66 this->setZeroZ();
magiwarriorx 0:603f46a29b61 67 }
magiwarriorx 0:603f46a29b61 68
magiwarriorx 0:603f46a29b61 69 void threeAxis::setWait(int waitPer){
magiwarriorx 0:603f46a29b61 70 wait = waitPer;
magiwarriorx 0:603f46a29b61 71 }
magiwarriorx 0:603f46a29b61 72
magiwarriorx 0:603f46a29b61 73 void threeAxis::setXdir(bool val){
magiwarriorx 0:603f46a29b61 74 defaultXdir = val;
magiwarriorx 0:603f46a29b61 75 _xDir = val;
magiwarriorx 0:603f46a29b61 76 }
magiwarriorx 0:603f46a29b61 77
magiwarriorx 0:603f46a29b61 78 void threeAxis::setYdir(bool val){
magiwarriorx 0:603f46a29b61 79 defaultYdir = val;
magiwarriorx 0:603f46a29b61 80 _yDir = val;
magiwarriorx 0:603f46a29b61 81 }
magiwarriorx 0:603f46a29b61 82
magiwarriorx 0:603f46a29b61 83 void threeAxis::setZdir(bool val){
magiwarriorx 0:603f46a29b61 84 defaultZdir = val;
magiwarriorx 0:603f46a29b61 85 _zDir = val;
magiwarriorx 0:603f46a29b61 86 }
magiwarriorx 0:603f46a29b61 87
magiwarriorx 0:603f46a29b61 88
magiwarriorx 0:603f46a29b61 89 void threeAxis::zeroX(){
magiwarriorx 0:603f46a29b61 90 _xEnable = 0;
magiwarriorx 0:603f46a29b61 91 _xDir = !defaultXdir;
magiwarriorx 0:603f46a29b61 92
magiwarriorx 0:603f46a29b61 93 while(!_xLimit){
magiwarriorx 0:603f46a29b61 94 _xStep = 1;
magiwarriorx 0:603f46a29b61 95 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 96 _xStep = 0;
magiwarriorx 0:603f46a29b61 97 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 98 }
magiwarriorx 0:603f46a29b61 99
magiwarriorx 0:603f46a29b61 100 this->setZeroX();
magiwarriorx 0:603f46a29b61 101 _xEnable = 1;
magiwarriorx 0:603f46a29b61 102
magiwarriorx 0:603f46a29b61 103 }
magiwarriorx 0:603f46a29b61 104
magiwarriorx 0:603f46a29b61 105
magiwarriorx 0:603f46a29b61 106
magiwarriorx 0:603f46a29b61 107 void threeAxis::zeroY(){
magiwarriorx 0:603f46a29b61 108 _yEnable = 0;
magiwarriorx 0:603f46a29b61 109 _yDir = !defaultYdir;
magiwarriorx 0:603f46a29b61 110
magiwarriorx 0:603f46a29b61 111 while(!_yLimit){
magiwarriorx 0:603f46a29b61 112 _yStep = 1;
magiwarriorx 0:603f46a29b61 113 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 114 _yStep = 0;
magiwarriorx 0:603f46a29b61 115 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 116 }
magiwarriorx 0:603f46a29b61 117
magiwarriorx 0:603f46a29b61 118 this->setZeroY();
magiwarriorx 0:603f46a29b61 119 _yEnable = 1;
magiwarriorx 0:603f46a29b61 120
magiwarriorx 0:603f46a29b61 121 }
magiwarriorx 0:603f46a29b61 122
magiwarriorx 0:603f46a29b61 123 void threeAxis::zeroZ(){
magiwarriorx 0:603f46a29b61 124 _zEnable = 0;
magiwarriorx 0:603f46a29b61 125 _zDir = !defaultZdir;
magiwarriorx 0:603f46a29b61 126
magiwarriorx 0:603f46a29b61 127 while(!_zLimit){
magiwarriorx 0:603f46a29b61 128 _zStep = 1;
magiwarriorx 0:603f46a29b61 129 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 130 _zStep = 0;
magiwarriorx 0:603f46a29b61 131 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 132 }
magiwarriorx 0:603f46a29b61 133
magiwarriorx 0:603f46a29b61 134 this->setZeroX();
magiwarriorx 0:603f46a29b61 135 _zEnable = 1;
magiwarriorx 0:603f46a29b61 136
magiwarriorx 0:603f46a29b61 137 }
magiwarriorx 0:603f46a29b61 138
magiwarriorx 0:603f46a29b61 139 void threeAxis::setLimits(bool val){
magiwarriorx 0:603f46a29b61 140 limitsEnabled = val;
magiwarriorx 0:603f46a29b61 141 }
magiwarriorx 0:603f46a29b61 142
magiwarriorx 1:43d856fad23a 143 /*void threeAxis::goTo(float xVal, float yVal, float zVal){
magiwarriorx 0:603f46a29b61 144 int xSteps = (int)xVal*_stepRatio;
magiwarriorx 0:603f46a29b61 145 int ySteps = (int)yVal*_stepRatio;
magiwarriorx 0:603f46a29b61 146 int zSteps = (int)zVal*_stepRatio;
magiwarriorx 0:603f46a29b61 147
magiwarriorx 1:43d856fad23a 148 this->goToRaw(xSteps, ySteps, zSteps);
magiwarriorx 1:43d856fad23a 149 }*/
magiwarriorx 0:603f46a29b61 150
magiwarriorx 1:43d856fad23a 151 void threeAxis::goTo(float xVal, float yVal, float zVal){
magiwarriorx 1:43d856fad23a 152
magiwarriorx 1:43d856fad23a 153 int xSteps = (int)xVal/_stepRatio;
magiwarriorx 1:43d856fad23a 154 int ySteps = (int)yVal/_stepRatio;
magiwarriorx 1:43d856fad23a 155 int zSteps = (int)zVal/_stepRatio;
magiwarriorx 1:43d856fad23a 156
magiwarriorx 1:43d856fad23a 157 //uLCD.printf("\n%.2f, %.2f, %.2f, %d, %d, %d", xVal, yVal, zVal, xSteps, ySteps, zSteps);
magiwarriorx 1:43d856fad23a 158
magiwarriorx 1:43d856fad23a 159 toX = xSteps;
magiwarriorx 1:43d856fad23a 160 toY = ySteps;
magiwarriorx 1:43d856fad23a 161 toZ = zSteps;
magiwarriorx 0:603f46a29b61 162
magiwarriorx 0:603f46a29b61 163
magiwarriorx 1:43d856fad23a 164
magiwarriorx 1:43d856fad23a 165 if (toX > _xMax / _stepRatio){
magiwarriorx 1:43d856fad23a 166 toX = _xMax / _stepRatio;
magiwarriorx 0:603f46a29b61 167 }
magiwarriorx 0:603f46a29b61 168
magiwarriorx 1:43d856fad23a 169 if (toY > _yMax / _stepRatio){
magiwarriorx 1:43d856fad23a 170 toY = _yMax / _stepRatio;
magiwarriorx 0:603f46a29b61 171 }
magiwarriorx 0:603f46a29b61 172
magiwarriorx 1:43d856fad23a 173 if (toZ > _zMax / _stepRatio){
magiwarriorx 1:43d856fad23a 174 toZ = _zMax / _stepRatio;
magiwarriorx 0:603f46a29b61 175 }
magiwarriorx 0:603f46a29b61 176
magiwarriorx 1:43d856fad23a 177 //uLCD.printf("\nTo: %d, %d, %d", toX, toY, toZ);
magiwarriorx 1:43d856fad23a 178 //uLCD.printf("\nCur: %d, %d, %d", currX, currY, currZ);
magiwarriorx 0:603f46a29b61 179 int xStepVal = 1;
magiwarriorx 0:603f46a29b61 180 int yStepVal = 1;
magiwarriorx 0:603f46a29b61 181 int zStepVal = 1;
magiwarriorx 0:603f46a29b61 182
magiwarriorx 0:603f46a29b61 183 _xDir = defaultXdir;
magiwarriorx 0:603f46a29b61 184 _yDir = defaultYdir;
magiwarriorx 0:603f46a29b61 185 _zDir = defaultZdir;
magiwarriorx 0:603f46a29b61 186
magiwarriorx 0:603f46a29b61 187 int x = toX - currX;
magiwarriorx 0:603f46a29b61 188 int y = toY - currY;
magiwarriorx 0:603f46a29b61 189 int z = toZ - currZ;
magiwarriorx 1:43d856fad23a 190
magiwarriorx 1:43d856fad23a 191 //uLCD.printf("\n%d,%d,%d", x,y,z);
magiwarriorx 0:603f46a29b61 192
magiwarriorx 0:603f46a29b61 193
magiwarriorx 0:603f46a29b61 194
magiwarriorx 0:603f46a29b61 195 if (x != 0 || y != 0 || z != 0){
magiwarriorx 0:603f46a29b61 196 if (x != 0){
magiwarriorx 0:603f46a29b61 197 _xEnable = 0;
magiwarriorx 0:603f46a29b61 198 if (x < 0){
magiwarriorx 0:603f46a29b61 199 _xDir = !defaultXdir;
magiwarriorx 0:603f46a29b61 200 x = x* -1;
magiwarriorx 0:603f46a29b61 201 xStepVal = xStepVal * -1;
magiwarriorx 0:603f46a29b61 202 }
magiwarriorx 0:603f46a29b61 203 }
magiwarriorx 0:603f46a29b61 204
magiwarriorx 0:603f46a29b61 205 if (y != 0){
magiwarriorx 0:603f46a29b61 206 _yEnable = 0;
magiwarriorx 0:603f46a29b61 207 if (y < 0){
magiwarriorx 0:603f46a29b61 208 _yDir = !defaultYdir;
magiwarriorx 0:603f46a29b61 209 y = y* -1;
magiwarriorx 0:603f46a29b61 210 yStepVal = yStepVal * -1;
magiwarriorx 0:603f46a29b61 211 }
magiwarriorx 0:603f46a29b61 212 }
magiwarriorx 0:603f46a29b61 213
magiwarriorx 0:603f46a29b61 214 if (z != 0){
magiwarriorx 0:603f46a29b61 215 _zEnable = 0;
magiwarriorx 0:603f46a29b61 216 if (z < 0){
magiwarriorx 0:603f46a29b61 217 _zDir = !defaultZdir;
magiwarriorx 0:603f46a29b61 218 z = z* -1;
magiwarriorx 0:603f46a29b61 219 zStepVal = zStepVal * -1;
magiwarriorx 0:603f46a29b61 220 }
magiwarriorx 0:603f46a29b61 221 }
magiwarriorx 0:603f46a29b61 222
magiwarriorx 0:603f46a29b61 223 while (x > 0 || y > 0 || z > 0){
magiwarriorx 0:603f46a29b61 224 if (x>0){
magiwarriorx 0:603f46a29b61 225 _xStep = 1;
magiwarriorx 0:603f46a29b61 226 x=x-1;
magiwarriorx 0:603f46a29b61 227 currX += xStepVal;
magiwarriorx 0:603f46a29b61 228 }
magiwarriorx 0:603f46a29b61 229 if (y>0){
magiwarriorx 0:603f46a29b61 230 _yStep = 1;
magiwarriorx 0:603f46a29b61 231 y=y-1;
magiwarriorx 0:603f46a29b61 232 currY += yStepVal;
magiwarriorx 0:603f46a29b61 233 }
magiwarriorx 0:603f46a29b61 234 if (z>0){
magiwarriorx 0:603f46a29b61 235 _zStep = 1;
magiwarriorx 0:603f46a29b61 236 z=z-1;
magiwarriorx 0:603f46a29b61 237 currZ += zStepVal;
magiwarriorx 0:603f46a29b61 238 }
magiwarriorx 0:603f46a29b61 239 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 240 _xStep = 0;
magiwarriorx 0:603f46a29b61 241 _yStep = 0;
magiwarriorx 0:603f46a29b61 242 _zStep = 0;
magiwarriorx 0:603f46a29b61 243 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 244 }
magiwarriorx 0:603f46a29b61 245 }
magiwarriorx 0:603f46a29b61 246
magiwarriorx 0:603f46a29b61 247 _xEnable = 1;
magiwarriorx 0:603f46a29b61 248 _yEnable = 1;
magiwarriorx 0:603f46a29b61 249 _zEnable = 1;
magiwarriorx 0:603f46a29b61 250 _xDir = defaultXdir;
magiwarriorx 0:603f46a29b61 251 _yDir = defaultYdir;
magiwarriorx 0:603f46a29b61 252 _zDir = defaultZdir;
magiwarriorx 0:603f46a29b61 253
magiwarriorx 0:603f46a29b61 254 }