Diff: main.cpp

Revision:

3:130a982ced94

Parent:

1:4b76a9beeae6

diff -r 4b76a9beeae6 -r 130a982ced94 main.cpp
--- a/main.cpp	Fri Oct 23 14:01:30 2015 +0000
+++ b/main.cpp	Sat Nov 21 17:34:19 2015 +0000
@@ -1,28 +1,29 @@
 // Hello World to sweep a servo through its full range
  
 #include "mbed.h"
-#include "Servo.h"
+//#include "Servo.h"
 #include "Motor.h"
-#include "LSM9DS0.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
+//#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
+//#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.
+//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
 Motor ML(p26, p7, p8);//Motor B pwm, fwd, rev
 DigitalOut STBY(p11); // Set STBY = 1, enable both motor; Set STBY = 0; disable both motor. 
-Servo myservo(p21);//PWM control for Servo
-AnalogIn IR(p20); //IR sensor
+//Servo myservo(p21);//PWM control for Servo
+AnalogIn IR(p); //IR sensor
 AnalogIn LeftEncoder(p19);
+AnalogIn RightEncoder(p20);
  
 //Return direction from servo - left, right or front
-enum DIRC{LEFT,RIGHT,FRONT};
+//enum DIRC{LEFT,RIGHT,FRONT};
  
 //Control Motor to turn right.
 void TurnRight()
@@ -30,7 +31,7 @@
     
       ML.speed(-0.3);
       MR.speed(0.3);
-      wait(2.1);
+      wait(3.0);
      
 }    
  
@@ -40,7 +41,7 @@
     
       ML.speed(0.3);
       MR.speed(-0.3);
-      wait(2.1);
+      wait(3.0);
      
 }    
  
@@ -48,7 +49,7 @@
 void Forward()
 {
     ML.speed(-0.55);
-    MR.speed(-0.5);
+    MR.speed(-0.55);
     wait(0.01);
 }
  
@@ -60,69 +61,39 @@
 }
  
 //Control the servo to TurnLeft.
-DIRC LookLeft()
-{
-    myservo = 0.0;
-    wait(1);
-    return LEFT;
-}
+//DIRC LookLeft()
+//{
+  //  myservo = 0.0;
+    //wait(1);
+    //return LEFT;
+//}
  
 //Control the servo to TurnRight.
-DIRC LookRight()
-{
-    myservo = 0.9;
-    wait(1);
-    return RIGHT;
-}
+//DIRC LookRight()
+//{
+  //  myservo = 0.9;
+    //wait(1);
+    //return RIGHT;
+//}
  
 //Control the servo to LookStraight.
-DIRC LookStraight()
-{
-    myservo = 0.5;
-    wait(1);
-    return FRONT;
-}
+//DIRC LookStraight()
+//{
+  //  myservo = 0.5;
+    //wait(1);
+    //return FRONT;
+//}
  
 int main() {
          
      STBY = 1;//enable both Motor
      wait(1.0);
-     DIRC looking_dir;
+  //   DIRC looking_dir;
     
     while(true)
     {
         Forward(); //Constantly go straight if there is no obstacle.
  
-        
-        while(1)//Control servo swing left and right, obtain IR value.
-        {
-            looking_dir = LookStraight();
-            if(IR>0.6) break;
-            looking_dir = LookLeft();
-            if(IR>0.6) break;
-            looking_dir = LookRight();
-            if(IR>0.6) break;
- 
-        }
+    }
         
-        //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