Diff: main.cpp

Revision:

2:26960ab3c751

Parent:

0:8239206a2f26

--- a/main.cpp	Tue Oct 20 03:01:16 2015 +0000
+++ b/main.cpp	Thu Oct 22 20:27:46 2015 +0000
@@ -1,4 +1,6 @@
-// Hello World to sweep a servo through its full range
+// Ojbective:
+// Sweep servo fullrange from left to right (with left having the priority) to determine the next move.
+// Robot will not move to obstacle closer than 10 cm, turn away instead
 
 #include "mbed.h"
 #include "Servo.h"
@@ -12,73 +14,121 @@
 // refresh time. set to 500 for checking the Data on pc.
 #define REFRESH_TIME_MS 500
 
+// IR reading of 10 cm converted to 0-1 scale
+#define WALL 0.7
+
 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
+DigitalOut STBY(p11); // Set STBY = 1, enable both motor; Set STBY = 0; disable both motor. 
 AnalogIn LeftEncoder(p19);
+AnalogIn RightEncoder(p18);
 
-void TurnRight()
+// Adjust wheel speed using rotary magnetic encoder
+void Adjust()
 {
-    //imu.readMag();
-    /*float current_dir = imu.calcHeading();
-    if (current_dir>=270)
+       //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);
+        float left = LeftEncoder.read();
+        float right = RightEncoder.read();
+        float diff = abs(left - right);
+        pc.printf("left: %.2f, right: %.2f, diff: %.2f\n", left, right, diff);
+        //TO DO: Utilize diff data to adjust speeds of two wheels
+}
+
+void TurnRight(float deg)
+{
+    imu.readMag();
+    float current_deg = imu.calcHeading();
+    if (current_deg >= 270)
     {
-        current_dir -=360;
+        current_deg -= 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);
+    //pc.printf("Initial degree: %.2f  ", current_deg);
+    float target_deg = current_deg + deg;
+    //pc.printf("\nTarget degree: %.2f\n", target_deg);
+    while(current_deg < target_deg)
+    {
+        ML.speed(-0.5);
+        MR.speed(0.5);
+        wait(0.001);
+        // update
+        imu.readMag();
+        current_deg = imu.calcHeading();
+        if (current_deg >= 270)
+        {
+            current_deg -=360;
+        }
+        //pc.printf("Current degree: %.2f  ", current_deg);
+    }
+    ML.speed(0.0);
+    MR.speed(0.0);
 }    
 
-void TurnLeft()
+void TurnLeft(float deg)
 {
-    
-      ML.speed(0.3);
-      MR.speed(-0.3);
-    
-      wait(2.1);
-     
-      ML.speed(0.0);
-      MR.speed(0.0);
+    imu.readMag();
+    float current_deg = imu.calcHeading();
+    if (current_deg <= 90)
+    {
+        current_deg += 360;
+    }
+    pc.printf("Initial degree: %.2f  ", current_deg);
+    float target_deg = current_deg - deg;
+    pc.printf("\nTarget degree: %.2f\n", target_deg);
+    while(current_deg > target_deg)
+    {
+        ML.speed(0.5);
+        MR.speed(-0.5);
+        wait(0.001);
+        // update
+        imu.readMag();
+        current_deg = imu.calcHeading();
+        if (current_deg <= 90)
+        {
+            current_deg += 360;
+        }
+        //pc.printf("Current degree: %.2f  ", current_deg);
+    }
+    ML.speed(0.0);
+    MR.speed(0.0);
 }    
 
-void Straight()
+void Backout()
 {
     ML.speed(-0.5);
     MR.speed(-0.5);
-    wait(0.01);
+    Adjust();
+    wait(5.0);
+}
+
+void Forward()
+{
+    ML.speed(0.5);
+    MR.speed(0.5);
+    Adjust();
+    wait(5.0);
 }
 
 void LookLeft()
 {
-    myservo = 0.0;
-    
+    myservo = myservo - 0.1;   
 }
 
 void LookRight()
 {
-    myservo = 0.9;
+    myservo = myservo + 0.1;
 }
 
 void LookStraight()
@@ -87,48 +137,66 @@
 }
 
 int main() {
-         
-     STBY = 1;
-     pc.baud(9600);
-    imu.begin();
+    //float deg;
+    STBY = 1;
+    pc.baud(9600);
+    uint16_t status = imu.begin();
+    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
+    pc.printf("Should be 0x49D4\n\n");
     LookStraight();
     wait(2.0);
+    //while(true)
+//    {
+//        TurnLeft(20.0);
+//    } // testing
+    /*
     while(true)
     {
-        if(IR>0.6)
+        if(IR >= WALL)
         {
-            LookLeft();
-            wait(2.0);
-            if(IR >0.6);
-                {
-                    LookRight();
-                    wait(2.0);
-                }
+            if (myservo <= 0.5 && myservo > 0.0)
+            { 
+                LookLeft();
+                wait(1.0);
+            } // Look further left
+            if (myservo > 0.5 && myservo < 1.0)
+            {
+                LookRight();
+                wait(1.0);
+            } // Look further right
+            if (myservo == 0.0)
+            {
+                LookStraight();
+                LookRight();
+                wait(1.0);
+            } // When reaches leftmost bound, reset to middle and start to look right
+            if (myservo == 1.0)
+            {
+                Backout();
+            } // When reaches rightmost bound, ie. sweeped the fullrange, backout
+            else
+            {
+                pc.printf("servo error.");
+            }
+            wait(5.0);
         }
- //           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);
+        else
+        {
+            // myservo has a range of 0.0 - 1.0 
+            if (myservo > 0.5)
+            {
+               deg = (myservo - 0.5) * 360;
+               TurnRight(deg);
+            }
+            else
+            {
+                deg = (0.5 - myservo) * 360;
+                TurnLeft(deg);
+            }
+            LookStraight();
+            Forward();
+            wait(1.0);
+        }
     }
-
-      
-    
+    */
 }