Drive Subsystem prototype
Dependencies: Adafruit_MS_PWMServoDriver_LandDrone Adafruit_MotorShield_LandDrone
main.cpp
00001 #include "mbed.h" 00002 #include "Adafruit_MotorShield.h" 00003 00004 /******************************************************************************/ 00005 ///Adafruit MotorShield V2 Initialization/// 00006 00007 I2C i2c(PTE25, PTE24); //I2C for Motor Shield 00008 //Motor shield setup definition 00009 Adafruit_MotorShield AFMS = Adafruit_MotorShield(D14, D15, 0x60 << 1); 00010 //Stepper motor declarations 00011 Adafruit_StepperMotor *LM = AFMS.getStepper(200,1); //200 Step/Rev, Port 1 00012 Adafruit_StepperMotor *RM = AFMS.getStepper(200,2); //200 Step/Rev, Port 2 00013 00014 /******************************************************************************/ 00015 00016 /******************************************************************************/ 00017 ///GPIO Declarations/// 00018 00019 DigitalIn IRSensor (D0); 00020 00021 /******************************************************************************/ 00022 00023 /******************************************************************************/ 00024 ///Function Declarations/// 00025 00026 void Forward(uint16_t count, uint8_t speed); 00027 void Reverse(uint16_t count, uint8_t speed); 00028 void zeroTurnR(uint16_t degree, uint8_t speed); 00029 void zeroTurnL(uint16_t degree, uint8_t speed); 00030 void IRFwd(uint8_t speed); 00031 void IRRev(uint8_t speed); 00032 void Wait(uint16_t count); 00033 00034 /******************************************************************************/ 00035 00036 /******************************************************************************/ 00037 ///MAIN ROUTINE/// 00038 /******************************************************************************/ 00039 int main() 00040 { 00041 AFMS.begin(); // create with the default frequency 1.6KHz 00042 //AFMS.begin(1000); // OR with a different frequency, say 1KHz 00043 00044 // Set the speed (RPM) to start, from 0 (off) to 255 (max speed) 00045 RM->setSpeed(200); 00046 LM->setSpeed(200); 00047 00048 //Court Suicide Drill 00049 //First mark 00050 IRFwd(200); 00051 Reverse(5,200); //back off line 1 00052 IRRev(200); 00053 Forward(50,200); //come off "start" 00054 00055 //Second mark 00056 IRFwd(200); 00057 Forward(50,200); //pass line 1 00058 IRFwd(200); 00059 Reverse(5,200); //back off line 2 00060 IRRev(200); 00061 Reverse(50,200); //pass line 1 00062 IRRev(200); 00063 00064 /* 00065 //Third mark 00066 IRFwd(200); 00067 Forward(40,200); //pass line 1 00068 IRFwd(200); 00069 Forward(40,200); //pass line 2 00070 IRFwd(200); 00071 Reverse(5,200); //back off line 3 00072 IRRev(200); 00073 Reverse(40,200); //pass line 2 00074 IRRev(200); 00075 Reverse(40,200); //pass line 1 00076 IRRev(200); 00077 */ 00078 } 00079 /******************************************************************************/ 00080 00081 /******************************************************************************* 00082 ///Forward/// 00083 Expects: 16 bit unsigned integer "count" 00084 8 bit unsigned integer "speed" 00085 Returns: void 00086 Definition: 00087 Forward runs a for loop to actuate right anf left stepper motors at "speed" RPM 00088 for "count" *2 steps. 00089 *******************************************************************************/ 00090 void Forward(uint16_t count, uint8_t speed) 00091 { 00092 uint16_t i = 0; 00093 00094 //Set the speed of each stepper to "speed" RPM 00095 RM -> setSpeed(speed); 00096 LM -> setSpeed(speed); 00097 00098 for(i = 0; i < count; i++) 00099 { 00100 RM->step(2, FORWARD, SINGLE); 00101 LM->step(2, BACKWARD, SINGLE); 00102 } 00103 } 00104 /******************************************************************************/ 00105 00106 /******************************************************************************* 00107 ///Reverse/// 00108 Expects: 16 bit unsigned integer "count" 00109 8 bit unsigned integer "speed" 00110 Returns: void 00111 Definition: 00112 Reverse runs a for loop to actuate right anf left stepper motors at "speed" RPM 00113 for "count" *2 steps. 00114 *******************************************************************************/ 00115 void Reverse(uint16_t count, uint8_t speed) 00116 { 00117 uint16_t i = 0; 00118 //Set the speed of each stepper to "speed" RPM 00119 RM -> setSpeed(speed); 00120 LM -> setSpeed(speed); 00121 00122 for(i = 0; i < count; i++) 00123 { 00124 RM->step(2, BACKWARD, SINGLE); 00125 LM->step(2, FORWARD, SINGLE); 00126 } 00127 } 00128 /******************************************************************************/ 00129 00130 /******************************************************************************* 00131 ///ZeroTurnR/// 00132 Expects: 16 bit unsigned integer "degree" 00133 8 bit unsigned integer "speed" 00134 Returns: void 00135 Definition: 00136 ZeroTurnR runs a for loop to actuate right and left stepper motors at "speed" 00137 RPM for "count" *2 steps. "count" is determined through an equation: 00138 count = platformRadius*(degree/180*pi)/(wheelCircumference/100) 00139 platformRadius = 3.5" 00140 wheelCircumference = 7.46" 00141 The platform will turn in the Counter-clockwise 00142 direction. 00143 *******************************************************************************/ 00144 void zeroTurnR(uint16_t degree, uint8_t speed) 00145 { 00146 uint16_t i = 0; 00147 uint16_t count = 0; 00148 00149 //Set the speed of each stepper to "speed" RPM 00150 RM -> setSpeed(speed); 00151 LM -> setSpeed(speed); 00152 00153 //Calcualte count 00154 //count = 3.5*(degree/180*pi)/(7.46/100) 00155 count = 0.81885*degree; 00156 00157 for(i = 0; i < count; i++) 00158 { 00159 RM->step(2, FORWARD, SINGLE); 00160 LM->step(2, FORWARD, SINGLE); 00161 } 00162 } 00163 /******************************************************************************/ 00164 00165 /******************************************************************************* 00166 ///ZeroTurnL/// 00167 Expects: 16 bit unsigned integer "degree" 00168 8 bit unsigned integer "speed" 00169 Returns: void 00170 Definition: 00171 ZeroTurnR runs a for loop to actuate right and left stepper motors at "speed" 00172 RPM for "count" *2 steps. "count" is determined through an equation: 00173 count = platformRadius*(degree/180*pi)/(wheelCircumference/100) 00174 platformRadius = 3.5" 00175 wheelCircumference = 7.46" 00176 The platform will turn in the Clockwise 00177 direction. 00178 *******************************************************************************/ 00179 void zeroTurnL(uint16_t degree, uint8_t speed) 00180 { 00181 uint16_t i = 0; 00182 uint16_t count = 0; 00183 00184 //Set the speed of each stepper to "speed" RPM 00185 RM -> setSpeed(speed); 00186 LM -> setSpeed(speed); 00187 00188 //Calcualte count 00189 //count = 3.5*(degree/180*pi)/(7.46/100) 00190 count = 0.81885*degree; 00191 00192 for(i = 0; i < count; i++) 00193 { 00194 RM->step(2, BACKWARD, SINGLE); 00195 LM->step(2, BACKWARD, SINGLE); 00196 } 00197 } 00198 /******************************************************************************/ 00199 00200 /******************************************************************************* 00201 ///IRFwd/// 00202 Expects: 8 bit unsigned integer "speed" 00203 Returns: void 00204 Definition: 00205 IRFwd runs a for loop to actuate right and left stepper motors at "speed" 00206 RPM until IRSensor(D0) is made. 00207 *******************************************************************************/ 00208 void IRFwd(uint8_t speed) 00209 { 00210 //Set the speed of each stepper to "speed" RPM 00211 RM -> setSpeed(speed); 00212 LM -> setSpeed(speed); 00213 00214 while (!IRSensor) 00215 { 00216 RM->step(2, FORWARD, SINGLE); 00217 LM->step(2, BACKWARD, SINGLE); 00218 } 00219 } 00220 /******************************************************************************/ 00221 00222 /******************************************************************************* 00223 ///IRRev/// 00224 Expects: 8 bit unsigned integer "speed" 00225 Returns: void 00226 Definition: 00227 IRFwd runs a for loop to actuate right and left stepper motors at "speed" 00228 RPM until IRSensor(D0) is made. 00229 *******************************************************************************/ 00230 void IRRev(uint8_t speed) 00231 { 00232 //Set the speed of each stepper to "speed" RPM 00233 RM -> setSpeed(speed); 00234 LM -> setSpeed(speed); 00235 00236 while (!IRSensor) 00237 { 00238 RM->step(2, BACKWARD, SINGLE); 00239 LM->step(2, FORWARD, SINGLE); 00240 } 00241 } 00242 /******************************************************************************/ 00243 00244 /******************************************************************************* 00245 ///Wait/// 00246 Expects: 16 bit unsigned integer "count" 00247 Returns: void 00248 Definition: 00249 Wait runs a for loop to delay for "count" cycles 00250 *******************************************************************************/ 00251 void Wait(uint16_t count) 00252 { 00253 uint16_t i = 0; 00254 00255 for(i = 0; i < count; i++) 00256 { 00257 00258 } 00259 } 00260 /******************************************************************************/
Generated on Mon Jul 25 2022 11:12:36 by
1.7.2