Diff: main.cpp

Revision:

1:4b76a9beeae6

Parent:

0:8239206a2f26

Child:

3:130a982ced94

--- a/main.cpp	Tue Oct 20 03:01:16 2015 +0000
+++ b/main.cpp	Fri Oct 23 14:01:30 2015 +0000
@@ -1,17 +1,17 @@
 // Hello World to sweep a servo through its full range
-
+ 
 #include "mbed.h"
 #include "Servo.h"
 #include "Motor.h"
 #include "LSM9DS0.h"
-
+ 
 // SDO_XM and SDO_G are pulled up, so our addresses are:
 #define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
 #define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW
-
+ 
 // refresh time. set to 500 for checking the Data on pc.
 #define REFRESH_TIME_MS 500
-
+ 
 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);//IMU, read the compass value. 
 Serial pc(USBTX, USBRX);//Only use this during the testing phase for pc debug.
 Motor MR(p25, p6, p5); // Motor A pwm, fwd, rev
@@ -20,115 +20,109 @@
 Servo myservo(p21);//PWM control for Servo
 AnalogIn IR(p20); //IR sensor
 AnalogIn LeftEncoder(p19);
-
+ 
+//Return direction from servo - left, right or front
+enum DIRC{LEFT,RIGHT,FRONT};
+ 
+//Control Motor to turn right.
 void TurnRight()
 {
-    //imu.readMag();
-    /*float current_dir = imu.calcHeading();
-    if (current_dir>=270)
-    {
-        current_dir -=360;
-    }
     
-    float target_dir = current_dir + 90;
-    */
-    
-      //pc.printf("%.2f",current_dir);
-      //pc.printf("       Targe:    %.2f\n",target_dir);
       ML.speed(-0.3);
       MR.speed(0.3);
-      //imu.readMag();
-      
-      //current_dir = imu.calcHeading();
-      
-      /*if (current_dir>=270)
-      {
-            current_dir -=360;
-      }*/
       wait(2.1);
      
-      ML.speed(0.0);
-      MR.speed(0.0);
 }    
-
+ 
+//Control Motor to turn left. 
 void TurnLeft()
 {
     
       ML.speed(0.3);
       MR.speed(-0.3);
-    
       wait(2.1);
      
-      ML.speed(0.0);
-      MR.speed(0.0);
 }    
-
-void Straight()
+ 
+//Control Motor to go forward.
+void Forward()
 {
-    ML.speed(-0.5);
+    ML.speed(-0.55);
     MR.speed(-0.5);
     wait(0.01);
 }
-
-void LookLeft()
+ 
+//Control Motor to Backup.
+ void Backup()
+{
+     ML.speed(0.55);
+     MR.speed(0.5);
+}
+ 
+//Control the servo to TurnLeft.
+DIRC LookLeft()
 {
     myservo = 0.0;
-    
+    wait(1);
+    return LEFT;
 }
-
-void LookRight()
+ 
+//Control the servo to TurnRight.
+DIRC LookRight()
 {
     myservo = 0.9;
+    wait(1);
+    return RIGHT;
 }
-
-void LookStraight()
+ 
+//Control the servo to LookStraight.
+DIRC LookStraight()
 {
     myservo = 0.5;
+    wait(1);
+    return FRONT;
 }
-
+ 
 int main() {
          
-     STBY = 1;
-     pc.baud(9600);
-    imu.begin();
-    LookStraight();
-    wait(2.0);
+     STBY = 1;//enable both Motor
+     wait(1.0);
+     DIRC looking_dir;
+    
     while(true)
     {
-        if(IR>0.6)
+        Forward(); //Constantly go straight if there is no obstacle.
+ 
+        
+        while(1)//Control servo swing left and right, obtain IR value.
         {
-            LookLeft();
-            wait(2.0);
-            if(IR >0.6);
-                {
-                    LookRight();
-                    wait(2.0);
-                }
+            looking_dir = LookStraight();
+            if(IR>0.6) break;
+            looking_dir = LookLeft();
+            if(IR>0.6) break;
+            looking_dir = LookRight();
+            if(IR>0.6) break;
+ 
         }
- //           Straight();
-
-   //     wait(10.0);
-     //   TurnRight();
-       // wait(2.0);
-        //TurnLeft();
         
-        //LookLeft();
-        //wait(2);
-        //myservo =  0.5;
-        //imu.readMag();
-        //float heading_turn = imu.calcHeading();
-        //float Turn_Right   = heading + 90;
-        
-      //if((heading_turn - Turn_Right)<0.1)
-        //{
-      //    MA.speed(1.0f); MB.speed(1.0f); 
-        //  pc.printf("%.2f\n",LeftEncoder.read());
-      //  wait(5);
-      else
-        LookStraight();
-        wait(1.0);
+        //Base on IR value, make correct movement. 
+        if(looking_dir == FRONT)
+        {
+            Backup();
+            wait(2.0);
+            TurnRight();
+        }
+        else if(looking_dir == LEFT)
+        {
+            Backup();
+            wait(2.0);
+            TurnRight();
+        }
+        else
+        {
+            Backup();
+            wait(2.0);
+            TurnLeft();
+        }
     }
-
-      
-    
-}
+}
\ No newline at end of file