Diff: vessel.h

Revision:

6:b45b74fd6a07

Parent:

5:07bbe020eb65

Child:

7:396fa2a8648d

--- a/vessel.h	Tue Jul 26 17:22:33 2016 +0000
+++ b/vessel.h	Wed Jul 27 02:45:45 2016 +0000
@@ -2,10 +2,12 @@
 #define VESSEL_H
 
 #include "mbed.h"
+#include "MODSERIAL.h"
 #include "MPU6050.h"
 #include "Servo.h"
 #include "IMU.h"
 #include "PID.h"
+#include <string>
 /*
             Cameras
       FL ----- F ->--- FR
@@ -28,6 +30,8 @@
       |        |       |
       6  ---<- 5 ----- 4
 */
+#define BUFFER_SIZE 255
+
 class Vessel
 {
 
@@ -58,8 +62,9 @@
     double xPoint, xIn, xOut;
     double yPoint, yIn, yOut;
     double zPoint, zIn, zOut;
+    double p_gain, i_gain, d_gain;
     PID pidy, pidr, pidp, pidX, pidY, pidZ;
-
+    char buffer[BUFFER_SIZE];
 public:
     void Start_IMU() {
         pc.printf("Starting up\n\r");
@@ -76,7 +81,8 @@
         pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT), 
         pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT),
         pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT),
-        pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT){
+        pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT)
+        {
 
         pidy.SetMode(AUTOMATIC);  //Yaw PID
         pidy.SetOutputLimits(-255,255);
@@ -258,10 +264,54 @@
         //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
         //pc.printf("%f,%f,%f,%f,%f \n\r",acczs, pitchs, rolls, zpr, (acczs + pitchs + rolls) / zpr + 0.5);
         //pc.printf("YAW: %f, %f, %f, %f, %f, %f, %f, %f\n\r", abs((acczs + pitchs + rolls) / zpr),abs((accys + yaws) / yy),abs((acczs + pitchs - rolls) / zpr),abs((accxs + yaws) / xy),abs((acczs - pitchs - rolls) / zpr),abs((accys + yaws) / yy),abs((acczs - pitchs + rolls) / zpr),abs((accxs + yaws) / yy));
-        // pc.printf("YAW: %f, %f, %f, %f\n\r", xOut, yawOut, yawIn, yawPoint);
-        //pc.printf("YAW: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint);
-        //pc.printf("ACC: %f, %f, %f\n\r", ax, ay, az);
+        // pc.printf("YAW: %f,%f, %f\n\r", ax, ay, az);
         //pc.printf("YPR: %f, %f, %f\n\r", yaw, pitch, roll);
     }
+    
+void updateCommand() {
+    char c = 0;
+    string command;
+    char buffer[BUFFER_SIZE] = {' '};
+    int buffer_iter = 0;    
+    pc.printf("Checking for command\n");
+
+    // Note: you need to actually read from the serial to clear the RX interrupt
+    if (pc.readable()) {
+       pc.printf("Found command\n");
+       while (pc.readable()) {
+            c = pc.getc();
+            pc.putc(c);
+            buffer[buffer_iter] = c;
+            buffer_iter++;
+        }
+        pc.printf("Command saved to buffer\n");
+        command = strtok (buffer," ,\n");
+        
+        if (strcmp(command.c_str(), "a")) {
+            this->yawPoint = atof(strtok (NULL, " ,\n"));
+            this->pitchPoint = atof(strtok (NULL, " ,\n"));
+            this->rollPoint = atof(strtok (NULL, " ,\n"));
+            pc.printf("Received Attitude points: yawPoint: %f, pitchPoint: %f, rollPoint: %f\n", this->yawPoint, this->pitchPoint, this->rollPoint);
+        } else if (strcmp(command.c_str(), "b")) {
+            this->xPoint = atof(strtok (NULL, " ,\n"));
+            this->yPoint = atof(strtok (NULL, " ,\n"));
+            this->zPoint = atof(strtok (NULL, " ,\n"));
+            pc.printf("Received X,Y,Z points: X: %f, Y: %f, Z: %f\n", this->xPoint, this->yPoint, this->zPoint);
+        } else if (strcmp(command.c_str(), "c")) {
+            this->p_gain = atof(strtok (NULL, " ,\n"));
+            this->i_gain = atof(strtok (NULL, " ,\n"));
+            this->d_gain = atof(strtok (NULL, " ,\n"));
+            
+            this->SetYawPID(this->p_gain, this->i_gain, this->d_gain);
+            pc.printf("Received PID values P: %f, I: %f, D: %f\n", this->p_gain, this->i_gain, this->d_gain);
+
+        }
+                
+        memset(buffer, ' ', sizeof(buffer));
+        buffer_iter = 0;
+        fflush(stdout);
+    }
+}
 };
+
 #endif
\ No newline at end of file