Diff: main.cpp

Revision:

0:8239206a2f26

Child:

1:4b76a9beeae6

Child:

2:26960ab3c751

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 20 03:01:16 2015 +0000
@@ -0,0 +1,134 @@
+// 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
+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
+AnalogIn LeftEncoder(p19);
+
+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);
+}    
+
+void TurnLeft()
+{
+    
+      ML.speed(0.3);
+      MR.speed(-0.3);
+    
+      wait(2.1);
+     
+      ML.speed(0.0);
+      MR.speed(0.0);
+}    
+
+void Straight()
+{
+    ML.speed(-0.5);
+    MR.speed(-0.5);
+    wait(0.01);
+}
+
+void LookLeft()
+{
+    myservo = 0.0;
+    
+}
+
+void LookRight()
+{
+    myservo = 0.9;
+}
+
+void LookStraight()
+{
+    myservo = 0.5;
+}
+
+int main() {
+         
+     STBY = 1;
+     pc.baud(9600);
+    imu.begin();
+    LookStraight();
+    wait(2.0);
+    while(true)
+    {
+        if(IR>0.6)
+        {
+            LookLeft();
+            wait(2.0);
+            if(IR >0.6);
+                {
+                    LookRight();
+                    wait(2.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);
+    }
+
+      
+    
+}