Diff: main.cpp

Revision:

9:b537a858040e

Parent:

8:1aeb13d290db

--- a/main.cpp	Thu Apr 30 23:58:54 2015 +0000
+++ b/main.cpp	Fri May 01 00:42:23 2015 +0000
@@ -33,6 +33,9 @@
 #define LINEAR_MULTIPLIER 5
 #define ANGULAR_MULTIPLIER 90
 
+#define LINEAR_MAX_SPEED 3.0
+#define ANGULAR_MAX_SPEED 90.0
+
 Serial pc(USBTX, USBRX);
 LSM9DS0 imu(p28, p27, LSM9DS0_G, LSM9DS0_XM);
 Mutex pcMutex;
@@ -186,8 +189,10 @@
 
         //Change global variables
         setPointMutex.lock();
-        setLinear = *tempForward * LINEAR_MULTIPLIER;
-        setAngular = *tempAngular * ANGULAR_MULTIPLIER;
+        setLinear = *tempForward;
+        setAngular = *tempAngular;
+        
+        /*
         if (setLinear < 0) {
             setLinear = -setLinear; // make pos
             lin_reverse = 1;
@@ -197,6 +202,7 @@
             setAngular = -setAngular; //make pos
             ang_reverse = 1;
         } else ang_reverse = 0;
+        */
         enabled = tempEnabled;
         if (tempPID) {
             startPID();
@@ -497,10 +503,10 @@
     pc.printf("\n");
 
     // Set up PID Loops
-    linearController.setInputLimits(0, LINEAR_MULTIPLIER);
-    angularController.setInputLimits(0, ANGULAR_MULTIPLIER);
-    linearController.setOutputLimits(0, 1.0);
-    angularController.setOutputLimits(0, 1.0);
+    linearController.setInputLimits(-1.0, 1.0);
+    angularController.setInputLimits(-1.0, 1.0);
+    linearController.setOutputLimits(-1.0, 1.0);
+    angularController.setOutputLimits(-1.0, 1.0);
     linearController.setSetPoint(0.0);
     angularController.setSetPoint(0.0);
     linearController.setMode(AUTO_MODE);
@@ -546,16 +552,14 @@
             if (abs(w_z) < 2.0f) w_z = 0; // if less than 2 deg/sec, assume it is noise and make it eqal to 0.
             //v_mag2 = v_x * v_x + v_y * v_y;
             processValueMutex.lock();
-            linearController.setProcessValue(v_x);
-            angularController.setProcessValue(w_z); // w_z = degrees per second
+            linearController.setProcessValue(v_x/LINEAR_MAX_SPEED);
+            angularController.setProcessValue(w_z/ANGULAR_MAX_SPEED); // w_z = degrees per second
             processValueMutex.unlock();
             //pc.printf("linearOutput: %f",linearOutput);
-        } else {// PID Disabled
-            linearOutput = setLinear/LINEAR_MULTIPLIER;
-            angularOutput = setAngular/ANGULAR_MULTIPLIER;
-        }
-
-        if (enabled) {
+            linearOutput = linearController.compute();
+            angularOutput = angularController.compute();
+            
+            /*
             if (lin_reverse == 0) {
                 linearOutput = linearController.compute();
             } else {
@@ -566,6 +570,15 @@
             } else {
                 angularOutput = -angularController.compute();
             }
+            */
+            
+        } else {// PID Disabled
+            linearOutput = setLinear;
+            angularOutput = setAngular;
+        }
+
+        if (enabled) {
+
             leftMotorCO = (linearOutput - angularOutput);
             rightMotorCO = (linearOutput + angularOutput);