Diff: main.cpp

Revision:

7:bde99b0b3a86

Parent:

6:c0a2ac7a8c26

Child:

8:1aeb13d290db

--- a/main.cpp	Thu Apr 30 20:16:00 2015 +0000
+++ b/main.cpp	Thu Apr 30 21:37:24 2015 +0000
@@ -36,9 +36,11 @@
 Serial pc(USBTX, USBRX);
 LSM9DS0 imu(p28, p27, LSM9DS0_G, LSM9DS0_XM);
 Mutex pcMutex;
+Mutex setPointMutex;
+Mutex processValueMutex;
 MODSERIAL xbee(p13, p14, 64, 128);
-PID linearController(1.0, 0.0, 0.0, RATE);
-PID angularController(1.0, 0.0, 0.0, RATE);
+PID linearController(0.00001, 0.0, 0.0, RATE);
+PID angularController(0.0000000001, 0.0, 0.0, RATE);
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
@@ -140,7 +142,7 @@
         //pc.printf("leftMotorCO: %f     rightMotorCO: %f\n", leftMotorCO, rightMotorCO);
         pc.printf("setLinear: %f     setAngular: %f\n", setLinear, setAngular);
         pc.printf("linearMagnitude: %f     angularSpeed: %f\n", v_mag2, w_z);
-        pc.printf("leftMotorCO: %f     rightMotorCO: %f\n", leftMotorCO, rightMotorCO);
+        pc.printf("leftMotorCO: %f     rightMotorCO: %f\n\n", leftMotorCO, rightMotorCO);
         Thread::wait(2000);
     }
 }
@@ -299,9 +301,9 @@
 
             //Average the samples, remove the bias, and calculate the acceleration
             //in m/s/s.
-            a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
-            a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
-            a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
+            a_x = -((a_xAccumulator / SAMPLES) - a_xBias) * g0;// * ACCELEROMETER_GAIN;
+            a_y = -((a_yAccumulator / SAMPLES) - a_yBias) * g0;// * ACCELEROMETER_GAIN;
+            a_z = -((a_zAccumulator / SAMPLES) - a_zBias) * g0;// * ACCELEROMETER_GAIN;
 
             a_xAccumulator = 0;
             a_yAccumulator = 0;
@@ -313,9 +315,9 @@
 
             // integrate to get velocity. make sure to subtract gravity downwards!
 
-            v_x = v_x + (a_x + last_a_x)/2 * ACC_RATE * SAMPLES * 1000;
-            v_y = v_y + (a_y + last_a_y)/2 * ACC_RATE * SAMPLES * 1000;
-            v_z = v_z + (a_z + last_a_z)/2 * ACC_RATE * SAMPLES * 1000;
+            v_x = v_x + (a_x + last_a_x)/2 * ACC_RATE * SAMPLES;
+            v_y = v_y + (a_y + last_a_y)/2 * ACC_RATE * SAMPLES;
+            v_z = v_z + (a_z + last_a_z)/2 * ACC_RATE * SAMPLES;
 
             last_a_x = a_x;
             last_a_y = a_y;
@@ -455,9 +457,9 @@
 
             //Average the samples, remove the bias, and calculate the angular
             //velocity in deg/s.
-            w_x = ((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN;
-            w_y = ((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN;
-            w_z = ((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN;
+            w_x = ((w_xAccumulator / SAMPLES) - w_xBias); //* GYROSCOPE_GAIN;
+            w_y = ((w_yAccumulator / SAMPLES) - w_yBias); //* GYROSCOPE_GAIN;
+            w_z = ((w_zAccumulator / SAMPLES) - w_zBias); //* GYROSCOPE_GAIN;
             //pcMutex.lock();
             //pc.printf("w_x: %f  w_y: %f  w_z: %f \n",w_x,w_y,w_z);
             //pcMutex.unlock();
@@ -516,10 +518,10 @@
     pc.printf("\n");
 
     // Set up PID Loops
-    linearController.setInputLimits(-LINEAR_MULTIPLIER * LINEAR_MULTIPLIER, LINEAR_MULTIPLIER * LINEAR_MULTIPLIER);
-    angularController.setInputLimits(-ANGULAR_MULTIPLIER, ANGULAR_MULTIPLIER);
-    linearController.setOutputLimits(-1.0, 1.0);
-    angularController.setOutputLimits(-1.0, 1.0);
+    linearController.setInputLimits(0, LINEAR_MULTIPLIER);
+    angularController.setInputLimits(0, ANGULAR_MULTIPLIER);
+    linearController.setOutputLimits(0, 1.0);
+    angularController.setOutputLimits(0, 1.0);
     linearController.setSetPoint(0.0);
     angularController.setSetPoint(0.0);
     linearController.setMode(AUTO_MODE);
@@ -532,6 +534,8 @@
 {
     float linearOutput = 0;
     float angularOutput = 0;
+    int lin_reverse;
+    int ang_reverse;
     /*
     float leftMotorCO = 0;
     float rightMotorCO = 0;
@@ -557,14 +561,31 @@
     while (1) {
         //pc.printf("in loop\n");
         if(usePID) {
-            linearController.setSetPoint(setLinear * setLinear);
+            if (setLinear < 0) {
+                setLinear = -setLinear; // make pos
+                lin_reverse = 1;
+            } else lin_reverse = 0;
+            if (setAngular < 0) {
+                setAngular = -setAngular; //make pos
+                ang_reverse = 1;
+            } else ang_reverse = 0;
+            setPointMutex.lock();
+            linearController.setSetPoint(setLinear);
             angularController.setSetPoint(setAngular);
-
-            v_mag2 = v_x * v_x + v_y * v_y;
-            linearController.setProcessValue(v_mag2);
-            angularController.setProcessValue(w_z); // w_z = degrees per second
-            linearOutput = linearController.compute();
-            angularOutput = angularController.compute();
+            setPointMutex.unlock();
+            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(abs(v_x));
+            angularController.setProcessValue(abs(w_z)); // w_z = degrees per second
+            processValueMutex.unlock();
+            if (lin_reverse == 0 | ang_reverse == 0) {
+                linearOutput = linearController.compute();
+                angularOutput = angularController.compute();
+            } else {
+                linearOutput = -linearController.compute();
+                angularOutput = -angularController.compute();
+            }
         } else {
             linearOutput = setLinear/LINEAR_MULTIPLIER;
             angularOutput = setAngular/ANGULAR_MULTIPLIER;