Diff: main.cpp

Revision:

4:a397b44a0fe8

Parent:

3:ffa0e1429a72

--- a/main.cpp	Mon Mar 09 20:41:28 2015 +0000
+++ b/main.cpp	Tue Mar 17 01:13:17 2015 +0000
@@ -13,18 +13,21 @@
 //Period in seconds of the the main loop
 #define PERIOD 0.5f
 //Period in seconds of PID loop
-#define RATE 0.1f
+#define RATE 0.05f
 #define PIDCYCLES 100
 //GPS
-GPS gps(D9, D7);
+GPS gps(PTC4, PTC3);
 
 //X-Bee
-Serial xBee(PTC15, PTC14);
+Serial xBee(PTD3, PTD2);       //boat
+//Serial xBee(PTC15, PTC14);   //car
 
 //PID
 //Kc, Ti, Td, interval
 PID controller(.1, .005, .01, RATE);
 
+Timer t2;
+
 //Enter new position here
 float goalPos[2] = {35.336020, -81.912420};
 
@@ -39,16 +42,6 @@
 
 int main()
 {
-    float motorSpeed = 0.5f;
-    //PID control of left and right motors based on input from gyro
-    //Goal is to have the vehicle go straight
-    controller.setInputLimits(-180, 180);
-    controller.setOutputLimits(-.5, .5);
-    //set mode to auto
-    controller.setMode(1);
-    //We want the difference to be zero
-    controller.setSetPoint(0);
-    
     xBee.baud(9600);
     
     xBee.printf("\nI'm Alive...\n");
@@ -62,23 +55,15 @@
         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);
-        }
-    }
+    float motorSpeed = 0.5f;
+    //PID control of left and right motors based on input from gyro
+    //Goal is to have the vehicle go straight
+    controller.setInputLimits(-180, 180);
+    controller.setOutputLimits(-.5, .5);
+    //set mode to auto
+    controller.setMode(1);
+    //We want the difference to be zero
+    controller.setSetPoint(0);
     
     xBee.printf("attempting to get a fix\n");