Diff: main.cpp

Revision:

8:1aeb13d290db

Parent:

7:bde99b0b3a86

Child:

9:b537a858040e

--- a/main.cpp	Thu Apr 30 21:37:24 2015 +0000
+++ b/main.cpp	Thu Apr 30 23:58:54 2015 +0000
@@ -39,8 +39,8 @@
 Mutex setPointMutex;
 Mutex processValueMutex;
 MODSERIAL xbee(p13, p14, 64, 128);
-PID linearController(0.00001, 0.0, 0.0, RATE);
-PID angularController(0.0000000001, 0.0, 0.0, RATE);
+PID linearController(1.0, 0.0, 0.0, RATE);
+PID angularController(1.0, 0.0, 0.0, RATE);
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
@@ -107,10 +107,10 @@
 
 
 //values for pwm wave to motors
-/*
+
 float linearOutput = 0;
 float angularOutput = 0;
-*/
+
 float leftMotorCO = 0;
 float rightMotorCO = 0;
 
@@ -119,7 +119,8 @@
 float setLinear = 0;
 float setAngular = 0;
 int enabled = 0;
-
+int lin_reverse;
+int ang_reverse;
 
 /**
  * Prototypes
@@ -141,7 +142,8 @@
         //pc.printf("linearOutput: %f    angularOutput: %f\n",linearOutput, angularOutput);
         //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("v_x: %f     angularSpeed: %f\n", v_x, w_z);
+        pc.printf("linearOutput: %f     angularOutput: %f\n", linearOutput, angularOutput);
         pc.printf("leftMotorCO: %f     rightMotorCO: %f\n\n", leftMotorCO, rightMotorCO);
         Thread::wait(2000);
     }
@@ -162,118 +164,95 @@
     usePID = 0;
 }
 
-//Variables for storing commands
-char commBuffer [64];
-int currentBuff = 0;
 
-
-void parseCommand()
-{
+void parseCommand(char buff[], int length) {
     float *tempForward, *tempAngular;
     volatile char forwardBuffer [4];
     volatile char angularBuffer [4];
     int tempEnabled, tempPID;
     char statusChar;
     
-    
-    // First char (commBuffer[0]) is header info. commBuffer[1-4] is forward direction in raw float value. CommBUffer[5-8] is reverse direction in raw float value.
-    //memcpy(&tempForward, (commBuffer + 1), 4);
-    //memcpy(&tempAngular, (commBuffer + 5), 4);
+    float *tempLinearPGain, *tempLinearIGain, *tempLinearDGain;
+    float *tempAngularPGain, *tempAngularIGain, *tempAngularDGain;
     
-
-    
-    // now convert the char arrays into floats
-    tempForward = (float*) (commBuffer + 1);
-    tempAngular = (float*) (commBuffer + 5);
-    
+    // Main control parsing
+    if (0xCC == buff[0] && 10 == length) {
     
-    //tempForward = (float*) forwardBuffer;
-    //tempAngular = (float*) angularBuffer;
-    
-    statusChar = commBuffer[9]; //contains data for enable and enablePID
-    tempPID = (int) (statusChar & 0x0F); //extract the last 4 bits and type cast to an int
-    tempEnabled = (int) (statusChar & 0xF0); //extract the first 4 bits and type cast to an int
-    //Change global variables
-    setLinear = *tempForward * LINEAR_MULTIPLIER;
-    setAngular = *tempAngular * ANGULAR_MULTIPLIER;
-    enabled = tempEnabled;
+        tempForward = (float*) (buff + 1);
+        tempAngular = (float*) (buff + 5);
+        statusChar = buff[9]; //contains data for enable and enablePID
+        tempPID = (int) (statusChar & 0x0F); //extract the last 4 bits and type cast to an int
+        tempEnabled = (int) (statusChar & 0xF0); //extract the first 4 bits and type cast to an int
+
+        //Change global variables
+        setPointMutex.lock();
+        setLinear = *tempForward * LINEAR_MULTIPLIER;
+        setAngular = *tempAngular * ANGULAR_MULTIPLIER;
+        if (setLinear < 0) {
+            setLinear = -setLinear; // make pos
+            lin_reverse = 1;
+        } else lin_reverse = 0;
 
-    if (tempPID) {
-        startPID();
-    } else {
-        stopPID();
+        if (setAngular < 0) {
+            setAngular = -setAngular; //make pos
+            ang_reverse = 1;
+        } else ang_reverse = 0;
+        enabled = tempEnabled;
+        if (tempPID) {
+            startPID();
+        } else {
+            stopPID();
+        }
+        setPointMutex.unlock();
+    // Parsing change PID values command
+    } else if (0x55 == buff[0] && 25 == length) {
+        tempLinearPGain = (float*) (buff + 1);
+        tempLinearIGain = (float*) (buff + 5);
+        tempLinearDGain = (float*) (buff + 9);
+        tempAngularPGain = (float*) (buff + 13);
+        tempAngularIGain = (float*) (buff + 17);
+        tempAngularDGain = (float*) (buff + 21);
+        linearController.setTunings(*tempLinearPGain, *tempLinearIGain, *tempLinearDGain);
+        angularController.setTunings(*tempAngularPGain, *tempAngularIGain, *tempAngularDGain);
     }
 
 }
 
+char waitForChar() {
+    while (!xbee.readable()) {
+        Thread::wait(10);
+    }
+    return xbee.getc();
+}
+
 void parseInput(void const *args)
 {
     static int i = 0;
+    static int length = 0;
+    static char commBuffer [64];
+    
+    while (xbee.readable()) {
+        xbee.getc();
+    }
     
     while (1) {
-        /*
-        while (xbee.readable()) {
-            commBuffer[1 - currentBuff][i] = xbee.getc();
-            if (commBuffer[1 - currentBuff][i] == 0xAA) {
-                i = 0;
-                currentBuff = 1 - currentBuff;
-                if (commBuffer[currentBuff][0] == 0xFF) {
-                    parseCommand();
-                    commReceived = 1;
-                }
-            } else {
-                ++i;
-            }
+        if (0x55 != waitForChar()) {
+            continue;
         }
-        */
-        
-        while (xbee.readable()) {
-            commBuffer[i] = xbee.getc();
-            if (commBuffer[i] == 0xAA) { //end of message char
-                i = 0;
-                if (commBuffer[0] == 0xFF) { // buffer recieved is correct length and bit. parse the data
-                    parseCommand();
-                    commReceived = 1;
-                }
-            } else {
-                ++i;
-            }
+        if (0x55 != waitForChar()) {
+            continue;
         }
+        length = waitForChar();
         
-
-
-
-
-
-        /*
-        while (xbee.readable()) {// something arrived from the xbee
-            commBuffer [0] = xbee.getc(); //[1 - currentBuff][i] = xbee.getc();
-            commBuffer [1] = xbee.getc();
-            if (commBuffer [0] == 0x5 && commBuffer[1] == 0x5) { //the frame from xbee has ended // [1 - currentBuff][i] == '\r') {
-                // next 4 chars (32 bits) are going to be the forward speed
-                for (int fBi = 0; fBi < 4; ++fBi) {
-                    forwardBuffer[fBi] = xbee.getc();
-                }
-                for (int rBi = 0; rBi < 4; ++rBi) {
-                    reverseBuffer[rBi] = xbee.getc();
-                }
-                enabled = (int) xbee.getc();
-                tempPID = (int) xbee.getc();
-                if (tempPID) {
-                    startPID();
-                } else {
-                    stopPID();
-                }
-                commBuffer [2] = xbee.getc();
-                commBuffer [3] = xbee.getc();
-                if (commBuffer [3] == 0xA && commBuffer [4] == 0xA) {
-                commReceived = 1;
-                }
-
-            }
+        for (i = 0; i < length; ++i) {
+            commBuffer[i] = waitForChar();
         }
-        */
-        Thread::wait(100);
+        if (waitForChar() != 0xAA) {
+            continue;
+        }
+        commReceived = 1;
+        parseCommand(commBuffer, length);
+        //Thread::wait(100);
     }
 }
 
