Diff: vessel.h

Revision:

7:396fa2a8648d

Parent:

6:b45b74fd6a07

Child:

8:2db98df6fbbb

--- a/vessel.h	Wed Jul 27 02:45:45 2016 +0000
+++ b/vessel.h	Fri Jul 29 15:34:59 2016 +0000
@@ -8,6 +8,10 @@
 #include "IMU.h"
 #include "PID.h"
 #include <string>
+#include "MS5837.h"
+
+MS5837 pressure_sensor = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS);
+
 /*
             Cameras
       FL ----- F ->--- FR
@@ -36,41 +40,48 @@
 {
 
 private:
-//    Servo m0;
-//    Servo m1;
-//    Servo m2;
-//    Servo m3;
-//    Servo m4;
-//    Servo m5;
-//    Servo m6;
-//    Servo m7;
-    
-    PwmOut m0;
-    PwmOut m1;
-    PwmOut m2;
-    PwmOut m3;
-    PwmOut m4;
-    PwmOut m5;
-    PwmOut m6;
-    PwmOut m7;
+    Servo m0;
+    Servo m1;
+    Servo m2;
+    Servo m3;
+    Servo m4;
+    Servo m5;
+    Servo m6;
+    Servo m7;
+
+    AnalogIn powerPin;
+    int motorState;
+//    PwmOut m0;
+//    PwmOut m1;
+//    PwmOut m2;
+//    PwmOut m3;
+//    PwmOut m4;
+//    PwmOut m5;
+//    PwmOut m6;
+//    PwmOut m7;
 
     PwmOut led1;
     MPU6050 mpu6050;
+
     double yawPoint, yawIn, yawOut;
     double rollPoint, rollIn, rollOut;
     double pitchPoint, pitchIn, pitchOut;
     double xPoint, xIn, xOut;
     double yPoint, yIn, yOut;
     double zPoint, zIn, zOut;
+    double dPoint, dIn, dOut;
     double p_gain, i_gain, d_gain;
-    PID pidy, pidr, pidp, pidX, pidY, pidZ;
+    PID pidy, pidr, pidp, pidX, pidY, pidZ, pidd;
     char buffer[BUFFER_SIZE];
 public:
+    float depth;
+
     void Start_IMU() {
         pc.printf("Starting up\n\r");
         pc.baud(9600);
         i2c.frequency(400000);  // use fast (400 kHz) I2C
         IMUinit(mpu6050);
+        pressure_sensor.MS5837Init();
         IMUPrintData(mpu6050);
     }
 
@@ -78,11 +89,13 @@
     Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1),
         pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
         pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
-        pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT), 
+        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),
+        pidd(&dIn, &dOut, &dPoint,1,1,1, DIRECT),
+        powerPin(A5)
+{
 
         pidy.SetMode(AUTOMATIC);  //Yaw PID
         pidy.SetOutputLimits(-255,255);
@@ -102,7 +115,11 @@
         pidZ.SetMode(AUTOMATIC);  //Pitch PID
         pidZ.SetOutputLimits(-255,255);
         zPoint = 0;
-        
+        pidd.SetMode(AUTOMATIC);  //Pitch PID
+        pidd.SetOutputLimits(-255,255);
+        wait(0.5);
+        dPoint = depth;
+
         m0 = 0.5;
         m1 = 0.5;
         m2 = 0.5;
@@ -112,6 +129,8 @@
         m6 = 0.5;
         m7 = 0.5;
         
+        motorState = 1;
+
         Start_IMU();
         pc.printf("Seagoat Initialized \n\r");
     }
@@ -139,44 +158,67 @@
     void SetZPID(double Kp, double Ki, double Kd) {
         pidZ.SetTunings(Kp, Ki, Kd);
     }
-    
+    void SetdPID(double Kp, double Ki, double Kd) {
+        pidd.SetTunings(Kp, Ki, Kd);
+    }
+
     //This is where the magic happens
-    void motorTest(){
-            pwmSweep(m0);
-            pwmSweep(m1);
-            pwmSweep(m2);
-            pwmSweep(m3);
-            pwmSweep(m4);
-            pwmSweep(m5);
-            pwmSweep(m6);
-            pwmSweep(m7);
-    }
-    
-    void pwmSweep(PwmOut motor){
-        for(float i = 0; i < 80; i++){
-            motor = i/255;
-            wait(0.002);
-        }   
-   //     for(float i = 80; i >= 0; i--){
+//    void motorTest(){
+//            pwmSweep(m0);
+//            pwmSweep(m1);
+//            pwmSweep(m2);
+//            pwmSweep(m3);
+//            pwmSweep(m4);
+//            pwmSweep(m5);
+//            pwmSweep(m6);
+//            pwmSweep(m7);
+//    }
+//
+//    void pwmSweep(PwmOut motor){
+//        for(float i = 0; i < 80; i++){
 //            motor = i/255;
 //            wait(0.002);
-//        }   
-    }
-    void calibrate(){
+//        }
+//   //     for(float i = 80; i >= 0; i--){
+////            motor = i/255;
+////            wait(0.002);
+////        }
+//    }
+    void calibrate() {
         IMUUpdate(mpu6050);
         pc.printf("Calibrating...\n\r");
+        //pressure_sensor.Barometer_MS5837();
+        //depth = pressure_sensor.MS5837_Pressure();
     }
-    
-    void update(){
+
+    void update() {
         //Update IMU Values
         IMUUpdate(mpu6050);
+
+        //pressure_sensor.Barometer_MS5837();
+        depth = pressure_sensor.MS5837_Pressure();
+        //pc.printf("Pressure: %f %f\n", depth, dPoint);
+        
+        //Detect if the switch is turned on
+        if(!motorState && powerPin.read() == 1){
+            initMotors(); 
+            motorState = 1;  
+            pc.printf("Motors Detected");
+            
+             yawPoint = yaw;
+        }
+        else if(powerPin.read() != 1){
+            motorState = 0;
+            neutralizeMotors();
+        }
+
         yawIn = yaw;
         rollIn = roll;
         pitchIn = pitch;
         xIn = ax;
         yIn = ay;
         zIn = az;
-        
+
         //Calculate PID values
         pidy.Compute();
         pidr.Compute();
@@ -184,7 +226,7 @@
         pidX.Compute();
         pidY.Compute();
         pidZ.Compute();
-        
+
         /*
                 Cameras
           FL ----- F ->--- FR
@@ -196,7 +238,7 @@
           |        |       ˅
           |        |       |
           BL ---<- B ----- BR
-    
+
           0  ----- 1 ->--- 2
           |        |       |
           ˄        |       |
@@ -206,43 +248,44 @@
           |        |       ˅
           |        |       |
           6  ---<- 5 ----- 4
-          
+
         */
