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@3:09abcf461dc2, 2021-12-16 (annotated)

Committer:
magiwarriorx
Date:
Thu Dec 16 10:51:37 2021 +0000
Revision:
3:09abcf461dc2
Parent:
2:5206484275e3 
Initial public commit;

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 2:5206484275e3 31 bufferIndex = 0;
magiwarriorx 2:5206484275e3 32 }
magiwarriorx 2:5206484275e3 33
magiwarriorx 2:5206484275e3 34 void threeAxis::addToBuffer(float xVal, float yVal, float zVal){
magiwarriorx 2:5206484275e3 35
magiwarriorx 2:5206484275e3 36 buffer[bufferIndex][0] = xVal;
magiwarriorx 2:5206484275e3 37 buffer[bufferIndex][1] = yVal;
magiwarriorx 2:5206484275e3 38 buffer[bufferIndex][2] = zVal;
magiwarriorx 2:5206484275e3 39
magiwarriorx 2:5206484275e3 40 bufferIndex = bufferIndex + 1;
magiwarriorx 2:5206484275e3 41 }
magiwarriorx 2:5206484275e3 42
magiwarriorx 2:5206484275e3 43 void threeAxis::runBuffer(){
magiwarriorx 2:5206484275e3 44 int i = 0;
magiwarriorx 3:09abcf461dc2 45 while(i < bufferIndex){
magiwarriorx 2:5206484275e3 46 goTo(buffer[i][0], buffer[i][1], buffer[i][2]);
magiwarriorx 2:5206484275e3 47 i = i+1;
magiwarriorx 2:5206484275e3 48 }
magiwarriorx 2:5206484275e3 49
magiwarriorx 2:5206484275e3 50 bufferIndex = 0;
magiwarriorx 0:603f46a29b61 51 }
magiwarriorx 0:603f46a29b61 52
magiwarriorx 0:603f46a29b61 53 void threeAxis::invertX(){
magiwarriorx 0:603f46a29b61 54 defaultXdir = !defaultXdir;
magiwarriorx 0:603f46a29b61 55 _xDir = defaultXdir;
magiwarriorx 0:603f46a29b61 56 }
magiwarriorx 0:603f46a29b61 57
magiwarriorx 0:603f46a29b61 58 void threeAxis::invertY(){
magiwarriorx 0:603f46a29b61 59 defaultYdir = !defaultYdir;
magiwarriorx 0:603f46a29b61 60 _yDir = defaultYdir;
magiwarriorx 0:603f46a29b61 61 }
magiwarriorx 0:603f46a29b61 62
magiwarriorx 0:603f46a29b61 63 void threeAxis::invertZ(){
magiwarriorx 0:603f46a29b61 64 defaultZdir = !defaultZdir;
magiwarriorx 0:603f46a29b61 65 _zDir = defaultZdir;
magiwarriorx 0:603f46a29b61 66 }
magiwarriorx 0:603f46a29b61 67
magiwarriorx 0:603f46a29b61 68 void threeAxis::setZeroX(){
magiwarriorx 0:603f46a29b61 69 toX = 0;
magiwarriorx 0:603f46a29b61 70 currX = 0;
magiwarriorx 0:603f46a29b61 71 }
magiwarriorx 0:603f46a29b61 72
magiwarriorx 0:603f46a29b61 73 void threeAxis::setZeroY(){
magiwarriorx 0:603f46a29b61 74 toY = 0;
magiwarriorx 0:603f46a29b61 75 currY = 0;
magiwarriorx 0:603f46a29b61 76 }
magiwarriorx 0:603f46a29b61 77
magiwarriorx 0:603f46a29b61 78 void threeAxis::setZeroZ(){
magiwarriorx 0:603f46a29b61 79 toZ = 0;
magiwarriorx 0:603f46a29b61 80 currZ = 0;
magiwarriorx 0:603f46a29b61 81 }
magiwarriorx 0:603f46a29b61 82
magiwarriorx 0:603f46a29b61 83 void threeAxis::setZero(){
magiwarriorx 0:603f46a29b61 84 this->setZeroX();
magiwarriorx 0:603f46a29b61 85 this->setZeroY();
magiwarriorx 0:603f46a29b61 86 this->setZeroZ();
magiwarriorx 0:603f46a29b61 87 }
magiwarriorx 0:603f46a29b61 88
magiwarriorx 0:603f46a29b61 89 void threeAxis::setWait(int waitPer){
magiwarriorx 0:603f46a29b61 90 wait = waitPer;
magiwarriorx 0:603f46a29b61 91 }
magiwarriorx 0:603f46a29b61 92
magiwarriorx 0:603f46a29b61 93 void threeAxis::setXdir(bool val){
magiwarriorx 0:603f46a29b61 94 defaultXdir = val;
magiwarriorx 0:603f46a29b61 95 _xDir = val;
magiwarriorx 0:603f46a29b61 96 }
magiwarriorx 0:603f46a29b61 97
magiwarriorx 0:603f46a29b61 98 void threeAxis::setYdir(bool val){
magiwarriorx 0:603f46a29b61 99 defaultYdir = val;
magiwarriorx 0:603f46a29b61 100 _yDir = val;
magiwarriorx 0:603f46a29b61 101 }
magiwarriorx 0:603f46a29b61 102
magiwarriorx 0:603f46a29b61 103 void threeAxis::setZdir(bool val){
magiwarriorx 0:603f46a29b61 104 defaultZdir = val;
magiwarriorx 0:603f46a29b61 105 _zDir = val;
magiwarriorx 0:603f46a29b61 106 }
magiwarriorx 0:603f46a29b61 107
magiwarriorx 0:603f46a29b61 108
magiwarriorx 0:603f46a29b61 109 void threeAxis::zeroX(){
magiwarriorx 0:603f46a29b61 110 _xEnable = 0;
magiwarriorx 0:603f46a29b61 111 _xDir = !defaultXdir;
magiwarriorx 0:603f46a29b61 112
magiwarriorx 0:603f46a29b61 113 while(!_xLimit){
magiwarriorx 0:603f46a29b61 114 _xStep = 1;
magiwarriorx 0:603f46a29b61 115 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 116 _xStep = 0;
magiwarriorx 0:603f46a29b61 117 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 118 }
magiwarriorx 0:603f46a29b61 119
magiwarriorx 0:603f46a29b61 120 this->setZeroX();
magiwarriorx 0:603f46a29b61 121 _xEnable = 1;
magiwarriorx 0:603f46a29b61 122
magiwarriorx 0:603f46a29b61 123 }
magiwarriorx 0:603f46a29b61 124
magiwarriorx 0:603f46a29b61 125
magiwarriorx 0:603f46a29b61 126
magiwarriorx 0:603f46a29b61 127 void threeAxis::zeroY(){
magiwarriorx 0:603f46a29b61 128 _yEnable = 0;
magiwarriorx 0:603f46a29b61 129 _yDir = !defaultYdir;
magiwarriorx 0:603f46a29b61 130
magiwarriorx 0:603f46a29b61 131 while(!_yLimit){
magiwarriorx 0:603f46a29b61 132 _yStep = 1;
magiwarriorx 0:603f46a29b61 133 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 134 _yStep = 0;
magiwarriorx 0:603f46a29b61 135 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 136 }
magiwarriorx 0:603f46a29b61 137
magiwarriorx 0:603f46a29b61 138 this->setZeroY();
magiwarriorx 0:603f46a29b61 139 _yEnable = 1;
magiwarriorx 0:603f46a29b61 140
magiwarriorx 0:603f46a29b61 141 }
magiwarriorx 0:603f46a29b61 142
magiwarriorx 0:603f46a29b61 143 void threeAxis::zeroZ(){
magiwarriorx 0:603f46a29b61 144 _zEnable = 0;
magiwarriorx 0:603f46a29b61 145 _zDir = !defaultZdir;
magiwarriorx 0:603f46a29b61 146
magiwarriorx 0:603f46a29b61 147 while(!_zLimit){
magiwarriorx 0:603f46a29b61 148 _zStep = 1;
magiwarriorx 0:603f46a29b61 149 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 150 _zStep = 0;
magiwarriorx 0:603f46a29b61 151 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 152 }
magiwarriorx 0:603f46a29b61 153
magiwarriorx 0:603f46a29b61 154 this->setZeroX();
magiwarriorx 0:603f46a29b61 155 _zEnable = 1;
magiwarriorx 0:603f46a29b61 156
magiwarriorx 0:603f46a29b61 157 }
magiwarriorx 0:603f46a29b61 158
magiwarriorx 0:603f46a29b61 159 void threeAxis::setLimits(bool val){
magiwarriorx 0:603f46a29b61 160 limitsEnabled = val;
magiwarriorx 0:603f46a29b61 161 }
magiwarriorx 0:603f46a29b61 162
magiwarriorx 3:09abcf461dc2 163 bool threeAxis::getLimitsEn(){
magiwarriorx 3:09abcf461dc2 164 return limitsEnabled;
magiwarriorx 3:09abcf461dc2 165 }
magiwarriorx 3:09abcf461dc2 166
magiwarriorx 1:43d856fad23a 167 /*void threeAxis::goTo(float xVal, float yVal, float zVal){
magiwarriorx 0:603f46a29b61 168 int xSteps = (int)xVal*_stepRatio;
magiwarriorx 0:603f46a29b61 169 int ySteps = (int)yVal*_stepRatio;
magiwarriorx 0:603f46a29b61 170 int zSteps = (int)zVal*_stepRatio;
magiwarriorx 0:603f46a29b61 171
magiwarriorx 1:43d856fad23a 172 this->goToRaw(xSteps, ySteps, zSteps);
magiwarriorx 1:43d856fad23a 173 }*/
magiwarriorx 0:603f46a29b61 174
magiwarriorx 1:43d856fad23a 175 void threeAxis::goTo(float xVal, float yVal, float zVal){
magiwarriorx 1:43d856fad23a 176
magiwarriorx 2:5206484275e3 177 int xSteps = (int)(xVal/_stepRatio);
magiwarriorx 2:5206484275e3 178 int ySteps = (int)(yVal/_stepRatio);
magiwarriorx 2:5206484275e3 179 int zSteps = (int)(zVal/_stepRatio);
magiwarriorx 1:43d856fad23a 180
magiwarriorx 1:43d856fad23a 181 //uLCD.printf("\n%.2f, %.2f, %.2f, %d, %d, %d", xVal, yVal, zVal, xSteps, ySteps, zSteps);
magiwarriorx 1:43d856fad23a 182
magiwarriorx 1:43d856fad23a 183 toX = xSteps;
magiwarriorx 1:43d856fad23a 184 toY = ySteps;
magiwarriorx 1:43d856fad23a 185 toZ = zSteps;
magiwarriorx 0:603f46a29b61 186
magiwarriorx 0:603f46a29b61 187
magiwarriorx 3:09abcf461dc2 188 if(limitsEnabled){
magiwarriorx 3:09abcf461dc2 189 if (toX > (int)(_xMax / _stepRatio)){
magiwarriorx 3:09abcf461dc2 190 toX = (int)(_xMax / _stepRatio);
magiwarriorx 3:09abcf461dc2 191 }
magiwarriorx 1:43d856fad23a 192
magiwarriorx 3:09abcf461dc2 193 if (toY > (int)(_yMax / _stepRatio)){
magiwarriorx 3:09abcf461dc2 194 toY = (int)(_yMax / _stepRatio);
magiwarriorx 3:09abcf461dc2 195 }
magiwarriorx 0:603f46a29b61 196
magiwarriorx 3:09abcf461dc2 197 if (toZ > (int)(_zMax / _stepRatio)){
magiwarriorx 3:09abcf461dc2 198 toZ = (int)(_zMax / _stepRatio);
magiwarriorx 3:09abcf461dc2 199 }
magiwarriorx 3:09abcf461dc2 200
magiwarriorx 3:09abcf461dc2 201 if (toX<0){
magiwarriorx 3:09abcf461dc2 202 toX = 0;
magiwarriorx 3:09abcf461dc2 203 }
magiwarriorx 3:09abcf461dc2 204
magiwarriorx 3:09abcf461dc2 205 if (toY<0){
magiwarriorx 3:09abcf461dc2 206 toY = 0;
magiwarriorx 3:09abcf461dc2 207 }
magiwarriorx 3:09abcf461dc2 208
magiwarriorx 3:09abcf461dc2 209 if (toZ<0){
magiwarriorx 3:09abcf461dc2 210 toZ = 0;
magiwarriorx 3:09abcf461dc2 211 }
magiwarriorx 3:09abcf461dc2 212 }
magiwarriorx 1:43d856fad23a 213 //uLCD.printf("\nTo: %d, %d, %d", toX, toY, toZ);
magiwarriorx 1:43d856fad23a 214 //uLCD.printf("\nCur: %d, %d, %d", currX, currY, currZ);
magiwarriorx 0:603f46a29b61 215 int xStepVal = 1;
magiwarriorx 0:603f46a29b61 216 int yStepVal = 1;
magiwarriorx 0:603f46a29b61 217 int zStepVal = 1;
magiwarriorx 0:603f46a29b61 218
magiwarriorx 0:603f46a29b61 219 _xDir = defaultXdir;
magiwarriorx 0:603f46a29b61 220 _yDir = defaultYdir;
magiwarriorx 0:603f46a29b61 221 _zDir = defaultZdir;
magiwarriorx 0:603f46a29b61 222
magiwarriorx 0:603f46a29b61 223 int x = toX - currX;
magiwarriorx 0:603f46a29b61 224 int y = toY - currY;
magiwarriorx 0:603f46a29b61 225 int z = toZ - currZ;
magiwarriorx 1:43d856fad23a 226
magiwarriorx 1:43d856fad23a 227 //uLCD.printf("\n%d,%d,%d", x,y,z);
magiwarriorx 0:603f46a29b61 228
magiwarriorx 0:603f46a29b61 229
magiwarriorx 0:603f46a29b61 230
magiwarriorx 0:603f46a29b61 231 if (x != 0 || y != 0 || z != 0){
magiwarriorx 0:603f46a29b61 232 if (x != 0){
magiwarriorx 0:603f46a29b61 233 _xEnable = 0;
magiwarriorx 0:603f46a29b61 234 if (x < 0){
magiwarriorx 0:603f46a29b61 235 _xDir = !defaultXdir;
magiwarriorx 0:603f46a29b61 236 x = x* -1;
magiwarriorx 0:603f46a29b61 237 xStepVal = xStepVal * -1;
magiwarriorx 0:603f46a29b61 238 }
magiwarriorx 0:603f46a29b61 239 }
magiwarriorx 0:603f46a29b61 240
magiwarriorx 0:603f46a29b61 241 if (y != 0){
magiwarriorx 0:603f46a29b61 242 _yEnable = 0;
magiwarriorx 0:603f46a29b61 243 if (y < 0){
magiwarriorx 0:603f46a29b61 244 _yDir = !defaultYdir;
magiwarriorx 0:603f46a29b61 245 y = y* -1;
magiwarriorx 0:603f46a29b61 246 yStepVal = yStepVal * -1;
magiwarriorx 0:603f46a29b61 247 }
magiwarriorx 0:603f46a29b61 248 }
magiwarriorx 0:603f46a29b61 249
magiwarriorx 0:603f46a29b61 250 if (z != 0){
magiwarriorx 0:603f46a29b61 251 _zEnable = 0;
magiwarriorx 0:603f46a29b61 252 if (z < 0){
magiwarriorx 0:603f46a29b61 253 _zDir = !defaultZdir;
magiwarriorx 0:603f46a29b61 254 z = z* -1;
magiwarriorx 0:603f46a29b61 255 zStepVal = zStepVal * -1;
magiwarriorx 0:603f46a29b61 256 }
magiwarriorx 0:603f46a29b61 257 }
magiwarriorx 0:603f46a29b61 258
magiwarriorx 0:603f46a29b61 259 while (x > 0 || y > 0 || z > 0){
magiwarriorx 0:603f46a29b61 260 if (x>0){
magiwarriorx 0:603f46a29b61 261 _xStep = 1;
magiwarriorx 0:603f46a29b61 262 x=x-1;
magiwarriorx 0:603f46a29b61 263 currX += xStepVal;
magiwarriorx 0:603f46a29b61 264 }
magiwarriorx 0:603f46a29b61 265 if (y>0){
magiwarriorx 0:603f46a29b61 266 _yStep = 1;
magiwarriorx 0:603f46a29b61 267 y=y-1;
magiwarriorx 0:603f46a29b61 268 currY += yStepVal;
magiwarriorx 0:603f46a29b61 269 }
magiwarriorx 0:603f46a29b61 270 if (z>0){
magiwarriorx 0:603f46a29b61 271 _zStep = 1;
magiwarriorx 0:603f46a29b61 272 z=z-1;
magiwarriorx 0:603f46a29b61 273 currZ += zStepVal;
magiwarriorx 0:603f46a29b61 274 }
magiwarriorx 0:603f46a29b61 275 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 276 _xStep = 0;
magiwarriorx 0:603f46a29b61 277 _yStep = 0;
magiwarriorx 0:603f46a29b61 278 _zStep = 0;
magiwarriorx 0:603f46a29b61 279 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 280 }
magiwarriorx 0:603f46a29b61 281 }
magiwarriorx 0:603f46a29b61 282
magiwarriorx 0:603f46a29b61 283 _xEnable = 1;
magiwarriorx 0:603f46a29b61 284 _yEnable = 1;
magiwarriorx 0:603f46a29b61 285 _zEnable = 1;
magiwarriorx 0:603f46a29b61 286 _xDir = defaultXdir;
magiwarriorx 0:603f46a29b61 287 _yDir = defaultYdir;
magiwarriorx 0:603f46a29b61 288 _zDir = defaultZdir;
magiwarriorx 0:603f46a29b61 289
magiwarriorx 0:603f46a29b61 290 }