File content as of revision 10:8cd741a65646:

#ifndef VESSEL_H
#define VESSEL_H

#include "mbed.h"
#include "MODSERIAL.h"
#include "MPU6050.h"
#include "Servo.h"
#include "IMU.h"
#include "PID.h"
#include <string>
#include "MS5837.h"

MS5837 pressure_sensor = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS);

#define BUFFER_SIZE 255

class Vessel
{

private:
    Servo m0;
    Servo m1;
    Servo m2;
    Servo m3;
    Servo m4;
    Servo m5;
    Servo m6;
    Servo m7;

    AnalogIn powerPin;
    int motorState;
    int runningState;
    Timer runningTime;

//    PwmOut m0;
//    PwmOut m1;
//    PwmOut m2;
//    PwmOut m3;
//    PwmOut m4;
//    PwmOut m5;
//    PwmOut m6;
//    PwmOut m7;

    PwmOut led1;
    MPU6050 mpu6050;

    double yawPoint, yawIn, yawOut, lastyawPoint;
    double rollPoint, rollIn, rollOut, lastrollPoint;
    double pitchPoint, pitchIn, pitchOut, lastpitchPoint;
    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, 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);
        runningTime.start();
    }

    //Initialise all of the vessels starting parameters
    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),
        pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT),
        pidY(&yIn, &yOut, &yPoint,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);
        yawPoint = 0;
        pidr.SetMode(AUTOMATIC);  //Roll PID
        pidr.SetOutputLimits(-255,255);
        pitchPoint = 0;
        pidp.SetMode(AUTOMATIC);  //Pitch PID
        pidp.SetOutputLimits(-255,255);
        rollPoint = 0;
        pidX.SetMode(AUTOMATIC);  //Pitch PID
        pidX.SetOutputLimits(-255,255);
        xPoint = 0;
        pidY.SetMode(AUTOMATIC);  //Pitch PID
        pidY.SetOutputLimits(-255,255);
        yPoint = 0;
        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;
        m3 = 0.5;
        m4 = 0.5;
        m5 = 0.5;
        m6 = 0.5;
        m7 = 0.5;

        motorState = 1;
        runningState = -1;

        Start_IMU();
        pc.printf("Seagoat Initialized \n\r");
    }

    void SetYawPID(double Kp, double Ki, double Kd) {
        pidy.SetTunings(Kp, Ki, Kd);
    }

    void SetRollPID(double Kp, double Ki, double Kd) {
        pidr.SetTunings(Kp, Ki, Kd);
    }

    void SetPitchPID(double Kp, double Ki, double Kd) {
        pidp.SetTunings(Kp, Ki, Kd);
    }

    void SetXPID(double Kp, double Ki, double Kd) {
        pidX.SetTunings(Kp, Ki, Kd);
    }

    void SetYPID(double Kp, double Ki, double Kd) {
        pidY.SetTunings(Kp, Ki, Kd);
    }

    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);
    }

    void calibrate() {
        IMUUpdate(mpu6050);
        pc.printf("Calibrating...\n\r");
        //pressure_sensor.Barometer_MS5837();
        //depth = pressure_sensor.MS5837_Pressure();
    }

    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) {
            runningTime.stop();
            initMotors();
            motorState = 1;
            runningState += 1;
            if(runningState == 2) runningState = 0;

            pc.printf("Motors Detected");

            yawPoint = yaw;
            if(runningState == 0) pitchPoint = pitch;
            else pitchPoint = 0;
            dPoint = depth;
            runningTime.reset();
            runningTime.start();
        } else if(powerPin.read() != 1) {
            motorState = 0;
        }

        yawIn = yaw;
        rollIn = roll;
        pitchIn = pitch;
        xIn = ax;
        yIn = ay;
        zIn = az;

        //Calculate PID values
        pidy.Compute();
        pidr.Compute();
        pidp.Compute();
        pidX.Compute();
        pidY.Compute();
        pidZ.Compute();

        /*
                Cameras
          FL ----- F ->--- FR
          |        |       |
          ˄        |       |
          |        |       |
          L        |       R
          |        |       |
          |        |       ˅
          |        |       |
          BL ---<- B ----- BR

          0  ----- 1 ->--- 2
          |        |       |
          ˄        |       |
          |        |       |
          7        |       3
          |        |       |
          |        |       ˅
          |        |       |
          6  ---<- 5 ----- 4

        */

        //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);

        float forwardThrust = 100;

        //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 yy  = (abs(yOut) + abs(yawOut)) * 255;
        float xy  = (abs(xOut) + abs(yawOut)) * 255;

        if (abs(zpr)<255) zpr = 255;
        if (abs(yy)<255) yy = 255;
        if (abs(xy)<255) xy = 255;

        if(runningState == 0) {
                if(runningTime < 1) {
                    SetYawPID(1,0,0);
                    SetRollPID(1,0,0);
                    SetPitchPID(1,0,0);
                    wait(1);
                }
                else if(runningTime < 15){ //15
                    m0 = (acczs + pitchs + rolls) / zpr + 0.5;
                    m1 = (accxs + yaws) / -xy + 0.5;
                    m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
                    //m3 = (accys + yaws) / yy + 0.5;
                    m3 = 0.7;
                    m4 = (acczs - pitchs - rolls) / zpr + 0.5;
                    m5 = (accxs + yaws) / -xy + 0.5;
                    m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
                    //m7 = (-accys + yaws) / -yy + 0.5;
                    m7 = 0.7;
                }
                else if(runningTime < 16){ //16
                    neutralizeMotors();
                    pitchPoint = 0;
                }
                else{
                    m0 = (acczs + pitchs + rolls) / zpr + 0.5;
                    m1 = (accxs + yaws) / -xy + 0.5;
                    m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
                    //m3 = (accys + yaws) / yy + 0.5;
                    m3 = 0.7;
                    m4 = (acczs - pitchs - rolls) / zpr + 0.5;
                    m5 = (accxs + yaws) / -xy + 0.5;
                    m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
                    //m7 = (-accys + yaws) / -yy + 0.5;
                    m7 = 0.7;
                }
        } 
        else if(runningState == 1) {
                if(runningTime < 1) {
                    SetYawPID(2,0,0.3);
                    SetRollPID(2,0,0.3);
                    SetPitchPID(2,0,0.3);
                    lastyawPoint = yawPoint;
                    wait(1);
                } else if(runningTime < 27) { //27
                    //Go straight to Gate
                    m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
                    m1 = (accxs + yaws) / -xy + 0.5;
                    m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
                    m3 = 0.7;
                    m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
                    m5 = (accxs + yaws) / -xy + 0.5;
                    m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
                    m7 = 0.7;
                } else if(runningTime < 28) { //28
                    neutralizeMotors();
                    //Set turning 180 angle aim for Gate
                    yawPoint = correctAngle(lastyawPoint - 160);
                } else if(runningTime < 32) { //32
                    //allign to new yaw
                    m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
                    m1 = (accxs + yaws) / -xy + 0.5;
                    m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
                    m3 = 0.5;
                    m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
                    m5 = (accxs + yaws) / -xy + 0.5;
                    m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
                    m7 = 0.5;
                }  else if(runningTime < 42) { //42
                    //go go go
                    m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
                    m1 = (accxs + yaws) / -xy + 0.5;
                    m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
                    m3 = 0.3;
                    m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
                    m5 = (accxs + yaws) / -xy + 0.5;
                    m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
                    m7 = 0.3;
                } else if(runningTime < 43) { //43
                    neutralizeMotors();
                    //Set turning 180 angle aim for Gate
                    yawPoint = correctAngle(lastyawPoint - 70);
                } else if(runningTime < 47) { //47
                    m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
                    m1 = (accxs + yaws) / -xy + 0.5;
                    m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
                    m3 = 0.5;
                    m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
                    m5 = (accxs + yaws) / -xy + 0.5;
                    m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
                    m7 = 0.5;
                } else if(runningTime < 52) { //52
                    m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
                    m1 = (accxs + yaws) / -xy + 0.5;
                    m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
                    m3 = 0.7;
                    m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
                    m5 = (accxs + yaws) / -xy + 0.5;
                    m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
                    m7 = 0.7;
                }
       //          else if(runningTime < 36) { //53
//                    neutralizeMotors();
//                    //Go Upside down
//                    rollPoint = correctAngle(lastrollPoint + 170);
//                    pc.printf("%f\n", rollPoint);
//                } else if(runningTime < 40) { //57
//                    m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
//                    m1 = (accxs + yaws) / -xy + 0.5;
//                    m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
//                    m3 = 0.5;
//                    m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
//                    m5 = (accxs + yaws) / -xy + 0.5;
//                    m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
//                    m7 = 0.5;
//                }
                 else if(runningTime < 58) { //63
                    //Surface
                    m0 = 0.7;
                    m1 = 0.5;
                    m2 = 0.7;
                    m3 = 0.5;
                    m4 = 0.7;
                    m5 = 0.5;
                    m6 = 0.7;
                    m7 = 0.5;
                }
                else{
                    neutralizeMotors();
                }
            }

            //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
            //pc.printf("%f,%f,%f, %f,%f,%f, %f, %f \n\r",powerPin.read(), ay, yaws, pitchs, rolls, yy, accys, (-accys + yaws - (forwardThrust * forwardThrust)) / -yy + 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, %f\n\r", yaw, pitch, roll, depth);
        }
        
        float correctAngle(float angle){
            if(angle > 180) return (angle - 360);
            else if(angle < -180) return (angle + 360); 
            else return angle;  
        }
            
        void updateCommand() {
            //char a = 0;
//            char i = 0;
//            char buffer[10] = {' '};
//            if (device.readable()) {
//                while(a != 'd') {
//                    a = device.getc();
//                    if ((a >= '0' && a <='9') || a == '.') {
//                        buffer[i] = a;
//                        i++;
//                    }
//                }
//                depth = atof(buffer);
//                pc.printf("Depth: '%f'\n", depth);
//            }
        }

        void initMotors() {

            neutralizeMotors();
            wait(0.5);
            m0.calibrate();
            m1.calibrate();
            m2.calibrate();
            m3.calibrate();
            m4.calibrate();
            m5.calibrate();
            m6.calibrate();
            m7.calibrate();
            wait(3);
        }

        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;
        }

    };

#endif