Revision 1:52716241d0c4, committed 2012-04-21

Comitter:

stvnrhodes

Date:

Sat Apr 21 07:12:29 2012 +0000

Parent:

0:6e7505d77f6a

Commit message:

For Adam

Changed in this revision

--- a/LinearMotion.c	Sat Apr 21 05:48:28 2012 +0000
+++ b/LinearMotion.c	Sat Apr 21 07:12:29 2012 +0000
@@ -1,12 +1,18 @@
 #include  "LinearMotion.h"
+#define STEPS_PER_PIXEL 100
 
-void LinearMotion::setStepMotors(Stepper * s1, Stepper * s2, Stepper * s3) {
+void LinearMotion::setStepMotors(Stepper * s1, Stepper * s2, Stepper * s3, DigitalOut * solenoid) {
+    _sol = solenoid;
     StepMotor[0] = s1;
     StepMotor[1] = s2;
     StepMotor[2] = s3;
 }
 
 void LinearMotion::interpolate(Vector length, int maxSpeed) {
+    interpolate(length, maxSpeed, false);
+}
+
+void LinearMotion::interpolate(Vector length, int maxSpeed, bool solenoidOn) {
     long tempLength;
     Stepper * tempStep;
         
@@ -16,7 +22,7 @@
     reorderedStepMotor[2] = StepMotor[2];
     if (abs(length.y) > abs(length.x)) {
         tempLength = length.y;
-        length.y = tempLength;
+        length.y = length.x;
         length.x = tempLength;
         tempStep = reorderedStepMotor[1];
         reorderedStepMotor[1] = reorderedStepMotor[0];
@@ -32,15 +38,105 @@
     }    
     
     enableSteppers(true);
-    doLinear(length,reorderedStepMotor,maxSpeed);
+    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];
+    reorderedStepMotor[0] = StepMotor[0];
+    reorderedStepMotor[1] = StepMotor[1];
+    reorderedStepMotor[2] = StepMotor[2];
+    if (abs(heightPos.y) > abs(heightPos.x)) {
+        tempLength = heightPos.y;
+        heightPos.y = tempLength;
+        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;
+    }    
+    
+    enableSteppers(true);
+    doLinearSideways(heightPos, basePos, reorderedStepMotor, maxSpeed, solenoidOn);
+    Vector homePosition;
+    homePosition.x = -(heightPos.x);
+    homePosition.y = -(heightPos.y);
+    homePosition.z = -(heightPos.z);
+    interpolate(homePosition, maxSpeed);
+    enableSteppers(false);
+    
 }
+
+void LinearMotion::doLinearSideways(Vector delta, Vector nextRow, Stepper ** stepMotor, int maxSpeed, bool solenoidOn) {
+    int fxy,fxz;
+    Vector lastRow, dir;
+    lastRow.x = -nextRow.x;
+    lastRow.y = -nextRow.y;
+    lastRow.z = -nextRow.z;
     
+    if(delta.x < 0)
+        dir.x = 0; //towards motor
+    else
+        dir.x = 1; //away from motor
+    if(delta.y < 0)
+        dir.y = 0; //towards motor
+    else
+        dir.y = 1; //away from motor
+    if(delta.z < 0)
+        dir.z = 0; //towards motor
+    else
+        dir.z = 1; //away from 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 currentPos;
+    currentPos.x = 0;
+    currentPos.y = 0;
+    currentPos.z = 0;
+    
+    while((currentPos.x<=delta.x)&&(currentPos.y<=delta.y)&&(currentPos.z<=delta.z)){
+        ++currentPos.x;
+         ///printf("Moving height: %d, %d, %d\n\r", currentPos.x, currentPos.y, currentPos.z);
+        if((currentPos.x % STEPS_PER_PIXEL) == 0) {
+            interpolate(nextRow, maxSpeed, solenoidOn);
+            interpolate(lastRow, maxSpeed, false);
+        }
+        stepMotor[0]->stepOn(dir.x);
+        fxy -= delta.y;
+        fxz -= delta.z;
+        if(fxy <= 0){
+            ++currentPos.y;
+            stepMotor[1]->stepOn(dir.y);
+            fxy += delta.x;
+        }
+        if(fxz <= 0){
+            ++currentPos.z;
+            stepMotor[2]->stepOn(dir.z);
+            fxz += delta.x;
+        }
+        wait_us(1);
+        stepMotor[0]->stepOff();
+        stepMotor[1]->stepOff();
+        stepMotor[2]->stepOff();
+        wait_us(maxSpeed);  //Implement linear accelleration!
+    }
+}
 
