StepperDrive.cpp

00001 #include "mbed.h"
00002 #include "StepperDrive.h"
00003 #include <cmath> // fabs, floor, signbit
00004 
00005 StepperDrive::StepperDrive(Serial &pc1, PinName in1, PinName in2, bool in3, PinName in4, PinName in5, bool in6, float in7, float in8, float in9):pc(pc1), leftStep(in1), leftDir(in2), rightStep(in4), rightDir(in5)
00006 {
00007     wheelCircum=in7;
00008     wheelSepar=in8;
00009     invertLeft=in3;
00010     invertRight=in6;
00011     pit.attach_us(this, &StepperDrive::pitCallback, in9);
00012     moveComplete=true;
00013 
00014 }
00015 
00016 int StepperDrive::move(float distance, float angle)
00017 {
00018     if(moveComplete==false) //if there is a move in progress
00019     {                       //return
00020         return -1;
00021     }
00022 
00023     float stepDistance=wheelCircum/(200*Ustep);
00024     float d, dl, dr;
00025 
00026     if(distance==0 && angle!=0)
00027     {
00028         d=angle*wheelSepar/2;
00029         leftSteps=d/stepDistance;
00030         rightSteps=-1*leftSteps;
00031         leftStepsPC=(angle>0 ? -1:1);
00032         rightStepsPC=-1*leftStepsPC;
00033         moveComplete=false;
00034     }
00035     else if(angle==0 && distance!=0)
00036     {
00037         leftSteps=distance/stepDistance;
00038         rightSteps=leftSteps;
00039         leftStepsPC=-1;
00040         rightStepsPC=-1;
00041         moveComplete=false;
00042     }
00043     else if(angle>0 && distance!=0)
00044     {
00045         dl=angle*(distance/angle+wheelSepar/2);
00046         dr=angle*(distance/angle-wheelSepar/2);
00047 
00048         leftSteps=dl/stepDistance;
00049         rightSteps=dr/stepDistance;
00050         leftStepsPC=-1;
00051         rightStepsPC=-1*(dr/dl);
00052         moveComplete=false;
00053     }
00054     else if(angle<0 && distance!=0)
00055     {
00056         dl=angle*(distance/angle-wheelSepar/2);
00057         dr=angle*(distance/angle+wheelSepar/2);
00058 
00059         leftSteps=dl/stepDistance;
00060         rightSteps=dr/stepDistance;
00061         leftStepsPC=-1*(dl/dr);
00062         rightStepsPC=-1;
00063         moveComplete=false;
00064     }
00065     leftError=0;
00066     rightError=0;
00067     return 0;
00068 }
00069 
00070 void StepperDrive::pitCallback()
00071 {
00072     if(moveComplete==true)
00073     {
00074         return;
00075     }
00076     else
00077     {
00078         if(leftSteps!=0)
00079         {
00080             leftError+=(fabs(leftStepsPC)-floor(fabs(leftStepsPC)));
00081             for(int i=0; i<floor(fabs(leftStepsPC)+leftError); i++)
00082             {
00083                 stepLeft(leftStepsPC>0);
00084             }
00085             if(signbit(leftSteps)!=signbit(leftSteps-floor(fabs(leftStepsPC)+leftError)))
00086             {
00087                 leftSteps=0;
00088             }
00089             if(leftError>=1)
00090             {
00091                 leftError-=1;
00092             }
00093         }
00094         if(rightSteps!=0)
00095         {
00096             rightError+=(fabs(rightStepsPC)-floor(fabs(rightStepsPC)));
00097             for(int i=0; i<floor(fabs(rightStepsPC)+rightError); i++)
00098             {
00099                 stepRight(rightStepsPC>0);
00100             }
00101             if(signbit(rightSteps)!=signbit(rightSteps-floor(fabs(rightStepsPC)+rightError)))
00102             {
00103                 rightSteps=0;
00104             }
00105             if(rightError>=1)
00106             {
00107                 rightError-=1;
00108             }
00109         }
00110         if(leftSteps==0 && rightSteps==0)
00111         {
00112             moveComplete=true;
00113         }
00114     }
00115 }
00116 
00117 void StepperDrive::stepLeft(bool dir)
00118 {
00119     leftDir=(invertLeft^dir);
00120     leftStep=1;
00121     wait_us(3);
00122     leftStep=0;
00123 
00124     /* completed 1 step (increment or decrement */
00125     if(leftSteps < 0)
00126         leftSteps++;
00127     else
00128         leftSteps--;
00129 }
00130 
00131 void StepperDrive::stepRight(bool dir)
00132 {
00133     rightDir=(invertRight^dir);
00134     rightStep=1;
00135     wait_us(3);
00136     rightStep=0;
00137 
00138     /* completed 1 step (increment or decrement */
00139     if(rightSteps < 0)
00140         rightSteps++;
00141     else
00142         rightSteps--;
00143 }
00144 
00145 bool StepperDrive::isMoveDone()
00146 {
00147     return moveComplete;
00148 }
00149 
00150 // FUNCTION:
00151 //      void pauseMove()
00152 // IN-PARAMETERS:
00153 //      None
00154 // OUT-PARAMETERS:
00155 //      None
00156 // DESCRIPTION:
00157 //      Stops current move. Saves remaining steps in leftStepsPause
00158 //      and rightStepsPause.
00159 void StepperDrive::pauseMove();
00160 {
00161     /* need to implement*/
00162 }
00163 
00164 // FUNCTION:
00165 //      void resumeMove()
00166 // IN-PARAMETERS:
00167 //      None
00168 // OUT-PARAMETERS:
00169 //      None
00170 // DESCRIPTION:
00171 //      Restores remaining steps from leftStepsPause
00172 //      and rightStepsPause to leftSteps and rightSteps. Resumes
00173 //      move.
00174 void StepperDrive::resumeMove();
00175 {
00176     /* need to implement */
00177 }