-        
+
         //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);
-        
+
         //Values used in Dynamic Magnitude Calculations
         float accxs = xOut * xOut * abs(xOut) / xOut;
         float accys = yOut * yOut * abs(yOut) / yOut;
         float acczs = zOut * zOut * abs(zOut) / zOut;
+        float depths = dOut * dOut;
         float yaws = yawOut * yawOut * abs(yawOut) / yawOut;
         float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut;
         float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
-        
+
         //Values used for Influence calculations
-        float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 255;
+        float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut) + abs(dOut)) * 255;
         float yy  = (abs(yOut) + abs(yawOut)) * 255;
-        float xy  = (abs(xOut) + abs(yawOut)) * 255; 
-        
+        float xy  = (abs(xOut) + abs(yawOut)) * 255;
+
 //        float zpr = (zOut + pitchOut + rollOut) * 255;
 //        float yy  = (yOut + yawOut) * 255;
-//        float xy  = (xOut + yawOut) * 255; 
-        
+//        float xy  = (xOut + yawOut) * 255;
+
 //        if (abs(zpr)<255 && abs(zpr)>=0) zpr = 255;
 //        if (abs(yy)<255 && abs(yy)>=0) yy = 255;
-//        if (abs(xy)<255 && abs(xy)>=0) xy = 255;        
+//        if (abs(xy)<255 && abs(xy)>=0) xy = 255;
 //        if (abs(zpr)>-255 && abs(zpr)<0) zpr = -255;
 //        if (abs(yy)>-255 && abs(yy)<0) yy = -255;
-//        if (abs(xy)>-255 && abs(xy)<0) xy = -255;  
-     
+//        if (abs(xy)>-255 && abs(xy)<0) xy = -255;
+
         if (abs(zpr)<255) zpr = 255;
         if (abs(yy)<255) yy = 255;
         if (abs(xy)<255) xy = 255;
-        
+
         //pc.printf("YAW: %f, %f, %f, %f, %f\n\r", zOut, pitchOut, rollOut, zpr, abs((acczs + pitchs + rolls) / zpr));
-        
+
         //Spit out PID values
-        
+
 //        m0 = abs((acczs + pitchs + rolls) / zpr);//
 //        m1 = abs((accys + yaws) / yy);
 //        m2 = abs((acczs + pitchs - rolls) / zpr);//
@@ -251,67 +294,92 @@
 //        m5 = abs((accys + yaws) / yy);
 //        m6 = abs((acczs - pitchs + rolls) / zpr);//
 //        m7 = abs((accxs + yaws) / yy);
-        
-        m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
+
+        m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;//
         m1 = (accys + yaws) / yy + 0.5;
-        m2 = (acczs + pitchs - rolls) / zpr + 0.5;//
-        m3 = (accxs + yaws) / xy + 0.5;
-        m4 = (acczs - pitchs - rolls) / zpr + 0.5;//
+        m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
+        m3 = (accxs + yaws) / -xy + 0.5;
+        m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
         m5 = (accys + yaws) / yy + 0.5;
-        m6 = (acczs - pitchs + rolls) / zpr + 0.5;//
+        m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
         m7 = (accxs + yaws) / yy + 0.5;
-        
+
         //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("%f,%f,%f, %f,%f,%f, %f, %f \n\r",powerPin.read(), acczs, yaws, pitchs, rolls, zpr, depths, (acczs + pitchs + rolls - depths) / 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\n\r", ax, ay, az);
-        //pc.printf("YPR: %f, %f, %f\n\r", yaw, pitch, roll);
+        //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
+    }
+
+    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);
+        }
+    }
+
+    void initMotors(){
+        
+        neutralizeMotors();
+        m0.calibrate();
+        m1.calibrate();
+        m2.calibrate();
+        m3.calibrate();
+        m4.calibrate();
+        m5.calibrate();
+        m6.calibrate();
+        m7.calibrate();
     }
     
-void updateCommand() {
-    char c = 0;
-    string command;
-    char buffer[BUFFER_SIZE] = {' '};
-    int buffer_iter = 0;    
-    pc.printf("Checking for command\n");
+    void neutralizeMotors(){
+        m0 = 0.5;
+        m1 = 0.5;
+        m2 = 0.5;
+        m3 = 0.5;
+        m4 = 0.5;
+        m5 = 0.5;
+        m6 = 0.5;
+        m7 = 0.5;
+    }
 
-    // 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