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 frontdistance = 0;
00013 int leftdistanceending = 0;
00014 //---------------------|
00015 // PIN INITIALIZATIONS |
00016 //---------------------|
00017 // Debug LED pin:
00018 //DigitalOut led(p25);
00019 DigitalIn sw(p20);
00020 Serial pc(USBTX,USBRX);
00021 // Setup Motor Driver pins:
00022 Motor Lfront(p21, p6, p5);  // PWM to p21, forward to p5, reverse to p6...
00023 Motor Rfront(p22, p8, p7);  // PWM to p22, forward to p7, reverse to p8...
00024 Motor Lback(p23, p10, p9);  // PWM to p23, forward to p10, reverse to p9...
00025 Motor Rback(p24, p12, p11);  // PWM to p24, forward to p12, reverse to p11...
00026 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
00027 
00028 Timer t;
00029 
00030 //-----------------------|
00031 // CLASS INITIALIZATIONS |
00032 //-----------------------|
00033 class mctrl {
00034 public:
00035     // Stop all motors:
00036     void stop(void) {
00037         Lfront.speed(0);
00038         Lback.speed(0);
00039         Rfront.speed(0);
00040         Rback.speed(0);
00041         wait(0.55);
00042     };
00043     
00044     // Go forward at constant speed:
00045     void fwd(void){
00046         //stop();
00047         
00048         Lfront.speed(0.85);
00049         Lback.speed(0.85);
00050         Rfront.speed(0.85);
00051         Rback.speed(0.85);
00052         wait(0.02);
00053     };
00054     
00055     // Reverse at constant speed:
00056     void rev(void){
00057         stop();
00058         Lfront.speed(-1);
00059         Lback.speed(-1);
00060         Rfront.speed(-1);
00061         Rback.speed(-1);
00062         wait(0.02);
00063     };
00064     
00065     // Turn left 90 degrees:
00066     void turnLeft(){
00067         stop();
00068         float degree = 0.0, angularV = 0.0;
00069         float currt = t.read();
00070         while(degree < 90) {
00071             while(imu.gyroAvailable());
00072             imu.readGyro();
00073             angularV = imu.gz;
00074             Lfront.speed(-1);
00075             Lback.speed(-1);
00076             Rfront.speed(1);
00077             Rback.speed(1);
00078             if(angularV > 50.0 || angularV <-50.0) {
00079                 degree += (abs(angularV))/100.00;
00080             }
00081             wait(.45);
00082         }
00083         stop();
00084         wait(0.02);
00085     };
00086     
00087     // Turn right 90 degrees:
00088     void turnRight(){
00089         stop();
00090         float degree = 0.0, angularV = 0.0;
00091         float currt = t.read();
00092         while(degree < 90) {
00093             while(imu.gyroAvailable());
00094             imu.readGyro();
00095             angularV = imu.gz;
00096             Lfront.speed(1);
00097             Lback.speed(1);
00098             Rfront.speed(-1);
00099             Rback.speed(-1);
00100             if(angularV > 50.0 || angularV <-50.0) {
00101                 degree += (abs(angularV))/100.00;
00102             }
00103             wait(.45);
00104         }
00105         stop();
00106         wait(0.02);
00107     };
00108     
00109     // Turn L or R, "mag" of turn prop. to dist. from wall
00110     void turnLeftScaled(double scale) {
00111         stop();
00112         Lfront.speed(-1 * scale);
00113         Lback.speed(-1 * scale);
00114         Rfront.speed(1 * scale);
00115         Rback.speed(1 * scale);
00116         //stop();
00117         wait(0.02);
00118     };
00119         
00120     void turnRightScaled(double scale) {
00121         stop();
00122         Lfront.speed(1 * scale);
00123         Lback.speed(1 * scale);
00124         Rfront.speed(-1 * scale);
00125         Rback.speed(-1 * scale);
00126         //stop();
00127         wait(0.02);
00128     };
00129 } mctrl;
00130 
00131 void dist(int distance) {
00132     frontdistance = distance;
00133 }
00134 void dist2(int distance)   // left sensor interrupt
00135 {
00136     leftdistanceending = distance;
00137 }
00138 void dist3(int distance)  // right sensor interrupt
00139 {
00140     /*if (distance < 150) {
00141      pc.printf("Right Sensor trigg"); 
00142     }
00143     else {
00144         pc.printf("Right Sensor echo");
00145     }*/
00146     //rightdistance = distance;
00147 }
00148 //------------------|
00149 // HELPER FUNCTIONS |
00150 //------------------|
00151 
00152 //------------------------------|
00153 // PIN INITIALIZATIONS (cont'd) |
00154 //------------------------------|
00155 // Setup Ultrasonic Sensor pins:
00156 ultrasonic usensor(p15, p16, .07, 1, &dist);  // trigger to p13, echo to p14...
00157 ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
00158 ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
00159 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm 
00160 void checkcollision()  // NOTE: by default "distance" is in mm...
00161 {
00162     if (frontdistance < 150) {
00163         
00164         mctrl.stop();
00165 
00166         // Turn 90 degrees
00167         if (tr) {
00168             mctrl.turnRight();
00169         } else {
00170             mctrl.turnLeft();
00171         }
00172         
00173         // Go forward X (1?) number of seconds.
00174         // change dstance???? globals??
00175         float currt = t.read();
00176         usensor.startUpdates();
00177         wait(.5);
00178         usensor.checkDistance();
00179         while (((t.read() - currt) < .75) && frontdistance > 150) {
00180             mctrl.fwd();
00181             usensor.startUpdates();
00182             wait(.5);
00183             usensor.checkDistance();
00184         }
00185         // Turn 90 degrees
00186         if (tr) {
00187             mctrl.turnRight();
00188         } else {
00189             mctrl.turnLeft();
00190         }
00191 
00192         tr = !tr;
00193     } else {
00194         // Continue forward until wall.
00195         mctrl.fwd();  
00196     }
00197 }
00198 
00199 //---------------|
00200 // MAIN FUNCTION |
00201 //---------------|
00202 int main() {
00203     // Use internal pullup for the switch:
00204     sw.mode(PullUp);
00205     
00206     // Initialize the Ultrasonic Sensor:
00207     usensor.startUpdates();
00208     usensor2.startUpdates();
00209     usensor3.startUpdates(); 
00210     wait(0.5);
00211     
00212     // obtain and print starting distances
00213     int forwardDist = usensor.getCurrentDistance();
00214     int leftdistancestarting = usensor2.getCurrentDistance();
00215     int rightDist = usensor3.getCurrentDistance();
00216     wait(0.5);
00217     
00218     pc.printf("Current Forward Starting Distance %d mm\r\n", forwardDist);
00219     pc.printf("Current Left Starting Distance %d mm\r\n", leftdistancestarting);
00220     pc.printf("Current Right Starting Distance %d mm\r\n", rightDist);
00221 
00222     // Initialize the Timer:
00223     t.start();
00224     
00225    //while(1) {
00226         while (!sw) {
00227             usensor.checkDistance();
00228             checkcollision();            
00229             //determine which sensor is closest to wall
00230             int currLeftDist = usensor2.getCurrentDistance();
00231             int currRightDist = usensor3.getCurrentDistance();
00232             
00233             // may also have to scale the difference (40, etc.) based on - 
00234             // iteration of the 90deg turn sequence (loss of accuracy)
00235             // or decrease numbers of turns
00236             
00237             //if left sensor closest to wall
00238             if (currLeftDist > 100 && currRightDist > 100) {
00239                 //mctrl.rev();
00240                 mctrl.fwd();
00241             } else {
00242             if (currLeftDist < currRightDist) {
00243                 //if robot has drifted toward the wall by a lot, scale R turn prop.
00244                 if ((leftdistancestarting - currLeftDist) > 40) {
00245                     mctrl.turnRightScaled(1);
00246                     mctrl.fwd();
00247                 //if drifted toward wall a little, scale accordingly
00248                 } else if ((leftdistancestarting - currLeftDist) > 5) {
00249                     mctrl.turnRightScaled(0.75);
00250                     mctrl.fwd();
00251                 //if drift away a lot, scale L turn accordingly
00252                 } else if ((currLeftDist - leftdistancestarting) > 40) {
00253                     mctrl.turnLeftScaled(1);
00254                     mctrl.fwd();
00255                 } else if ((currLeftDist - leftdistancestarting) > 5) {
00256                     mctrl.turnLeftScaled(0.75);
00257                     mctrl.fwd();
00258                 } //else {
00259 //                    mctrl.fwd();
00260 //                }
00261             } else if (currLeftDist > currRightDist) {
00262                 //if robot has drifted toward the wall by a lot, scale L turn by a lot
00263                 if ((rightDist - currRightDist) > 40) {
00264                     mctrl.turnLeftScaled(1);
00265                     mctrl.fwd();
00266                 //if drifted toward wall a little, scale accordingly
00267                 } else if ((rightDist - currRightDist) > 5) {
00268                     mctrl.turnLeftScaled(0.75);
00269                     mctrl.fwd();
00270                 //if drift away a lot, scale R turn accordingly
00271                 } else if ((currRightDist - rightDist) > 40) {
00272                     mctrl.turnRightScaled(1);
00273                     mctrl.fwd();
00274                 } else if ((currRightDist - rightDist) > 5) {
00275                     mctrl.turnRightScaled(0.75);
00276                     mctrl.fwd();
00277                 } 
00278             }
00279             // may be weird when updating after turning? maybe wait/etc.
00280             leftdistancestarting = currLeftDist;
00281             rightDist = currRightDist;
00282             wait(.03);
00283             }       
00284         }
00285         mctrl.stop();
00286 }
00287