main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"    // IMU
00003 #include "ultrasonic.h" // Ultrasonic Sensor
00004 #include "Motor.h"      // Motor Drivers
00005 #include "PinDetect.h"  // Pin Detect (for switch)
00006 
00007 
00008 // Global Variables:
00009 bool tr = true;
00010 bool turnflag = false;
00011  
00012 int leftDist = 0;
00013 int rightDist = 0;
00014 int forwardDist = 0; 
00015 int leftForwardDist = 0;
00016 int rightForwardDist = 0;
00017 
00018 //---------------------|
00019 // PIN INITIALIZATIONS |
00020 //---------------------|
00021 // Debug LED pin:
00022 //DigitalOut led(p25);
00023 DigitalIn sw(p20);
00024 Serial pc(USBTX,USBRX);
00025 // Setup Motor Driver pins:
00026 Motor Lfront(p21, p6, p5);  // PWM to p21, forward to p5, reverse to p6...
00027 Motor Rfront(p22, p8, p7);  // PWM to p22, forward to p7, reverse to p8...
00028 Motor Lback(p23, p10, p9);  // PWM to p23, forward to p10, reverse to p9...
00029 Motor Rback(p24, p12, p11);  // PWM to p24, forward to p12, reverse to p11...
00030 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
00031 
00032 Timer t;
00033 
00034 //-----------------------|
00035 // CLASS INITIALIZATIONS |
00036 //-----------------------|
00037 class mctrl {
00038 public:
00039     // Stop all motors:
00040     void stop(void) {
00041         Lfront.speed(0);
00042         Lback.speed(0);
00043         Rfront.speed(0);
00044         Rback.speed(0);
00045         wait(0.55);
00046     };
00047     
00048     // Go forward at constant speed:
00049     void fwd(void){
00050         //stop();
00051         
00052         Lfront.speed(0.85);
00053         Lback.speed(0.85);
00054         Rfront.speed(0.85);
00055         Rback.speed(0.85);
00056         wait(0.02);
00057     };
00058     
00059     // Reverse at constant speed:
00060     void rev(void){
00061         stop();
00062         Lfront.speed(-1);
00063         Lback.speed(-1);
00064         Rfront.speed(-1);
00065         Rback.speed(-1);
00066         wait(0.25);
00067     };
00068     
00069     // Turn left 90 degrees:
00070     void turnLeft(){
00071         stop();
00072         float degree = 0.0, angularV = 0.0;
00073         float currt = t.read();
00074         while(degree < 90) {
00075             while(imu.gyroAvailable());
00076             imu.readGyro();
00077             angularV = imu.gz;
00078             Lfront.speed(1);
00079             Lback.speed(1);
00080             Rfront.speed(-1);
00081             Rback.speed(-1);
00082             if(angularV > 50.0 || angularV <-50.0) 
00083             {
00084                 degree += (abs(angularV))/100.00;
00085             }
00086             wait(.4);
00087         }
00088         stop();
00089         wait(0.02);
00090     };
00091     
00092     // Turn right 90 degrees:
00093     void turnRight(){
00094         stop();
00095         float degree = 0.0, angularV = 0.0;
00096         float currt = t.read();
00097         while(degree < 90) {
00098             while(imu.gyroAvailable());
00099             imu.readGyro();
00100             angularV = imu.gz;
00101             Lfront.speed(1);
00102             Lback.speed(1);
00103             Rfront.speed(-1);
00104             Rback.speed(-1);
00105             if(angularV > 50.0 || angularV <-50.0) {
00106                 degree += (abs(angularV))/100.00;
00107             }
00108             wait(.4);
00109         }
00110         stop();
00111         wait(0.02);
00112     };
00113     
00114     // Turn L or R, "mag" of turn prop. to dist. from wall
00115     // motor speed values are normalized to -1 to 1 (aka no 1.25)
00116     void turnLeftScaled(double scale) {
00117         // try commenting out this stop otherwise robot will stop before correcting
00118         stop();
00119         Lfront.speed(-1 * scale);
00120         Lback.speed(-1 * scale);
00121         Rfront.speed(1 * scale);
00122         Rback.speed(1 * scale);
00123         //stop();
00124         wait(0.02);
00125     };
00126         
00127     void turnRightScaled(double scale) {
00128         stop();
00129         Lfront.speed(1 * scale);
00130         Lback.speed(1 * scale);
00131         Rfront.speed(-1 * scale);
00132         Rback.speed(-1 * scale);
00133         //stop();
00134         wait(0.02);
00135     };
00136 } mctrl;
00137 
00138 //------------------|
00139 // HELPER FUNCTIONS |
00140 //------------------|
00141 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm 
00142 void dist(int distance)  // NOTE: by default "distance" is in mm...
00143 {
00144     if (distance < 200) {
00145         
00146         mctrl.stop();
00147         
00148         if ( leftDist < rightDist)
00149         {
00150             mctrl.turnRight();
00151             mctrl.stop();
00152             //mctrl.fwd(); 
00153             
00154         }
00155         
00156         else 
00157         {
00158             mctrl.turnLeft(); 
00159             mctrl.stop();
00160             //mctrl.fwd();
00161         }
00162     }
00163     
00164     else {
00165         mctrl.fwd();
00166     }
00167 };
00168 
00169 void dist2(int distance)   // left sensor interrupt
00170 {
00171     if (distance < 150)
00172     {
00173         mctrl.stop();
00174         mctrl.turnRight();
00175         mctrl.stop();
00176     }
00177     
00178     else
00179     {
00180 
00181         mctrl.fwd(); 
00182     }
00183 
00184 }
00185 
00186 void dist3(int distance)  // right sensor interrupt
00187 {
00188     if (distance < 150)
00189     {
00190         mctrl.stop();
00191         mctrl.turnLeft();
00192         mctrl.stop();
00193     }
00194     
00195     else
00196     {
00197         mctrl.fwd(); 
00198     }
00199 
00200 }
00201 
00202 void dist4(int distance) {
00203     if (distance < 130) {
00204         mctrl.stop();
00205         mctrl.turnRight();
00206         mctrl.stop();
00207     } 
00208     
00209     else {
00210         mctrl.fwd();
00211     }
00212 }
00213 
00214 void dist5(int distance) {
00215     if (distance < 130) {
00216         mctrl.stop();
00217         mctrl.turnLeft();
00218         mctrl.stop();
00219     } else {
00220         
00221         mctrl.fwd();
00222     }
00223 }
00224 
00225 //------------------------------|
00226 // PIN INITIALIZATIONS (cont'd) |
00227 //------------------------------|
00228 // Setup Ultrasonic Sensor pins:
00229 ultrasonic usensor(p15, p16, .07, 1, &dist);  // trigger to p13, echo to p14...
00230                                              // update every .07 secs w/ timeout after 1 sec...
00231                                               // call "dist" when the distance changes...
00232 ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
00233 ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
00234 
00235 // new sonars added
00236 ultrasonic usensor4(p15, p29, .07, 1, &dist4); //sonar to the left of the forward sensor
00237 ultrasonic usensor5(p15, p30, .07, 1, &dist5); //sonar to the right of the forward sensor
00238 
00239 
00240 //---------------|
00241 // MAIN FUNCTION |
00242 //---------------|
00243 int main() {
00244     // Use internal pullup for the switch:
00245     sw.mode(PullUp);
00246     
00247     // Initialize the Ultrasonic Sensor:
00248     usensor.startUpdates();
00249     usensor2.startUpdates();
00250     usensor3.startUpdates(); 
00251     usensor4.startUpdates();
00252     usensor5.startUpdates();
00253     wait(0.5);
00254     
00255     // obtain and print starting distances
00256     forwardDist = usensor.getCurrentDistance();
00257     leftDist = usensor2.getCurrentDistance();
00258     rightDist = usensor3.getCurrentDistance();
00259     leftForwardDist = usensor4.getCurrentDistance();
00260     rightForwardDist = usensor5.getCurrentDistance();
00261     wait(0.5);
00262 
00263     // Initialize the Timer:
00264     t.start();
00265     int starttime = t.read();
00266     
00267    while(1) {
00268         while (!sw && t.read() - starttime < 60) {
00269             
00270             
00271             
00272              usensor.checkDistance(); 
00273              usensor2.checkDistance();
00274              usensor3.checkDistance();
00275              usensor4.checkDistance();
00276              usensor5.checkDistance();
00277              forwardDist = usensor.getCurrentDistance();
00278              leftDist = usensor2.getCurrentDistance(); 
00279              rightDist = usensor3.getCurrentDistance();
00280              leftForwardDist = usensor4.getCurrentDistance();
00281              rightForwardDist = usensor5.getCurrentDistance(); 
00282              
00283              if ( leftForwardDist > 100 && rightForwardDist > 100 ) 
00284              {
00285              mctrl.fwd(); 
00286              }
00287              
00288              else{
00289                  
00290                  mctrl.rev();
00291                  mctrl.turnRight();
00292                  mctrl.turnRight();
00293                  mctrl.fwd();
00294                  
00295             }
00296              //rightForwardDist = usensor5.getCurrentDistance(); 
00297              /*
00298              pc.printf("fowardDist: %d\n ", usensor.getCurrentDistance());
00299              wait (1);
00300              pc.printf("LeftDist: %d\n", usensor2.getCurrentDistance());
00301              wait(1);
00302              pc.printf("RightDist: %d\n", usensor3.getCurrentDistance());
00303              wait(1);
00304              pc.printf("leftForwardDist: %d\n ", usensor4.getCurrentDistance());
00305              wait (1);
00306              pc.printf("rightForwardDist: %d\n", usensor5.getCurrentDistance());
00307              wait(1);*/
00308 
00309              
00310     
00311              
00312         }
00313         // if switch flipped, stop robot
00314         mctrl.stop();
00315     }
00316 
00317 }