Revision 3:ffa0e1429a72, committed 2015-03-09

Comitter:

Spilly

Date:

Mon Mar 09 20:41:28 2015 +0000

Parent:

2:503a5ac6c3b6

Child:

4:a397b44a0fe8

Commit message:

gps

Changed in this revision

--- a/main.cpp	Wed Mar 04 17:48:08 2015 +0000
+++ b/main.cpp	Mon Mar 09 20:41:28 2015 +0000
@@ -7,13 +7,13 @@
 //Radius of the earth in meters
 #define EARTHRADIUS 6378100.0f
 //Tolerance for heading actual and heading needed
-#define HEADDIFF 2.0f
+#define HEADDIFF 0.0f
 //Tolerance for whether or not vehicle has arrived at goal position
 #define ARRIVED 100.0f
 //Period in seconds of the the main loop
 #define PERIOD 0.5f
 //Period in seconds of PID loop
-#define RATE .001
+#define RATE 0.1f
 #define PIDCYCLES 100
 //GPS
 GPS gps(D9, D7);
@@ -23,7 +23,7 @@
 
 //PID
 //Kc, Ti, Td, interval
-PID controller(.01, .01, 20, RATE);
+PID controller(.1, .005, .01, RATE);
 
 //Enter new position here
 float goalPos[2] = {35.336020, -81.912420};
@@ -44,7 +44,8 @@
     //Goal is to have the vehicle go straight
     controller.setInputLimits(-180, 180);
     controller.setOutputLimits(-.5, .5);
-    controller.setMode(0);
+    //set mode to auto
+    controller.setMode(1);
     //We want the difference to be zero
     controller.setSetPoint(0);
     
@@ -56,6 +57,29 @@
     gps.Init();
     xBee.printf("gps initialized\n");
     
+    while(!xBee.readable())
+    {
+        xBee.printf("Press any key to begin\n");
+        wait(1);
+    }
+    
+    //Test PID heading
+    while(1)
+    {
+        if(updateAngles())
+        {
+            makeVector();
+            //set heading to a fixed number for testing
+            float magDiff = whichWay(yaw, 0);
+            controller.setProcessValue(magDiff);
+            
+            motorSpeed = controller.compute();
+            goForward(0.5f + motorSpeed,0.5f - motorSpeed);
+            xBee.printf("heading = %f\tdiff = %f\tspeed = %f\n", yaw, magDiff, motorSpeed);
+            wait(RATE);
+        }
+    }
+    
     xBee.printf("attempting to get a fix\n");
     
     //wait until we have a gps fix
@@ -153,7 +177,8 @@
     if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
 }
 
-//Decide which direction to turn based on current compass heading(magHead) and heading required(calcHead)
+//Outputs difference in compass heading(magHead) and heading required(calcHead)
+//negative is left and positive is right
 float whichWay(float magHead, float calcHead)
 {
     float magDiff;
@@ -162,6 +187,7 @@
     
     //Is the heading off enough to care?
     if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360 - HEADDIFF)))
+    //if(1)
     {
         //quadrant I
         if(calcHead < 90)
@@ -278,105 +304,4 @@
         return magDiff;
     }
     return 0;
-}
-
-
-
-//Test for verifying heading
-void testYaw(void)
-{
-    //set test position to indicate a necessary heading of:
-    curPos[0] = 34.833716f;
-    curPos[1] = -82.404188f;
-    
-    //due north
-    goalPos[0] = 35.833716f;
-    goalPos[1] = -82.404188f;
-    
-    //Convert starting position and goal position to a vector
-    makeVector();
-    
-    xBee.printf("dist %fmeters\tangle %fdegrees due north\n", polarVector[0], polarVector[1]);
-    
-    //due south
-    goalPos[0] = 33.833716f;
-    goalPos[1] = -82.404188f;
-    
-    
-    //Convert starting position and goal position to a vector
-    makeVector();
-    
-    xBee.printf("dist %fmeters\tangle %fdegrees due south\n", polarVector[0], polarVector[1]);
-    
-    //due east
-    goalPos[0] = 34.833716f;
-    goalPos[1] = -81.404188f;
-    
-    //Convert starting position and goal position to a vector
-    makeVector();
-    
-    xBee.printf("dist %fmeters\tangle %fdegrees due east\n", polarVector[0], polarVector[1]);
-    
-    
-    //due west
-    goalPos[0] = 34.833716f;
-    goalPos[1] = -83.404188f;
-    
-    //Convert starting position and goal position to a vector
-    makeVector();
-    
-    xBee.printf("dist %fmeters\tangle %fdegrees due west\n", polarVector[0], polarVector[1]);
-    
-    //north east
-    goalPos[0] = 35.833716f;
-    goalPos[1] = -81.404188f;
-    
-    //Convert starting position and goal position to a vector
-    makeVector();
-    
-    xBee.printf("dist %fmeters\tangle %fdegrees north east\n", polarVector[0], polarVector[1]);
-    
-    
-    //north west
-    goalPos[0] = 35.833716f;
-    goalPos[1] = -83.404188f;
-    
-    
-    //Convert starting position and goal position to a vector
-    makeVector();
-    
-    xBee.printf("dist %fmeters\tangle %fdegrees north west\n", polarVector[0], polarVector[1]);
-    
-    //south east
-    goalPos[0] = 33.833716f;
-    goalPos[1] = -81.404188f;
-    
-    //Convert starting position and goal position to a vector
-    makeVector();
-    
-    xBee.printf("dist %fmeters\tangle %fdegrees south east\n", polarVector[0], polarVector[1]);
-    
-    
-    //south west
-    goalPos[0] = 33.833716f;
-    goalPos[1] = -83.404188f;
-    
-    
-    //Convert starting position and goal position to a vector
-    makeVector();
-    
-    xBee.printf("dist %fmeters\tangle %fdegrees south west\n", polarVector[0], polarVector[1]);
-    
-    while(1);
-    
-    /*
-    //below is useful when checking compass heading
-    while(1)
-    {
-        updateAngles();
-        char direction = whichWay(yaw, polarVector[1]);
-        //printf("accX %f\taccY %f\taccZ %f\tmagX %f\tmagY %f\tmagZ %f\troll %f\tpitch %f\tyaw %f\tturn %c\n", accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2], roll, pitch, yaw, direction);
-        wait(1);
-    }
-    */
 }
\ No newline at end of file

--- a/modSensData.h	Wed Mar 04 17:48:08 2015 +0000
+++ b/modSensData.h	Mon Mar 09 20:41:28 2015 +0000
@@ -183,7 +183,7 @@
 int updateAngles()
 {
     readIMU();
-    if((accel[0] <= THRESHOLD) && (accel[1] <= THRESHOLD) && (accel[2] <= THRESHOLD))
+    if((sqrt(accel[0] * accel[0]) <= THRESHOLD) && (sqrt(accel[1] * accel[1]) <= THRESHOLD) && (sqrt(accel[2] * accel[2]) <= THRESHOLD))
     {
         //float prevYaw = yaw;
         getAttitude();