File content as of revision 11:1b838130dc8c:

#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

/* Thee
This is the vessel class this class is meant to be the top level function for 
sub. 
The functionalities are as follows:
    - Initialize sensors and PID control loops
    - update:
        - Update sensor values
        - Then updates controller values
        - mix controller output to actuator output
*/
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
    // <axis>In = input to PID loop
    // <axis>Out = output from PID loop
    // <axis>Point = is the setpoint aka the desired output of PID loop
    ////////////////////////////////////////////////////////////////////////////
    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;
        
        // .5 is null .5> is reverse and forward is >.5
        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");
    }
    
    ////////////////////////////////////////////////////////////////////////////
    // Functions for setting PID gains
    ////////////////////////////////////////////////////////////////////////////
    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);
    }
/////////////////////////////////////////////////////////////////////////////////


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

    void update() {
        
        ///////////////////////////////////////////////
        //Update IMU Values and store values them in:
        //    ax, ay, az, gx, gy, gz, yaw, pitch, roll 
        // (a<axis> and g<axis> are raw sensor values and y, p, r are madgwick values
        ////////////////////////////////////////////////
        IMUUpdate(mpu6050);
        ////////////////////////////////////////////////


        //////////////////////////////////////////////////////////////////////
        // CAUTION max centimeter level resolution takes a really long time to 
        // calculate. Reduce sensor resolution if you want to increase the update
        // rate. Alternatively you can offload the pressure measurement to another
        // MCU or a RTOS can be used so that the updates don't affect other 
        // processes. Note that this version of mbed that was used to code this
        // does not support i2c. If a i2c read is called in a thread the MCU will
        // lock up
        ////////////////////////////////////////////////////////////////////////
        //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;
        }

        // Sets sensor values as Input (feedback) to the PID loops
        yawIn = yaw;
        rollIn = roll;
        pitchIn = pitch;
        xIn = ax;
        yIn = ay;
        zIn = az;

        //Calculate PID output 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
        // I modified this section of marcos code because he was essentially
        // squaring the output of the PID loops. I don't know why he squares it
        // I think the same thing can be accomplished by just taking the absolute
        // values of the output... I highly recommend removing this step. I am
        // just keeping it in so you can see what we did.
        float accxs = xOut ^ 2;
        float accys = yOut ^ 2;
        float acczs = zOut ^ 2;
        float depths = dOut ^ 2;
        float yaws = yawOut ^ 2;
        float pitchs = pitchOut ^ 2; 
        float rolls = rollOut ^ 2;

        //Values used for Influence calculations
        // This is part of the method marco used for Pitch and Roll control
        // I would recomend looking at how quadcopter controllers are designed.
        // You can see example code here:
        //      https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control/mc_att_control_main.cpp
        // 
        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;
        /////////////////////////////////////////////////////////////////////////
        // Running state 0 just moves forward
        // Running state 1 moves forward then trys to do a 180 just before the 
        // navigation gate. Running state is toggled every time the main power 
        // switch is toggled. 
        // Also this sections mixes the desired control response to the proper acuators.
        ////////////////////////////////////////////////////////////////////////
        if(runningState == 0) {
                if(runningTime < 1) {
                    // Set PID gain
                    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();
                }
            }
            ////////////////////////////////////////////////////////////////////
            // Debugging output ////////////////////////////////////////////////
            //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);
        }
        
        // Handles rotational singularities (could also use quaternions...)
        float correctAngle(float angle){
            if(angle > 180) return (angle - 360);
            else if(angle < -180) return (angle + 360); 
            else return angle;  
        }
        
        // Update command reads a string from the pc via uart. This wasn't used
        // during the competition because we didn't have a PC. I don't rember if
        // still functions properly. 
        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