Adam Resnick
/
StepperMotors
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.
LinearMotion.cpp@1:cf60b60a1b5b, 2012-05-04 (annotated)
- Committer:
- stretch
- Date:
- Fri May 04 06:25:41 2012 +0000
- Revision:
- 1:cf60b60a1b5b
- Parent:
- 0:3058939fa37c
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| stretch | 0:3058939fa37c | 1 | #include "LinearMotion.h" |
| stretch | 1:cf60b60a1b5b | 2 | |
| stretch | 0:3058939fa37c | 3 | |
| stretch | 0:3058939fa37c | 4 | void LinearMotion::setStepMotors(Stepper * s1, Stepper * s2, Stepper * s3, DigitalOut * solenoid, bool * pause, bool * stop) { |
| stretch | 0:3058939fa37c | 5 | _sol = solenoid; |
| stretch | 0:3058939fa37c | 6 | StepMotor[0] = s1; |
| stretch | 0:3058939fa37c | 7 | StepMotor[1] = s2; |
| stretch | 0:3058939fa37c | 8 | StepMotor[2] = s3; |
| stretch | 0:3058939fa37c | 9 | reversing = false; |
| stretch | 0:3058939fa37c | 10 | _paused = pause; |
| stretch | 0:3058939fa37c | 11 | _stopped = stop; |
| stretch | 0:3058939fa37c | 12 | |
| stretch | 0:3058939fa37c | 13 | } |
| stretch | 0:3058939fa37c | 14 | |
| stretch | 0:3058939fa37c | 15 | void LinearMotion::interpolate(Vector length, int maxSpeed) { |
| stretch | 0:3058939fa37c | 16 | interpolate(length, maxSpeed, false); |
| stretch | 0:3058939fa37c | 17 | } |
| stretch | 0:3058939fa37c | 18 | |
| stretch | 0:3058939fa37c | 19 | void LinearMotion::interpolate(Vector length, int maxSpeed, bool solenoidOn) { |
| stretch | 0:3058939fa37c | 20 | long tempLength; |
| stretch | 0:3058939fa37c | 21 | Stepper * tempStep; |
| stretch | 0:3058939fa37c | 22 | Stepper * reorderedStepMotor[3] = {StepMotor[0], StepMotor[1], StepMotor[2]}; |
| stretch | 0:3058939fa37c | 23 | |
| stretch | 0:3058939fa37c | 24 | _z = false; |
| stretch | 0:3058939fa37c | 25 | if (abs(length.y) > abs(length.x)) { |
| stretch | 0:3058939fa37c | 26 | tempLength = length.y; |
| stretch | 0:3058939fa37c | 27 | length.y = length.x; |
| stretch | 0:3058939fa37c | 28 | length.x = tempLength; |
| stretch | 0:3058939fa37c | 29 | tempStep = reorderedStepMotor[1]; |
| stretch | 0:3058939fa37c | 30 | reorderedStepMotor[1] = reorderedStepMotor[0]; |
| stretch | 0:3058939fa37c | 31 | reorderedStepMotor[0] = tempStep; |
| stretch | 0:3058939fa37c | 32 | } |
| stretch | 0:3058939fa37c | 33 | if (abs(length.z) > abs(length.x)) { |
| stretch | 0:3058939fa37c | 34 | tempLength = length.z; |
| stretch | 0:3058939fa37c | 35 | length.z = length.x; |
| stretch | 0:3058939fa37c | 36 | length.x = tempLength; |
| stretch | 0:3058939fa37c | 37 | tempStep = reorderedStepMotor[2]; |
| stretch | 0:3058939fa37c | 38 | reorderedStepMotor[2] = reorderedStepMotor[0]; |
| stretch | 0:3058939fa37c | 39 | reorderedStepMotor[0] = tempStep; |
| stretch | 0:3058939fa37c | 40 | _z = true; |
| stretch | 0:3058939fa37c | 41 | } |
| stretch | 0:3058939fa37c | 42 | |
| stretch | 0:3058939fa37c | 43 | enableSteppers(true); |
| stretch | 0:3058939fa37c | 44 | doLinear(length, reorderedStepMotor, maxSpeed, solenoidOn); |
| stretch | 0:3058939fa37c | 45 | enableSteppers(false); |
| stretch | 0:3058939fa37c | 46 | } |
| stretch | 0:3058939fa37c | 47 | |
| stretch | 0:3058939fa37c | 48 | void LinearMotion::interpolateSquare(Vector basePos, Vector heightPos, int maxSpeed, bool solenoidOn) { |
| stretch | 0:3058939fa37c | 49 | long tempLength; |
| stretch | 0:3058939fa37c | 50 | Stepper * tempStep; |
| stretch | 0:3058939fa37c | 51 | |
| stretch | 0:3058939fa37c | 52 | Stepper * reorderedStepMotor[3] = {StepMotor[0], StepMotor[1], StepMotor[2]}; |
| stretch | 0:3058939fa37c | 53 | Vector homePosition = {-heightPos.x, -heightPos.y, -heightPos.z}; |
| stretch | 0:3058939fa37c | 54 | _z_slantways = false; |
| stretch | 0:3058939fa37c | 55 | |
| stretch | 0:3058939fa37c | 56 | if (abs(heightPos.y) > abs(heightPos.x)) { |
| stretch | 0:3058939fa37c | 57 | tempLength = heightPos.y; |
| stretch | 0:3058939fa37c | 58 | heightPos.y = heightPos.x; |
| stretch | 0:3058939fa37c | 59 | heightPos.x = tempLength; |
| stretch | 0:3058939fa37c | 60 | tempStep = reorderedStepMotor[1]; |
| stretch | 0:3058939fa37c | 61 | reorderedStepMotor[1] = reorderedStepMotor[0]; |
| stretch | 0:3058939fa37c | 62 | reorderedStepMotor[0] = tempStep; |
| stretch | 0:3058939fa37c | 63 | } |
| stretch | 0:3058939fa37c | 64 | if (abs(heightPos.z) > abs(heightPos.x)) { |
| stretch | 0:3058939fa37c | 65 | tempLength = heightPos.z; |
| stretch | 0:3058939fa37c | 66 | heightPos.z = heightPos.x; |
| stretch | 0:3058939fa37c | 67 | heightPos.x = tempLength; |
| stretch | 0:3058939fa37c | 68 | tempStep = reorderedStepMotor[2]; |
| stretch | 0:3058939fa37c | 69 | reorderedStepMotor[2] = reorderedStepMotor[0]; |
| stretch | 0:3058939fa37c | 70 | reorderedStepMotor[0] = tempStep; |
| stretch | 0:3058939fa37c | 71 | _z_slantways = true; |
| stretch | 0:3058939fa37c | 72 | } |
| stretch | 0:3058939fa37c | 73 | |
| stretch | 0:3058939fa37c | 74 | if (solenoidOn) |
| stretch | 0:3058939fa37c | 75 | _bmp.openImg("/local/pic.bmp"); |
| stretch | 0:3058939fa37c | 76 | reversing = false; |
| stretch | 0:3058939fa37c | 77 | enableSteppers(true); |
| stretch | 0:3058939fa37c | 78 | doLinearSideways(heightPos, basePos, reorderedStepMotor, maxSpeed, solenoidOn); |
| stretch | 0:3058939fa37c | 79 | if (solenoidOn) |
| stretch | 0:3058939fa37c | 80 | _bmp.closeImg(); |
| stretch | 0:3058939fa37c | 81 | if (reversing) { |
| stretch | 0:3058939fa37c | 82 | homePosition.x -= basePos.x; |
| stretch | 0:3058939fa37c | 83 | homePosition.y -= basePos.y; |
| stretch | 0:3058939fa37c | 84 | homePosition.z -= basePos.z; |
| stretch | 0:3058939fa37c | 85 | } |
| stretch | 0:3058939fa37c | 86 | interpolate(homePosition, maxSpeed); |
| stretch | 0:3058939fa37c | 87 | } |
| stretch | 0:3058939fa37c | 88 | |
| stretch | 0:3058939fa37c | 89 | void LinearMotion::doLinearSideways(Vector delta, Vector nextRow, |
| stretch | 0:3058939fa37c | 90 | Stepper ** stepMotor, int maxSpeed, bool solenoidOn) { |
| stretch | 0:3058939fa37c | 91 | int fxy,fxz; |
| stretch | 0:3058939fa37c | 92 | int rowLoc = 0; |
| stretch | 1:cf60b60a1b5b | 93 | int delay = _initial_delay; |
| stretch | 0:3058939fa37c | 94 | int delayPos = 0; |
| stretch | 0:3058939fa37c | 95 | Vector lastRow = {-nextRow.x, -nextRow.y, -nextRow.z}; |
| stretch | 0:3058939fa37c | 96 | Vector dir = {1, 1, 1}; |
| stretch | 0:3058939fa37c | 97 | long stepsPerPixel; |
| stretch | 1:cf60b60a1b5b | 98 | _z_slantways ? stepsPerPixel = _z_steps_per_pixel : stepsPerPixel = _steps_per_pixel; |
| stretch | 0:3058939fa37c | 99 | |
| stretch | 0:3058939fa37c | 100 | if(delta.x < 0) |
| stretch | 0:3058939fa37c | 101 | dir.x = 0; //towards motor |
| stretch | 0:3058939fa37c | 102 | if(delta.y < 0) |
| stretch | 0:3058939fa37c | 103 | dir.y = 0; //towards motor |
| stretch | 0:3058939fa37c | 104 | if(delta.z < 0) |
| stretch | 0:3058939fa37c | 105 | dir.z = 0; //towards motor |
| stretch | 0:3058939fa37c | 106 | delta.x = abs(delta.x); |
| stretch | 0:3058939fa37c | 107 | delta.y = abs(delta.y); |
| stretch | 0:3058939fa37c | 108 | delta.z = abs(delta.z); |
| stretch | 0:3058939fa37c | 109 | fxy = delta.x - delta.y; |
| stretch | 0:3058939fa37c | 110 | fxz = delta.x - delta.z; |
| stretch | 0:3058939fa37c | 111 | |
| stretch | 0:3058939fa37c | 112 | Vector curPos = {0, 0, 0}; |
| stretch | 0:3058939fa37c | 113 | |
| stretch | 0:3058939fa37c | 114 | while(!(*_stopped) && (curPos.x<=delta.x) && (curPos.y<=delta.y) && (curPos.z<=delta.z)){ |
| stretch | 0:3058939fa37c | 115 | // printf("Moving height: %d, %d, %d\n\r", curPos.x, curPos.y, curPos.z); |
| stretch | 0:3058939fa37c | 116 | if ((curPos.x % stepsPerPixel) == 0) { |
| stretch | 0:3058939fa37c | 117 | Vector * row = NULL; |
| stretch | 0:3058939fa37c | 118 | if (solenoidOn) |
| stretch | 0:3058939fa37c | 119 | _bmp.setRow(rowLoc++); |
| stretch | 0:3058939fa37c | 120 | if (!(solenoidOn && _bmp.isBlankRow())) { |
| stretch | 0:3058939fa37c | 121 | reversing ? row = &lastRow : row = &nextRow; |
| stretch | 0:3058939fa37c | 122 | interpolate(*row, maxSpeed, solenoidOn); |
| stretch | 0:3058939fa37c | 123 | reversing = !reversing; |
| stretch | 0:3058939fa37c | 124 | enableSteppers(true); // Hacky |
| stretch | 0:3058939fa37c | 125 | } |
| stretch | 0:3058939fa37c | 126 | } |
| stretch | 0:3058939fa37c | 127 | ++curPos.x; |
| stretch | 0:3058939fa37c | 128 | if ((curPos.x % stepsPerPixel) <= stepsPerPixel/2) { |
| stretch | 0:3058939fa37c | 129 | delayPos++; |
| stretch | 0:3058939fa37c | 130 | delay = delayTime(delay, delayPos); |
| stretch | 0:3058939fa37c | 131 | } else { |
| stretch | 0:3058939fa37c | 132 | delayPos--; |
| stretch | 0:3058939fa37c | 133 | delay = delayTime(delay, -delayPos); |
| stretch | 0:3058939fa37c | 134 | } |
| stretch | 0:3058939fa37c | 135 | stepMotor[0]->stepOn(dir.x); |
| stretch | 0:3058939fa37c | 136 | fxy -= delta.y; |
| stretch | 0:3058939fa37c | 137 | fxz -= delta.z; |
| stretch | 0:3058939fa37c | 138 | if(fxy <= 0){ |
| stretch | 0:3058939fa37c | 139 | ++curPos.y; |
| stretch | 0:3058939fa37c | 140 | stepMotor[1]->stepOn(dir.y); |
| stretch | 0:3058939fa37c | 141 | fxy += delta.x; |
| stretch | 0:3058939fa37c | 142 | } |
| stretch | 0:3058939fa37c | 143 | if(fxz <= 0){ |
| stretch | 0:3058939fa37c | 144 | ++curPos.z; |
| stretch | 0:3058939fa37c | 145 | stepMotor[2]->stepOn(dir.z); |
| stretch | 0:3058939fa37c | 146 | fxz += delta.x; |
| stretch | 0:3058939fa37c | 147 | } |
| stretch | 0:3058939fa37c | 148 | wait_us(1); |
| stretch | 0:3058939fa37c | 149 | stepMotor[0]->stepOff(); |
| stretch | 0:3058939fa37c | 150 | stepMotor[1]->stepOff(); |
| stretch | 0:3058939fa37c | 151 | stepMotor[2]->stepOff(); |
| stretch | 0:3058939fa37c | 152 | wait_us(delay); |
| stretch | 0:3058939fa37c | 153 | while(*_paused) {;} |
| stretch | 0:3058939fa37c | 154 | } |
| stretch | 0:3058939fa37c | 155 | } |
| stretch | 0:3058939fa37c | 156 | |
| stretch | 0:3058939fa37c | 157 | void LinearMotion::doLinear(Vector delta, Stepper** stepMotor, int maxSpeed, bool solenoidOn) { |
| stretch | 0:3058939fa37c | 158 | int fxy,fxz; |
| stretch | 0:3058939fa37c | 159 | int pixelLoc = 0; |
| stretch | 1:cf60b60a1b5b | 160 | int delay = _initial_delay; |
| stretch | 0:3058939fa37c | 161 | int farthestDelay = 0; |
| stretch | 0:3058939fa37c | 162 | Vector dir = {1, 1, 1}; |
| stretch | 0:3058939fa37c | 163 | long stepsPerPixel; |
| stretch | 1:cf60b60a1b5b | 164 | _z ? stepsPerPixel = _z_steps_per_pixel : stepsPerPixel = _steps_per_pixel; |
| stretch | 0:3058939fa37c | 165 | |
| stretch | 0:3058939fa37c | 166 | if(delta.x < 0) |
| stretch | 0:3058939fa37c | 167 | dir.x = 0; //towards motor |
| stretch | 0:3058939fa37c | 168 | if(delta.y < 0) |
| stretch | 0:3058939fa37c | 169 | dir.y = 0; //towards motor |
| stretch | 0:3058939fa37c | 170 | if(delta.z < 0) |
| stretch | 0:3058939fa37c | 171 | dir.z = 0; //towards motor |
| stretch | 0:3058939fa37c | 172 | delta.x = abs(delta.x); |
| stretch | 0:3058939fa37c | 173 | delta.y = abs(delta.y); |
| stretch | 0:3058939fa37c | 174 | delta.z = abs(delta.z); |
| stretch | 0:3058939fa37c | 175 | fxy = delta.x - delta.y; |
| stretch | 0:3058939fa37c | 176 | fxz = delta.x - delta.z; |
| stretch | 0:3058939fa37c | 177 | |
| stretch | 0:3058939fa37c | 178 | Vector curPos; |
| stretch | 0:3058939fa37c | 179 | curPos.x = 0; |
| stretch | 0:3058939fa37c | 180 | curPos.y = 0; |
| stretch | 0:3058939fa37c | 181 | curPos.z = 0; |
| stretch | 0:3058939fa37c | 182 | while (!(*_stopped) && (curPos.x<=delta.x)&&(curPos.y<=delta.y)&&(curPos.z<=delta.z)){ |
| stretch | 0:3058939fa37c | 183 | if (solenoidOn) { |
| stretch | 1:cf60b60a1b5b | 184 | if ((reversing && ((delta.x - curPos.x - stepsPerPixel / _step_buffer) % stepsPerPixel == 0)) || |
| stretch | 1:cf60b60a1b5b | 185 | (!reversing && ((curPos.x + stepsPerPixel / _step_buffer) % stepsPerPixel == 0))) |
| stretch | 0:3058939fa37c | 186 | *_sol = _bmp.isPixel(pixelLoc++, reversing); |
| stretch | 1:cf60b60a1b5b | 187 | else if ((reversing && ((delta.x - curPos.x + stepsPerPixel / _step_buffer) % stepsPerPixel == 0)) || |
| stretch | 1:cf60b60a1b5b | 188 | (!reversing && ((curPos.x - stepsPerPixel / _step_buffer) % stepsPerPixel == 0)) ) |
| stretch | 0:3058939fa37c | 189 | *_sol = 0; |
| stretch | 0:3058939fa37c | 190 | } |
| stretch | 0:3058939fa37c | 191 | ++curPos.x; |
| stretch | 0:3058939fa37c | 192 | if (delay > maxSpeed) { |
| stretch | 0:3058939fa37c | 193 | delay = delayTime(delay, curPos.x); |
| stretch | 0:3058939fa37c | 194 | farthestDelay = curPos.x; |
| stretch | 0:3058939fa37c | 195 | } else if (delta.x - curPos.x < farthestDelay) { |
| stretch | 0:3058939fa37c | 196 | delay = delayTime(delay, -curPos.x); |
| stretch | 0:3058939fa37c | 197 | } |
| stretch | 0:3058939fa37c | 198 | stepMotor[0]->stepOn(dir.x); |
| stretch | 0:3058939fa37c | 199 | fxy -= delta.y; |
| stretch | 0:3058939fa37c | 200 | fxz -= delta.z; |
| stretch | 0:3058939fa37c | 201 | if (fxy <= 0){ |
| stretch | 0:3058939fa37c | 202 | ++curPos.y; |
| stretch | 0:3058939fa37c | 203 | stepMotor[1]->stepOn(dir.y); |
| stretch | 0:3058939fa37c | 204 | fxy += delta.x; |
| stretch | 0:3058939fa37c | 205 | } |
| stretch | 0:3058939fa37c | 206 | if (fxz <= 0){ |
| stretch | 0:3058939fa37c | 207 | ++curPos.z; |
| stretch | 0:3058939fa37c | 208 | stepMotor[2]->stepOn(dir.z); |
| stretch | 0:3058939fa37c | 209 | fxz += delta.x; |
| stretch | 0:3058939fa37c | 210 | } |
| stretch | 0:3058939fa37c | 211 | wait_us(1); |
| stretch | 0:3058939fa37c | 212 | stepMotor[0]->stepOff(); |
| stretch | 0:3058939fa37c | 213 | stepMotor[1]->stepOff(); |
| stretch | 0:3058939fa37c | 214 | stepMotor[2]->stepOff(); |
| stretch | 0:3058939fa37c | 215 | wait_us(delay); //Implement linear accelleration! |
| stretch | 0:3058939fa37c | 216 | if (*_paused) { |
| stretch | 0:3058939fa37c | 217 | enableSteppers(false); |
| stretch | 1:cf60b60a1b5b | 218 | *_sol = 0; |
| stretch | 0:3058939fa37c | 219 | while(*_paused){printf("Pausing!\n\r");} |
| stretch | 0:3058939fa37c | 220 | enableSteppers(true); |
| stretch | 0:3058939fa37c | 221 | } |
| stretch | 0:3058939fa37c | 222 | } |
| stretch | 1:cf60b60a1b5b | 223 | *_sol = 0; |
| stretch | 0:3058939fa37c | 224 | } |
| stretch | 1:cf60b60a1b5b | 225 | |
| stretch | 1:cf60b60a1b5b | 226 | void LinearMotion::updateSettings(long steps_per_pixel, long initial_delay, long step_buffer) { |
| stretch | 1:cf60b60a1b5b | 227 | _steps_per_pixel = steps_per_pixel; |
| stretch | 1:cf60b60a1b5b | 228 | _z_steps_per_pixel = (steps_per_pixel * 8) / 5; |
| stretch | 1:cf60b60a1b5b | 229 | _step_buffer = step_buffer; // Make a gap of _steps_per_pixel/_step_buffer between each pixel |
| stretch | 1:cf60b60a1b5b | 230 | _initial_delay = initial_delay; // in us |
| stretch | 1:cf60b60a1b5b | 231 | } |
| stretch | 0:3058939fa37c | 232 | |
| stretch | 0:3058939fa37c | 233 | int LinearMotion::delayTime (int oldDelay, int stepNumber) { |
| stretch | 0:3058939fa37c | 234 | return oldDelay - (2*oldDelay)/(4*stepNumber + 1); |
| stretch | 0:3058939fa37c | 235 | } |
| stretch | 0:3058939fa37c | 236 | |
| stretch | 0:3058939fa37c | 237 | |
| stretch | 0:3058939fa37c | 238 | void LinearMotion::enableSteppers(bool en) { |
| stretch | 0:3058939fa37c | 239 | StepMotor[0]->enable(en); |
| stretch | 0:3058939fa37c | 240 | StepMotor[1]->enable(en); |
| stretch | 0:3058939fa37c | 241 | StepMotor[2]->enable(en); |
| stretch | 0:3058939fa37c | 242 | } |