Diff: LinearMotion.cpp

Revision:

0:3058939fa37c

Child:

1:cf60b60a1b5b

diff -r 000000000000 -r 3058939fa37c LinearMotion.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearMotion.cpp	Sat Apr 28 21:08:58 2012 +0000
@@ -0,0 +1,236 @@
+#include  "LinearMotion.h"
+#define STEPS_PER_PIXEL 600
+#define Z_STEPS_PER_PIXEL 960
+#define STEP_BUFFER 4 // Make a gap of STEPS_PER_PIXEL/STEP_BUFFER  between each pixel
+#define INITIAL_DELAY 1200 // in us
+
+void LinearMotion::setStepMotors(Stepper * s1, Stepper * s2, Stepper * s3, DigitalOut * solenoid, bool * pause, bool * stop) {
+    _sol = solenoid;
+    StepMotor[0] = s1;
+    StepMotor[1] = s2;
+    StepMotor[2] = s3;
+    reversing = false;
+    _paused = pause;
+    _stopped = stop;
+    
+}
+
+void LinearMotion::interpolate(Vector length, int maxSpeed) {
+    interpolate(length, maxSpeed, false);
+}
+
+void LinearMotion::interpolate(Vector length, int maxSpeed, bool solenoidOn) {
+    long tempLength;
+    Stepper * tempStep;
+    Stepper * reorderedStepMotor[3] = {StepMotor[0], StepMotor[1], StepMotor[2]};
+
+    _z = false;
+    if (abs(length.y) > abs(length.x)) {
+        tempLength = length.y;
+        length.y = length.x;
+        length.x = tempLength;
+        tempStep = reorderedStepMotor[1];
+        reorderedStepMotor[1] = reorderedStepMotor[0];
+        reorderedStepMotor[0] = tempStep;
+    }
+    if (abs(length.z) > abs(length.x)) {
+        tempLength = length.z;
+        length.z = length.x;
+        length.x = tempLength;
+        tempStep = reorderedStepMotor[2];
+        reorderedStepMotor[2] = reorderedStepMotor[0];
+        reorderedStepMotor[0] = tempStep;
+        _z = true;
+    }    
+    
+    enableSteppers(true);
+    doLinear(length, reorderedStepMotor, maxSpeed, solenoidOn);
+    enableSteppers(false);
+}
+
+void LinearMotion::interpolateSquare(Vector basePos, Vector heightPos, int maxSpeed, bool solenoidOn) {
+    long tempLength;
+    Stepper * tempStep;
+    
+    Stepper * reorderedStepMotor[3] = {StepMotor[0], StepMotor[1], StepMotor[2]};
+    Vector homePosition = {-heightPos.x, -heightPos.y, -heightPos.z};
+    _z_slantways = false;
+    
+    if (abs(heightPos.y) > abs(heightPos.x)) {
+        tempLength = heightPos.y;
+        heightPos.y = heightPos.x;
+        heightPos.x = tempLength;
+        tempStep = reorderedStepMotor[1];
+        reorderedStepMotor[1] = reorderedStepMotor[0];
+        reorderedStepMotor[0] = tempStep;
+    }
+    if (abs(heightPos.z) > abs(heightPos.x)) {
+        tempLength = heightPos.z;
+        heightPos.z = heightPos.x;
+        heightPos.x = tempLength;
+        tempStep = reorderedStepMotor[2];
+        reorderedStepMotor[2] = reorderedStepMotor[0];
+        reorderedStepMotor[0] = tempStep;
+        _z_slantways = true;
+    }    
+    
+    if (solenoidOn) 
+        _bmp.openImg("/local/pic.bmp");
+    reversing = false;
+    enableSteppers(true);
+    doLinearSideways(heightPos, basePos, reorderedStepMotor, maxSpeed, solenoidOn);
+    if (solenoidOn)
+        _bmp.closeImg();
+    if (reversing) {
+        homePosition.x -= basePos.x;
+        homePosition.y -= basePos.y;
+        homePosition.z -= basePos.z;
+    }
+    interpolate(homePosition, maxSpeed);
+}
+
+void LinearMotion::doLinearSideways(Vector delta, Vector nextRow,
+                     Stepper ** stepMotor, int maxSpeed, bool solenoidOn) {
+    int fxy,fxz;
+    int rowLoc = 0;
+    int delay = INITIAL_DELAY;
+    int delayPos = 0;
+    Vector lastRow = {-nextRow.x, -nextRow.y, -nextRow.z};
+    Vector dir = {1, 1, 1};
+    long stepsPerPixel;
+    _z_slantways ? stepsPerPixel = Z_STEPS_PER_PIXEL : stepsPerPixel = STEPS_PER_PIXEL;
+    
+    if(delta.x < 0)
+        dir.x = 0; //towards motor
+    if(delta.y < 0)
+        dir.y = 0; //towards motor
+    if(delta.z < 0)
+        dir.z = 0; //towards motor
+    delta.x = abs(delta.x);
+    delta.y = abs(delta.y);
+    delta.z = abs(delta.z);
+    fxy = delta.x - delta.y;
+    fxz = delta.x - delta.z;
+        
+    Vector curPos = {0, 0, 0};
+    
+    while(!(*_stopped) && (curPos.x<=delta.x) && (curPos.y<=delta.y) && (curPos.z<=delta.z)){
+       // printf("Moving height: %d, %d, %d\n\r", curPos.x, curPos.y, curPos.z);
+        if ((curPos.x % stepsPerPixel) == 0) {
+            Vector * row = NULL;
+            if (solenoidOn)
+                _bmp.setRow(rowLoc++);
+            if (!(solenoidOn && _bmp.isBlankRow())) {
+                reversing ? row = &lastRow : row = &nextRow;
+                interpolate(*row, maxSpeed, solenoidOn);
+                reversing = !reversing;
+                enableSteppers(true); // Hacky
+            }
+        }
+        ++curPos.x;
+        if ((curPos.x % stepsPerPixel) <= stepsPerPixel/2) {
+            delayPos++;
+            delay = delayTime(delay, delayPos);
+        } else {
+            delayPos--;
+            delay = delayTime(delay, -delayPos);
+        }
+        stepMotor[0]->stepOn(dir.x);
+        fxy -= delta.y;
+        fxz -= delta.z;
+        if(fxy <= 0){
+            ++curPos.y;
+            stepMotor[1]->stepOn(dir.y);
+            fxy += delta.x;
+        }
+        if(fxz <= 0){
+            ++curPos.z;
+            stepMotor[2]->stepOn(dir.z);
+            fxz += delta.x;
+        }
+        wait_us(1);
+        stepMotor[0]->stepOff();
+        stepMotor[1]->stepOff();
+        stepMotor[2]->stepOff();
+        wait_us(delay);
+        while(*_paused) {;}
+    }
+}
+
+void LinearMotion::doLinear(Vector delta, Stepper** stepMotor, int maxSpeed, bool solenoidOn) {
+    int fxy,fxz;
+    int pixelLoc = 0;
+    int delay = INITIAL_DELAY;
+    int farthestDelay = 0;
+    Vector dir = {1, 1, 1};
+    long stepsPerPixel;
+    _z ? stepsPerPixel = Z_STEPS_PER_PIXEL : stepsPerPixel = STEPS_PER_PIXEL;
+    
+    if(delta.x < 0)
+        dir.x = 0; //towards motor
+    if(delta.y < 0)
+        dir.y = 0; //towards motor
+    if(delta.z < 0)
+        dir.z = 0; //towards motor
+    delta.x = abs(delta.x);
+    delta.y = abs(delta.y);
+    delta.z = abs(delta.z);
+    fxy = delta.x - delta.y;
+    fxz = delta.x - delta.z;
+        
+    Vector curPos;
+    curPos.x = 0;
+    curPos.y = 0;
+    curPos.z = 0;
+    while (!(*_stopped) && (curPos.x<=delta.x)&&(curPos.y<=delta.y)&&(curPos.z<=delta.z)){
+        if (solenoidOn) {
+            if ((reversing && ((delta.x - curPos.x - stepsPerPixel / STEP_BUFFER) % stepsPerPixel == 0)) ||
+               (!reversing && ((curPos.x + stepsPerPixel / STEP_BUFFER) % stepsPerPixel == 0)))
+                *_sol = _bmp.isPixel(pixelLoc++, reversing);
+            else if ((reversing &&  ((delta.x - curPos.x + stepsPerPixel / STEP_BUFFER) % stepsPerPixel == 0)) ||
+                    (!reversing &&  ((curPos.x - stepsPerPixel / STEP_BUFFER) % stepsPerPixel == 0)) )
+                *_sol = 0;
+        }
+        ++curPos.x;
+        if (delay > maxSpeed) {
+            delay = delayTime(delay, curPos.x);
+            farthestDelay = curPos.x;
+        } else if (delta.x - curPos.x < farthestDelay) {
+            delay = delayTime(delay, -curPos.x);
+        }
+        stepMotor[0]->stepOn(dir.x);
+        fxy -= delta.y;
+        fxz -= delta.z;
+        if (fxy <= 0){
+            ++curPos.y;
+            stepMotor[1]->stepOn(dir.y);
+            fxy += delta.x;
+        }
+        if (fxz <= 0){
+            ++curPos.z;
+            stepMotor[2]->stepOn(dir.z);
+            fxz += delta.x;
+        }
+        wait_us(1);
+        stepMotor[0]->stepOff();
+        stepMotor[1]->stepOff();
+        stepMotor[2]->stepOff();
+        wait_us(delay);  //Implement linear accelleration!
+        if (*_paused) {
+            enableSteppers(false);
+            while(*_paused){printf("Pausing!\n\r");}
+            enableSteppers(true);
+        }
+    }
+}
+      
+int LinearMotion::delayTime (int oldDelay, int stepNumber) {
+    return oldDelay - (2*oldDelay)/(4*stepNumber + 1);
+}
+
+      
+void LinearMotion::enableSteppers(bool en) {
+    StepMotor[0]->enable(en);
+    StepMotor[1]->enable(en);
+    StepMotor[2]->enable(en);
+}