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@2:5206484275e3, 2021-12-15 (annotated)

Committer:
magiwarriorx
Date:
Wed Dec 15 02:10:48 2021 +0000
Revision:
2:5206484275e3
Parent:
1:43d856fad23a
Child:
3:09abcf461dc2
buffer fixed

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 2:5206484275e3 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 1:43d856fad23a 163 /*void threeAxis::goTo(float xVal, float yVal, float zVal){
magiwarriorx 0:603f46a29b61 164 int xSteps = (int)xVal*_stepRatio;
magiwarriorx 0:603f46a29b61 165 int ySteps = (int)yVal*_stepRatio;
magiwarriorx 0:603f46a29b61 166 int zSteps = (int)zVal*_stepRatio;
magiwarriorx 0:603f46a29b61 167
magiwarriorx 1:43d856fad23a 168 this->goToRaw(xSteps, ySteps, zSteps);
magiwarriorx 1:43d856fad23a 169 }*/
magiwarriorx 0:603f46a29b61 170
magiwarriorx 1:43d856fad23a 171 void threeAxis::goTo(float xVal, float yVal, float zVal){
magiwarriorx 1:43d856fad23a 172
magiwarriorx 2:5206484275e3 173 int xSteps = (int)(xVal/_stepRatio);
magiwarriorx 2:5206484275e3 174 int ySteps = (int)(yVal/_stepRatio);
magiwarriorx 2:5206484275e3 175 int zSteps = (int)(zVal/_stepRatio);
magiwarriorx 1:43d856fad23a 176
magiwarriorx 1:43d856fad23a 177 //uLCD.printf("\n%.2f, %.2f, %.2f, %d, %d, %d", xVal, yVal, zVal, xSteps, ySteps, zSteps);
magiwarriorx 1:43d856fad23a 178
magiwarriorx 1:43d856fad23a 179 toX = xSteps;
magiwarriorx 1:43d856fad23a 180 toY = ySteps;
magiwarriorx 1:43d856fad23a 181 toZ = zSteps;
magiwarriorx 0:603f46a29b61 182
magiwarriorx 0:603f46a29b61 183
magiwarriorx 1:43d856fad23a 184
magiwarriorx 2:5206484275e3 185 if (toX > (int)(_xMax / _stepRatio)){
magiwarriorx 2:5206484275e3 186 toX = (int)(_xMax / _stepRatio);
magiwarriorx 0:603f46a29b61 187 }
magiwarriorx 0:603f46a29b61 188
magiwarriorx 2:5206484275e3 189 if (toY > (int)(_yMax / _stepRatio)){
magiwarriorx 2:5206484275e3 190 toY = (int)(_yMax / _stepRatio);
magiwarriorx 0:603f46a29b61 191 }
magiwarriorx 0:603f46a29b61 192
magiwarriorx 2:5206484275e3 193 if (toZ > (int)(_zMax / _stepRatio)){
magiwarriorx 2:5206484275e3 194 toZ = (int)(_zMax / _stepRatio);
magiwarriorx 0:603f46a29b61 195 }
magiwarriorx 0:603f46a29b61 196
magiwarriorx 1:43d856fad23a 197 //uLCD.printf("\nTo: %d, %d, %d", toX, toY, toZ);
magiwarriorx 1:43d856fad23a 198 //uLCD.printf("\nCur: %d, %d, %d", currX, currY, currZ);
magiwarriorx 0:603f46a29b61 199 int xStepVal = 1;
magiwarriorx 0:603f46a29b61 200 int yStepVal = 1;
magiwarriorx 0:603f46a29b61 201 int zStepVal = 1;
magiwarriorx 0:603f46a29b61 202
magiwarriorx 0:603f46a29b61 203 _xDir = defaultXdir;
magiwarriorx 0:603f46a29b61 204 _yDir = defaultYdir;
magiwarriorx 0:603f46a29b61 205 _zDir = defaultZdir;
magiwarriorx 0:603f46a29b61 206
magiwarriorx 0:603f46a29b61 207 int x = toX - currX;
magiwarriorx 0:603f46a29b61 208 int y = toY - currY;
magiwarriorx 0:603f46a29b61 209 int z = toZ - currZ;
magiwarriorx 1:43d856fad23a 210
magiwarriorx 1:43d856fad23a 211 //uLCD.printf("\n%d,%d,%d", x,y,z);
magiwarriorx 0:603f46a29b61 212
magiwarriorx 0:603f46a29b61 213
magiwarriorx 0:603f46a29b61 214
magiwarriorx 0:603f46a29b61 215 if (x != 0 || y != 0 || z != 0){
magiwarriorx 0:603f46a29b61 216 if (x != 0){
magiwarriorx 0:603f46a29b61 217 _xEnable = 0;
magiwarriorx 0:603f46a29b61 218 if (x < 0){
magiwarriorx 0:603f46a29b61 219 _xDir = !defaultXdir;
magiwarriorx 0:603f46a29b61 220 x = x* -1;
magiwarriorx 0:603f46a29b61 221 xStepVal = xStepVal * -1;
magiwarriorx 0:603f46a29b61 222 }
magiwarriorx 0:603f46a29b61 223 }
magiwarriorx 0:603f46a29b61 224
magiwarriorx 0:603f46a29b61 225 if (y != 0){
magiwarriorx 0:603f46a29b61 226 _yEnable = 0;
magiwarriorx 0:603f46a29b61 227 if (y < 0){
magiwarriorx 0:603f46a29b61 228 _yDir = !defaultYdir;
magiwarriorx 0:603f46a29b61 229 y = y* -1;
magiwarriorx 0:603f46a29b61 230 yStepVal = yStepVal * -1;
magiwarriorx 0:603f46a29b61 231 }
magiwarriorx 0:603f46a29b61 232 }
magiwarriorx 0:603f46a29b61 233
magiwarriorx 0:603f46a29b61 234 if (z != 0){
magiwarriorx 0:603f46a29b61 235 _zEnable = 0;
magiwarriorx 0:603f46a29b61 236 if (z < 0){
magiwarriorx 0:603f46a29b61 237 _zDir = !defaultZdir;
magiwarriorx 0:603f46a29b61 238 z = z* -1;
magiwarriorx 0:603f46a29b61 239 zStepVal = zStepVal * -1;
magiwarriorx 0:603f46a29b61 240 }
magiwarriorx 0:603f46a29b61 241 }
magiwarriorx 0:603f46a29b61 242
magiwarriorx 0:603f46a29b61 243 while (x > 0 || y > 0 || z > 0){
magiwarriorx 0:603f46a29b61 244 if (x>0){
magiwarriorx 0:603f46a29b61 245 _xStep = 1;
magiwarriorx 0:603f46a29b61 246 x=x-1;
magiwarriorx 0:603f46a29b61 247 currX += xStepVal;
magiwarriorx 0:603f46a29b61 248 }
magiwarriorx 0:603f46a29b61 249 if (y>0){
magiwarriorx 0:603f46a29b61 250 _yStep = 1;
magiwarriorx 0:603f46a29b61 251 y=y-1;
magiwarriorx 0:603f46a29b61 252 currY += yStepVal;
magiwarriorx 0:603f46a29b61 253 }
magiwarriorx 0:603f46a29b61 254 if (z>0){
magiwarriorx 0:603f46a29b61 255 _zStep = 1;
magiwarriorx 0:603f46a29b61 256 z=z-1;
magiwarriorx 0:603f46a29b61 257 currZ += zStepVal;
magiwarriorx 0:603f46a29b61 258 }
magiwarriorx 0:603f46a29b61 259 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 260 _xStep = 0;
magiwarriorx 0:603f46a29b61 261 _yStep = 0;
magiwarriorx 0:603f46a29b61 262 _zStep = 0;
magiwarriorx 0:603f46a29b61 263 Thread::wait(wait);
magiwarriorx 0:603f46a29b61 264 }
magiwarriorx 0:603f46a29b61 265 }
magiwarriorx 0:603f46a29b61 266
magiwarriorx 0:603f46a29b61 267 _xEnable = 1;
magiwarriorx 0:603f46a29b61 268 _yEnable = 1;
magiwarriorx 0:603f46a29b61 269 _zEnable = 1;
magiwarriorx 0:603f46a29b61 270 _xDir = defaultXdir;
magiwarriorx 0:603f46a29b61 271 _yDir = defaultYdir;
magiwarriorx 0:603f46a29b61 272 _zDir = defaultZdir;
magiwarriorx 0:603f46a29b61 273
magiwarriorx 0:603f46a29b61 274 }