File content as of revision 3:5ffe7e9c0bb3:

#ifndef VESSEL_H
#define VESSEL_H

#include "mbed.h"
#include "MPU6050.h"
#include "Servo.h"
#include "IMU.h"
#include "PID.h"
/*
            Cameras
      FL ----- F ->--- FR
      |        |       |
      ˄        |       |
      |        |       |
      L        |       R
      |        |       |
      |        |       ˅
      |        |       |
      BL ---<- B ----- BR

      0  ----- 1 ->--- 2
      |        |       |
      ˄        |       |
      |        |       |
      7        |       3
      |        |       |
      |        |       ˅
      |        |       |
      6  ---<- 5 ----- 4
*/
class Vessel
{

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

    PwmOut led1;
    MPU6050 mpu6050;
    double yawPoint, yawIn, yawOut;
    double rollPoint, rollIn, rollOut;
    double pitchPoint, pitchIn, pitchOut;
    PID pidy, pidr, pidp;

public:
    void Start_IMU() {
        pc.printf("Starting up\n\r");
        pc.baud(9600);
        i2c.frequency(400000);  // use fast (400 kHz) I2C
        IMUinit(mpu6050);
        IMUPrintData(mpu6050);
    }

    //Initialise all of the vessels starting parameters
    Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D9), 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) {

        pidy.SetMode(AUTOMATIC);  //Yaw PID
        pidy.SetOutputLimits(0,255);
        yawPoint = 125;
        pidr.SetMode(AUTOMATIC);  //Yaw PID
        pidr.SetOutputLimits(0,255);
        rollPoint = 125;
        pidp.SetMode(AUTOMATIC);  //Yaw PID
        pidp.SetOutputLimits(0,255);
        rollPoint = 125;
        
        Start_IMU();
        pc.printf("Seagoat Ready to Go\n\r");
    }

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

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

    void SetPitchPID(double Kp, double Ki, double Kd) {
        pidy.SetTunings(Kp, Ki, Kd);
    }
    
    //This is where the magic happens
    void update(){
            
    }
};
#endif