File content as of revision 7:396fa2a8648d:

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

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

      0  ----- 1 ->--- 2
      |        |       |
      ˄        |       |
      |        |       |
      7        |       3
      |        |       |
      |        |       ˅
      |        |       |
      6  ---<- 5 ----- 4
*/
#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;
//    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, 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);
    }

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

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

    //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--){
////            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() {
        //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();
        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);

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

//        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(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(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);//
//        m3 = abs((accxs + yaws) / xy);
//        m4 = abs((acczs - pitchs - rolls) / zpr);//
//        m5 = abs((accys + yaws) / yy);
//        m6 = abs((acczs - pitchs + rolls) / zpr);//
//        m7 = abs((accxs + yaws) / yy);

        m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;//
        m1 = (accys + yaws) / yy + 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 - 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,%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, %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 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