Diff: main.cpp

Revision:

9:36807f7e58fb

Parent:

8:198568d5746b

Child:

11:805ef606a29d

--- a/main.cpp	Sun May 01 23:56:41 2016 +0000
+++ b/main.cpp	Mon May 02 04:33:43 2016 +0000
@@ -12,7 +12,7 @@
 AnalogIn infraredF(p17); //Front infrared distance sensor
 Serial pc(USBTX, USBRX);
 Timer t;
-char str[40];                           //buffer for xbee messages
+char str[60];                           //buffer for xbee messages
 float maxspeed = 0.4;
 float minspeed = -0.4;
 float leftDist, rightDist, frontDist;   //Distances from robot to obstacles
@@ -20,11 +20,19 @@
 float xmag, ymag, zmag, head;           //Magnetic readings
 int ldist, rdist, fdist;                
 Serial xbee(p28,p27);                   //xbee over serial
-
+Mutex stdio_mutex;
+float heading;
 //dual H bridge Polulu
 Motor MotorRi(p21, p23, p24);
 Motor MotorLe(p22, p26, p25);
+float ti = 0, timelast = 0, deltat;
 
+float imu[] = {0, 0, 0};
+float velX[] = {0, 0, 0};
+float velY[] = {0, 0, 0};
+float posX[] = {0, 0, 0};
+float posY[] = {0, 0, 0};
+float compass[] = {0, 0, 0};
 
 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
 {
@@ -32,7 +40,6 @@
     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
     mx = -mx;
-    float heading;
     if (my == 0.0)
         heading = (mx < 0.0) ? 180.0 : 0.0;
     else
@@ -51,7 +58,19 @@
 
     //pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
     //pc.printf("Magnetic Heading: %f degress\n\r",heading);
-    head = heading;
+    head = heading * PI / 180.0;
+}
+
+void shiftArrays() {
+    int i;
+    for (i = 2; i > 0; i--) {
+        posX[i] = posX[i - 1];
+        posY[i] = posY[i - 1];
+        velX[i] = velX[i - 1];
+        velY[i] = velY[i - 1];
+        imu[i] = imu[i - 1];
+        compass[i] = compass[i-1];
+    }
 }
 
 void IMU_thread(void const *args) {
@@ -61,11 +80,11 @@
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
     IMU.calibrate(1);
-    pc.printf("calibrated accel, now do mag\r\n");
+    //pc.printf("calibrated accel, now do mag\r\n");
     IMU.calibrateMag(0);
-    pc.printf("calibrated mag\r\n");
+    //pc.printf("calibrated mag\r\n");
     while(1) {
-        pc.printf("check accelAvailable\r\n");
+        //pc.printf("check accelAvailable\r\n");
         while(!IMU.accelAvailable());
         //pc.printf("got accel\r\n");
         IMU.readAccel();
@@ -73,14 +92,27 @@
         //pc.printf("got mag\r\n");
         IMU.readMag();
         xaccel = IMU.calcAccel(IMU.ax);
+        if (abs(xaccel) < 0.13)
+            xaccel = 0.0;
         yaccel = IMU.calcAccel(IMU.ay);
+        if (abs(yaccel) < 0.13)
+            yaccel = 0.0;
         zaccel = IMU.calcAccel(IMU.az);
+        if (abs(zaccel) < 0.13)
+            zaccel = 0.0;
         //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));
-        Thread::wait(500); //Wait 1/2 second
+        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));
+        shiftArrays();
+        imu[0] = xaccel;
+        compass[0] = head;
+        velX[0] += imu[0]*cos(compass[0])  * deltat;
+        velY[0] += imu[0]*sin(compass[0])  * deltat;
+        posX[0] += velX[0] * deltat;
+        posY[0] += velY[0] * deltat;
+       // Thread::wait(50); //Wait 
     }    
 }
 
@@ -114,12 +146,15 @@
 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
+        ti = t.read();
+        deltat = ti-timelast;
+        stdio_mutex.lock();
+        sprintf (str, "%0.6f,%0.6f,%0.6f,%d,%d,%d,%0.6f\n", posX[0], posY[0], heading, fdist, ldist, rdist, deltat);   //buffer data to be sent to computer
         pc.printf(str);
-        //pc.printf("%0.6f\r\n", zmag);
+        //pc.printf("%0.6f\r\n", zmag*9.8);
         xbee.printf(str);
-        //stdio_mutex.unlock();
+        stdio_mutex.unlock();
+        timelast = ti;
         Thread::wait(500);
     }
 }        
@@ -130,31 +165,31 @@
     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 (fdist > 40) {                   //if there's room to go forward
             if (ldist < 20) {
                 MotorRi.speed(0.1);
-                MotorLe.speed(0.4);
-                Thread::wait(80);
+                MotorLe.speed(0.3);
+                Thread::wait(40);
             }
             else if (rdist < 20) {
-                MotorRi.speed(0.4);
+                MotorRi.speed(0.3);
                 MotorLe.speed(0.1);
-                Thread::wait(80);
+                Thread::wait(40);
             }
             else {
-                MotorRi.speed(0.4);
-                MotorLe.speed(0.4);
+                MotorRi.speed(0.3);
+                MotorLe.speed(0.3);
             }
         }    
         else if (rdist < ldist) {               //hard turn to the left
-            MotorRi.speed(0.4);
-            MotorLe.speed(-0.4); 
-            Thread::wait(200);         
+            MotorRi.speed(0.3);
+            MotorLe.speed(-0.3); 
+            Thread::wait(100);         
         }
         else if (ldist < rdist) {               //hard turn to the right
             MotorRi.speed(-0.4);
             MotorLe.speed(0.4);   
-            Thread::wait(200);        
+            Thread::wait(100);        
         }
     }
 }