Drive Subsystem prototype

Dependencies:   Adafruit_MS_PWMServoDriver_LandDrone Adafruit_MotorShield_LandDrone

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 /******************************************************************************/