File content as of revision 4:b37fd183e46a:

#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 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;
    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(-255,255);
        yawPoint = 0;
        pidr.SetMode(AUTOMATIC);  //Yaw PID
        pidr.SetOutputLimits(-255,255);
        rollPoint = 0;
        pidp.SetMode(AUTOMATIC);  //Yaw PID
        pidp.SetOutputLimits(-255,255);
        rollPoint = 0;
        
        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) {
        pidr.SetTunings(Kp, Ki, Kd);
    }

    void SetPitchPID(double Kp, double Ki, double Kd) {
        pidp.SetTunings(Kp, Ki, Kd);
    }
    
    //This is where the magic happens
    void update(){
        //Update IMU Values
        IMUUpdate(mpu6050);
        yawIn = yaw;
        rollIn = roll;
        pitchIn = pitch;
        
        //Calculate PID values
        pidy.Compute();
        //pidr.Compute();
        //pidp.Compute();
        
        //Spit out PID values
        double yo = abs(yawOut/255); //Dividing once to reduce overhead
        
        m0 = yo;
        m1 = yo;
        m2 = yo;
        m3 = yo;
        m4 = yo;
        m5 = yo;
        m6 = yo;
        m7 = yo;
        
        pc.printf("yaw0: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint);
    }
};
#endif