Diff: main.cpp

Revision:

8:198568d5746b

Parent:

7:cbccf5c5da6d

Child:

9:36807f7e58fb

--- a/main.cpp	Thu Apr 28 05:20:35 2016 +0000
+++ b/main.cpp	Sun May 01 23:56:41 2016 +0000
@@ -3,6 +3,8 @@
 #include "LSM9DS1.h"
 #define DECLINATION -4.94
 #define PI 3.14159
+#include "Motor.h"
+
 
 // Timer example: https://developer.mbed.org/handbook/Timer
 AnalogIn infraredR(p20); //Right infrared distance sensor
@@ -10,20 +12,18 @@
 AnalogIn infraredF(p17); //Front infrared distance sensor
 Serial pc(USBTX, USBRX);
 Timer t;
-char str[40];
-    
-float leftDist, rightDist, frontDist; //Distances from robot to obstacles
-float xaccel, yaccel, zaccel;   //Acceleration in the x, y, and z directions
-float xmag, ymag, zmag, head;         //Magnetic readings
-int ldist, rdist, fdist;
+char str[40];                           //buffer for xbee messages
+float maxspeed = 0.4;
+float minspeed = -0.4;
+float leftDist, rightDist, frontDist;   //Distances from robot to obstacles
+float xaccel, yaccel, zaccel;           //Acceleration in the x, y, and z directions
+float xmag, ymag, zmag, head;           //Magnetic readings
+int ldist, rdist, fdist;                
+Serial xbee(p28,p27);                   //xbee over serial
 
 //dual H bridge Polulu
-PwmOut PWMA(p21);           //connect p21 to PWMA
-PwmOut PWMB(p22);           //connect p22 to PWMB
-DigitalOut Ain1(p23);       //connect p23 to Ain1   
-DigitalOut Ain2(p24);       //connect p24 to Ain2
-
-
+Motor MotorRi(p21, p23, p24);
+Motor MotorLe(p22, p26, p25);
 
 
 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
@@ -55,26 +55,31 @@
 }
 
 void IMU_thread(void const *args) {
-    LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+    LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
     IMU.begin();
     if (!IMU.begin()) {
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
     IMU.calibrate(1);
+    pc.printf("calibrated accel, now do mag\r\n");
     IMU.calibrateMag(0);
+    pc.printf("calibrated mag\r\n");
     while(1) {
+        pc.printf("check accelAvailable\r\n");
         while(!IMU.accelAvailable());
+        //pc.printf("got accel\r\n");
         IMU.readAccel();
         while(!IMU.magAvailable(X_AXIS));
+        //pc.printf("got mag\r\n");
         IMU.readMag();
         xaccel = IMU.calcAccel(IMU.ax);
         yaccel = IMU.calcAccel(IMU.ay);
         zaccel = IMU.calcAccel(IMU.az);
-        pc.printf("%f\r\n",zaccel);
+        //pc.printf("%f\r\n",zaccel);
         xmag = IMU.calcMag(IMU.mx);
         ymag = IMU.calcMag(IMU.my);
         zmag = IMU.calcMag(IMU.mz);
-        printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        //printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
         Thread::wait(500); //Wait 1/2 second
     }    
 }
@@ -85,7 +90,6 @@
         leftDist = 80.0f - leftDist;
         ldist = (int)leftDist;
         Thread::wait(500);     //wait 1/2 second before updating
-        //pc.printf("Left distance %d\r\n", ldist);
         }
 }
 
@@ -94,9 +98,7 @@
         rightDist = infraredR * 80.0f;
         rightDist = 80.0f - rightDist;
         rdist = (int)rightDist;
-        rdist = (int)rightDist;
         Thread::wait(500);     //wait 1/2 second before updating
-        //pc.printf("Right distance %d\r\n", rdist);
         }
 }
 
@@ -106,17 +108,56 @@
         frontDist = 80.0f - frontDist;
         fdist = (int)frontDist;
         Thread::wait(500);     //wait 1/2 second before updating
-        pc.printf("Front distance %d\r\n", fdist);
         }
 }    
 
 void send_thread(void const *args) {
+    xbee.baud(9600);
     while (1) {
+        //stdio_mutex.lock();
         sprintf (str, "%0.6f,%0.6f,%0.6f,%d,%d,%d,%d\n", xaccel, zaccel, ymag, fdist, ldist, rdist, t.read_us());   //buffer data to be sent to computer
+        pc.printf(str);
+        //pc.printf("%0.6f\r\n", zmag);
+        xbee.printf(str);
+        //stdio_mutex.unlock();
         Thread::wait(500);
     }
 }        
 
+void motor_thread(void const *args) {
+    Thread::wait(5000);        //allow gyro and accel to calibrate on level surface
+    MotorRi.speed(0.4);
+    MotorLe.speed(-0.4);
+    Thread::wait(3000);         //allow mag to calibrate by spinning
+    while(1) {
+        if (fdist > 30) {                   //if there's room to go forward
+            if (ldist < 20) {
+                MotorRi.speed(0.1);
+                MotorLe.speed(0.4);
+                Thread::wait(80);
+            }
+            else if (rdist < 20) {
+                MotorRi.speed(0.4);
+                MotorLe.speed(0.1);
+                Thread::wait(80);
+            }
+            else {
+                MotorRi.speed(0.4);
+                MotorLe.speed(0.4);
+            }
+        }    
+        else if (rdist < ldist) {               //hard turn to the left
+            MotorRi.speed(0.4);
+            MotorLe.speed(-0.4); 
+            Thread::wait(200);         
+        }
+        else if (ldist < rdist) {               //hard turn to the right
+            MotorRi.speed(-0.4);
+            MotorLe.speed(0.4);   
+            Thread::wait(200);        
+        }
+    }
+}        
 
 int main() {
     //Test
@@ -125,10 +166,9 @@
     Thread right(right_thread);
     Thread front(front_thread);
     Thread IMU(IMU_thread);
+    Thread Mot(motor_thread);
     Thread send(send_thread);
-    Ain1=Ain2=1;                //enabling PWM
-    while(1) {
-        PWMA.write(0.5f);       //writing 50% duty cycle
-        PWMB.write(0.5f);       //to pwm A and B
-    }
+    while(1){  
+         //do stuff 
+        }
 }