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@0:603f46a29b61, 2021-12-15 (annotated)

Committer:
magiwarriorx
Date:
Wed Dec 15 00:01:51 2021 +0000
Revision:
0:603f46a29b61
Child:
1:43d856fad23a
Please work (zeroing);

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