-void LinearMotion::doLinear(Vector delta, Stepper** stepMotor, int maxSpeed) {
+void LinearMotion::doLinear(Vector delta, Stepper** stepMotor, int maxSpeed, bool solenoidOn) {
     int fxy,fxz;
     Vector dir;
     
@@ -69,6 +165,10 @@
     
     while((currentPos.x<=delta.x)&&(currentPos.y<=delta.y)&&(currentPos.z<=delta.z)){
         ++currentPos.x;
+//        printf("Moving base: %d, %d, %d,, %d, %d\n\r", currentPos.x, currentPos.y, currentPos.z, delta.x, delta.y);
+        if(solenoidOn && (currentPos.x % STEPS_PER_PIXEL == 0)) {
+            *_sol = !(*_sol);
+        }
         stepMotor[0]->stepOn(dir.x);
         fxy -= delta.y;
         fxz -= delta.z;

--- a/LinearMotion.h	Sat Apr 21 05:48:28 2012 +0000
+++ b/LinearMotion.h	Sat Apr 21 07:12:29 2012 +0000
@@ -1,69 +1,73 @@
-/* mbed Stepper Library
- * Copyright (c) 2012 Steven Rhodes
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
- 
-#ifndef LINEARMOTION_H
-#define LINEARMOTION_H
-
-#include "mbed.h"
-#include "Stepper.h"
-
-typedef struct {
-    long x;
-    long y;
-    long z;
-} Vector;
-
-class LinearMotion {
-
-public:
-
-    /** Interpolate to point specified by the vector (relative to current position)
-     * 
-     * @param Stepper * s1 The x stepper motor
-     * @param Stepper * s2 The y stepper motor
-     * @param Stepper * s3 The z stepper motor
-     */
-    void setStepMotors(Stepper * s1, Stepper * s2, Stepper * s3);
-
-    /** Interpolate to point specified by the vector (relative to current position)
-     * 
-     * @param Vector length The vector specifying where to go
-     * @param int maxSpeed Max speed used to get to position
-     */
-    void interpolate(Vector length, int maxSpeed);
-
-    /** Interpolate to square specified by the vector (relative to current position)
-     * 
-     * @param Vector basePos Position defining the base of the square
-     * @param Vector heightPos Position defining the height of the square
-     * @param int maxSpeed Max speed used to get to position
-     */
-    void interpolateSquare(Vector basePos, Vector heightPos, int maxSpeed, bool solenoidOn);
-  
-private:
-
-    void doLinear(Vector delta, Stepper** stepMotor, int maxSpeed);
-    void enableSteppers(bool en);
-    Stepper * StepMotor[3];
-};
- 
+/* mbed Stepper Library
+ * Copyright (c) 2012 Steven Rhodes
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+ 
+#ifndef LINEARMOTION_H
+#define LINEARMOTION_H
+
+#include "mbed.h"
+#include "Stepper.h"
+
+typedef struct {
+    long x;
+    long y;
+    long z;
+} Vector;
+
+class LinearMotion {
+
+public:
+
+    /** Interpolate to point specified by the vector (relative to current position)
+     * 
+     * @param Stepper * s1 The x stepper motor
+     * @param Stepper * s2 The y stepper motor
+     * @param Stepper * s3 The z stepper motor
+     */
+    void setStepMotors(Stepper * s1, Stepper * s2, Stepper * s3, DigitalOut * solenoid);
+
+    /** Interpolate to point specified by the vector (relative to current position)
+     * 
+     * @param Vector length The vector specifying where to go
+     * @param int maxSpeed Max speed used to get to position
+     * @param bool solenoidOn Whether to have the solenoid on
+     */
+    void interpolate(Vector length, int maxSpeed, bool solenoidOn);
+    void interpolate(Vector length, int maxSpeed);
+    
+    /** Interpolate to square specified by the vector (relative to current position)
+     * 
+     * @param Vector basePos Position defining the base of the square
+     * @param Vector heightPos Position defining the height of the square
+     * @param int maxSpeed Max speed used to get to position
+     * @param bool solenoidOn Whether to have the solenoid on
+     */
+    void interpolateSquare(Vector basePos, Vector heightPos, int maxSpeed, bool solenoidOn);
+  
+private:
+    void doLinearSideways(Vector delta, Vector nextRow, Stepper ** stepMotor, int maxSpeed, bool solenoidOn);
+    void doLinear(Vector delta, Stepper** stepMotor, int maxSpeed, bool solenoidOn);
+    void enableSteppers(bool en);
+    Stepper * StepMotor[3];
+    DigitalOut * _sol;
+};
+ 
  #endif
\ No newline at end of file

--- a/Stepper.cpp	Sat Apr 21 05:48:28 2012 +0000
+++ b/Stepper.cpp	Sat Apr 21 07:12:29 2012 +0000
@@ -1,18 +1,18 @@
-#include "Stepper.h"
-
-Stepper::Stepper(PinName step, PinName dir, PinName en) : _step(step), _dir(dir), _en(en) {
-    _step = 0, _dir = 0, _en = 1;
-}
-
-void Stepper::stepOn(bool direction) {
-    _dir = direction;
-    _step = 1;
-}
-
-void Stepper::stepOff(void) {
-    _step = 0;
-}
-
-void Stepper::enable(bool en) {
-    _en = !en;
+#include "Stepper.h"
+
+Stepper::Stepper(PinName step, PinName dir, PinName en) : _step(step), _dir(dir), _en(en) {
+    _step = 0, _dir = 0, _en = 1;
+}
+
+void Stepper::stepOn(bool direction) {
+    _dir = direction;
+    _step = 1;
+}
+
+void Stepper::stepOff(void) {
+    _step = 0;
+}
+
+void Stepper::enable(bool en) {
+    _en = !en;
 }
\ No newline at end of file

--- a/Stepper.h	Sat Apr 21 05:48:28 2012 +0000
+++ b/Stepper.h	Sat Apr 21 07:12:29 2012 +0000
@@ -1,59 +1,59 @@
-/* mbed Stepper Library
- * Copyright (c) 2012 Steven Rhodes
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-#ifndef STEPPER_H
-#define STEPPER_H
-
-#include "mbed.h"
-
-class Stepper {
-
-public:
-
-    /** Create a stepper object connected to the specified clk pin and dir pin
-     *
-     * @param pin step pin to trigger for steps 
-     * @param pin dir pin to choose direction
-     * @param pin en pin to enable the motor
-     */
-    Stepper(PinName step, PinName dir, PinName en);
-    
-    /** Turn on the stepper motor
-     * @param direction 1 or 0
-     */
-    void stepOn(bool direction);  
-    
-    /** Turn off the stepper motor **/
-    void stepOff(void);
-    
-    /** Enable the motor 
-     @param en Whether to enable the motor
-     */
-    void enable(bool en);
-  
-private:  
-    DigitalOut _step;
-    DigitalOut _dir;
-    DigitalOut _en;
-     
-};
-
+/* mbed Stepper Library
+ * Copyright (c) 2012 Steven Rhodes
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef STEPPER_H
+#define STEPPER_H
+
+#include "mbed.h"
+
+class Stepper {
+
+public:
+
+    /** Create a stepper object connected to the specified clk pin and dir pin
+     *
+     * @param pin step pin to trigger for steps 
+     * @param pin dir pin to choose direction
+     * @param pin en pin to enable the motor
+     */
+    Stepper(PinName step, PinName dir, PinName en);
+    
+    /** Turn on the stepper motor
+     * @param direction 1 or 0
+     */
+    void stepOn(bool direction);  
+    
+    /** Turn off the stepper motor **/
+    void stepOff(void);
+    
+    /** Enable the motor 
+     @param en Whether to enable the motor
+     */
+    void enable(bool en);
+  
+private:  
+    DigitalOut _step;
+    DigitalOut _dir;
+    DigitalOut _en;
+     
+};
+
 #endif
\ No newline at end of file