main.cpp

00001 #include "mbed.h"
00002 #include "stepper.h"
00003 #define ACCEL_ON  1
00004 #define ACCEL_OFF 0
00005 #define SPEED 1000 //If forcing linear acceleration, speed maxes out around 299 (16th step mode, no load)
00006 #define STEPS_PER_REV 3200
00007 /*1B:Blue
00008 1A:Red
00009 2A:Black
00010 2B:Green*/
00011 
00012 DigitalOut xEnable(p14);
00013 DigitalOut yEnable(p28);
00014 DigitalOut zEnable(p21);
00015 DigitalOut solenoid(p29);
00016 stepper x(p6,p5);
00017 stepper y(p24,p27);
00018 stepper z(p22,p23);
00019 DigitalOut led(LED1);
00020 Serial pc(USBTX,USBRX);
00021 
00022 typedef struct {
00023     long x;
00024     long y;
00025     long z;
00026 } Vector;
00027 
00028 typedef struct {
00029     long longest;
00030     long other1;
00031     long other2;
00032 } Length;
00033 
00034 /*axis.longest returns the number of steps to move along the longest direction. axis.other1 and axis.other2 are the steps to move in the other two directions. &x is a pointer to the stepper controlling the axis of greatest travel.
00035 &y corresponds to axis.other1 and &z corresponds to axis.other2. startPos.x is the starting position of the longest travel direction. startPos.y corresponds to &y and startPos.z corresponds to &z.*/
00036 
00037 void doLinear(Vector startPos, Length axis, stepper* x, stepper* y, stepper* z, int speed){
00038     int fxy,fxz,x0,y0,z0 =0;
00039     int DX = (startPos.x+axis.longest)-startPos.x;
00040     if(DX<0)
00041         x0 = 0; //towards motor
00042     else
00043         x0 = 1; //away from motor
00044     DX = abs(DX);
00045     
00046     int DY = (startPos.y+axis.other1)-startPos.y;
00047     if(DY<0)
00048         y0 = 0; //towards motor
00049     else
00050         y0 = 1; //away from motor
00051     DY = abs(DY);
00052     fxy = DX-DY;
00053     
00054     int DZ = (startPos.z+axis.other2)-startPos.z;
00055     if(DZ<0)
00056         z0 = 0; //towards motor
00057     else
00058         z0 = 1; //away from motor
00059     DZ = abs(DZ);
00060     fxz = DX-DZ;
00061     
00062     
00063     Vector currentPos;
00064     currentPos.x=0;
00065     currentPos.y=0;
00066     currentPos.z=0;
00067     
00068     while((currentPos.x!=DX)||(currentPos.y!=DY)||(currentPos.z!=DZ)){
00069         //pc.printf("%d\t%d\t%d\r\n",currentPos.x,currentPos.y,currentPos.z);
00070         ++currentPos.x;
00071         x->step(1,x0,speed);
00072         fxy -= DY;
00073         fxz -= DZ;
00074         if(fxy<=0){
00075             ++currentPos.y;
00076             y->step(1,y0,speed);
00077             fxy += DX;
00078         }
00079         if(fxz<=0){
00080             ++currentPos.z;
00081             zEnable = 0;
00082             z->step(1,z0,speed);
00083             fxz += DX;
00084         }
00085         wait_us(1);
00086         x->stepOff();
00087         y->stepOff();
00088         z->stepOff();
00089         wait_us(speed);
00090     }
00091 }
00092             
00093 void order(Vector startPos, Vector finalPos,int speed){
00094     Vector length;
00095     long tempLength;
00096     stepper * tempStep;
00097     
00098     length.x = finalPos.x - startPos.x;
00099     length.y = finalPos.y - startPos.y;
00100     length.z = finalPos.z - startPos.z;
00101     stepper * stepMotor[3];
00102     stepMotor[0] = &x;
00103     stepMotor[1] = &y;
00104     stepMotor[2] = &z;
00105     if (abs(length.y) > abs(length.x)) {
00106         tempLength = length.y;
00107         length.y = tempLength;
00108         length.x = tempLength;
00109         tempStep = stepMotor[1];
00110         stepMotor[1] = stepMotor[0];
00111         stepMotor[0] = tempStep;
00112     }
00113     if (abs(length.z) > abs(length.x)) {
00114         tempLength = length.z;
00115         length.z = length.x;
00116         length.x = tempLength;
00117         tempStep = stepMotor[2];
00118         stepMotor[2] = stepMotor[0];
00119         stepMotor[0] = tempStep;
00120     }    
00121     
00122     Length length1;
00123     length1.longest = length.x;
00124     length1.other1 = length.y;
00125     length1.other2 = length.z;  
00126     xEnable = 0;
00127     yEnable = 0;
00128     zEnable = 0;
00129     doLinear(startPos,length1,stepMotor[0],stepMotor[1],stepMotor[2],speed);
00130     xEnable = 1;
00131     yEnable = 1;
00132     zEnable = 1;
00133 }
00134         
00135 int main(){
00136     solenoid = 0;
00137     led = 1;
00138 
00139     Vector startPos;
00140     startPos.x = 0;
00141     startPos.y = 0;
00142     startPos.z = 0;
00143     
00144     Vector finalPos;
00145     finalPos.x = -321;
00146     finalPos.y = -64;
00147     finalPos.z = -960;
00148    
00149      
00150     //pc.printf("\r\nx\ty\tz\r\n");
00151     //pc.printf("%d\t%d\t%d\r\n",startPos.x,startPos.y,startPos.z);
00152     
00153     order(startPos,finalPos,200);
00154     led = 0;
00155 }