File content as of revision 6:b45b74fd6a07:

#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>
/*
            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;
    
    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 p_gain, i_gain, d_gain;
    PID pidy, pidr, pidp, pidX, pidY, pidZ;
    char buffer[BUFFER_SIZE];
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(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)
        {

        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;
        
        m0 = 0.5;
        m1 = 0.5;
        m2 = 0.5;
        m3 = 0.5;
        m4 = 0.5;
        m5 = 0.5;
        m6 = 0.5;
        m7 = 0.5;
        
        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);
    }
    
    //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");
    }
    
    void update(){
        //Update IMU Values
        IMUUpdate(mpu6050);
        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 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; 
        
//        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) / zpr + 0.5;//
        m1 = (accys + yaws) / yy + 0.5;
        m2 = (acczs + pitchs - rolls) / zpr + 0.5;//
        m3 = (accxs + yaws) / xy + 0.5;
        m4 = (acczs - pitchs - rolls) / zpr + 0.5;//
        m5 = (accys + yaws) / yy + 0.5;
        m6 = (acczs - pitchs + rolls) / 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 \n\r",acczs, pitchs, rolls, zpr, (acczs + pitchs + rolls) / 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\n\r", yaw, pitch, roll);
    }
    
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);
    }
}
};

#endif