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.

Dependents:   CNCAirbrushCode

LinearMotion.cpp@0:3058939fa37c, 2012-04-28 (annotated)

Committer:
stretch
Date:
Sat Apr 28 21:08:58 2012 +0000
Revision:
0:3058939fa37c
Child:
1:cf60b60a1b5b
Initial Public Release

Who changed what in which revision?

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