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 int leftdistancestarting= 0;
00012 int leftdistanceending= 0;
00013 //---------------------|
00014 // PIN INITIALIZATIONS |
00015 //---------------------|
00016 // Debug LED pin:
00017 //DigitalOut led(p25);
00018 DigitalIn sw(p20);
00019 Serial pc(USBTX,USBRX);
00020 // Setup Motor Driver pins:
00021 Motor Lfront(p21, p6, p5);  // PWM to p21, forward to p6, reverse to p5...
00022 Motor Rfront(p22, p8, p7);  // PWM to p22, forward to p8, reverse to p7...
00023 Motor Lback(p23, p10, p9);  // PWM to p23, forward to p10, reverse to p9...
00024 Motor Rback(p24, p12, p11);  // PWM to p24, forward to p12, reverse to p11...
00025 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
00026 
00027 Timer t;
00028 
00029 //-----------------------|
00030 // CLASS INITIALIZATIONS |
00031 //-----------------------|
00032 class mctrl {
00033 public:
00034     // Stop all motors:
00035     void stop(void) {
00036         Lfront.speed(0);
00037         Lback.speed(0);
00038         Rfront.speed(0);
00039         Rback.speed(0);
00040         wait(0.55);
00041     };
00042     
00043     // Go forward at constant speed:
00044     void fwd(void){
00045         //stop();
00046         
00047         Lfront.speed(1);
00048         Lback.speed(1);
00049         Rfront.speed(1);
00050         Rback.speed(1);
00051         wait(0.02);
00052     };
00053     
00054     // Reverse at constant speed:
00055     void rev(void){
00056         stop();
00057         Lfront.speed(-1);
00058         Lback.speed(-1);
00059         Rfront.speed(-1);
00060         Rback.speed(-1);
00061         wait(0.02);
00062     };
00063     
00064     // Turn left 90 degrees:
00065     void turnLeft(float currt){
00066         stop();
00067         while ((t.read()-currt) < 1.53) {
00068             Lfront.speed(-1);
00069             Lback.speed(-1);
00070             Rfront.speed(1);
00071             Rback.speed(1);
00072         }
00073         stop();
00074         //wait(0.02);
00075     };
00076     
00077     // Turn right 90 degrees:
00078     void turnRight(){
00079         stop();
00080         float degree = 0.0, angularV = 0.0;
00081         float currt = t.read();
00082         /*while ((t.read()-currt) < 1.56) {
00083             Lfront.speed(1);
00084             Lback.speed(1);
00085             Rfront.speed(-1);
00086             Rback.speed(-1);
00087         }*/
00088         while(degree < 90) {
00089             while(imu.gyroAvailable());
00090             imu.readGyro();
00091             angularV = imu.gz;
00092             Lfront.speed(1);
00093             Lback.speed(1);
00094             Rfront.speed(-1);
00095             Rback.speed(-1);
00096             if(angularV > 50.0 || angularV <-50.0) {
00097                 degree += (abs(angularV))/100.00;
00098             }
00099             wait(.4);
00100         }
00101         stop();
00102         wait(0.02);
00103     };
00104     
00105     
00106             
00107         
00108 } mctrl;
00109 
00110 //------------------|
00111 // HELPER FUNCTIONS |
00112 //------------------|
00113 void dist(int distance)  // NOTE: by default "distance" is in mm...
00114 {
00115     if (distance < 150) {
00116         
00117         mctrl.stop();
00118     // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
00119     // 1) Turn 90 degrees (hardcoded => 1.4 seconds).
00120     // 2) Go forward 1.5 seconds.
00121     // 3) Turn 90 degrees (hardcoded => 1.4 seconds).
00122     // 4) Continue forward until wall.
00123     
00124             
00125         // [Step 1]
00126         float currt = t.read();
00127         if (tr) {mctrl.turnRight();} else {mctrl.turnLeft(currt);}
00128         // [Step 2]
00129         currt = t.read();
00130         while ((t.read()-currt) < 1) {mctrl.fwd();}
00131         // [Step 3]
00132         currt = t.read();
00133         if (tr) {mctrl.turnRight();} else {mctrl.turnLeft(currt);}
00134         // [End]
00135         tr = !tr;
00136     }
00137     else {
00138         mctrl.fwd();
00139         
00140     }
00141 }
00142 
00143 void dist2(int distance)   // left sensor interrupt
00144 {
00145     
00146      leftdistanceending = distance; 
00147     //pc.printf(" Distance %d mm\r\n", distance);
00148 
00149 
00150 }
00151 
00152 void dist3(int distance)  // right sensor interrupt
00153 {
00154     /*if (distance < 150) {
00155      pc.printf("Right Sensor trigg"); 
00156     }
00157     else {
00158         pc.printf("Right Sensor echo");
00159     }*/
00160     //rightdistance = distance;
00161 }
00162 
00163 
00164 //------------------------------|
00165 // PIN INITIALIZATIONS (cont'd) |
00166 //------------------------------|
00167 // Setup Ultrasonic Sensor pins:
00168 ultrasonic usensor(p15, p16, .07, 1, &dist);  // trigger to p13, echo to p14...
00169                                              // update every .07 secs w/ timeout after 1 sec...
00170                                               // call "dist" when the distance changes...
00171 ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
00172 ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
00173 
00174 
00175 //---------------|
00176 // MAIN FUNCTION |
00177 //---------------|
00178 int main() {
00179     // Use internal pullup for the switch:
00180     sw.mode(PullUp);
00181     // Initialize the Ultrasonic Sensor:
00182     usensor.startUpdates();
00183     usensor2.startUpdates();
00184     usensor3.startUpdates(); 
00185     wait(0.5);
00186     int leftdistancestarting = usensor2.getCurrentDistance();
00187     wait(0.5);
00188     pc.printf("Current Starting Distance %d mm\r\n", leftdistancestarting);
00189 
00190     // Initialize the Timer:
00191     t.start();
00192     
00193    while(1) {
00194         while (!sw)
00195         {
00196     
00197         usensor.checkDistance();
00198         usensor2.checkDistance();
00199         usensor3.checkDistance();
00200         pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending);
00201 
00202         }
00203         mctrl.stop();
00204 
00205     }
00206 }
00207