Justin Jordan
/
R5_StepperDrive
update 1/27/16
Fork of R5_StepperDrive by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 27 2022 06:33:05 by 1.7.2