@@ -532,10 +511,9 @@
 
 int main()
 {
-    float linearOutput = 0;
-    float angularOutput = 0;
-    int lin_reverse;
-    int ang_reverse;
+    //float linearOutput = 0;
+    //float angularOutput = 0;
+
     /*
     float leftMotorCO = 0;
     float rightMotorCO = 0;
@@ -561,14 +539,6 @@
     while (1) {
         //pc.printf("in loop\n");
         if(usePID) {
-            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);
@@ -576,22 +546,26 @@
             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
+            linearController.setProcessValue(v_x);
+            angularController.setProcessValue(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 {
+            //pc.printf("linearOutput: %f",linearOutput);
+        } else {// PID Disabled
             linearOutput = setLinear/LINEAR_MULTIPLIER;
             angularOutput = setAngular/ANGULAR_MULTIPLIER;
         }
 
         if (enabled) {
+            if (lin_reverse == 0) {
+                linearOutput = linearController.compute();
+            } else {
+                linearOutput = -linearController.compute();
+            }
+            if (ang_reverse == 0) {
+                angularOutput = angularController.compute();
+            } else {
+                angularOutput = -angularController.compute();
+            }
             leftMotorCO = (linearOutput - angularOutput);
             rightMotorCO = (linearOutput + angularOutput);