File content as of revision 1:3f291f2f80d3:

//Robosub Control Interface

#include "mbed.h"
#include "IMU.h"
#include "PID.h"
#include "HMC5883L.h"

//------------Declare Objects------------------//

//      (m1)         (m2)
//          |       |
//          |       |
// (Ltrhust)|       |(Rthrust)
//          |       |
//          |       |    
//      (m3)         (m4)   

PwmOut      m1(D3);
PwmOut      m2(D4);
PwmOut      m3(D5);
PwmOut      m4(D6);

PwmOut      Lthrust(D7);
PwmOut      Rthrust(D8);

// Declare mpu6050 and compass object
MPU6050 mpu1;
HMC5883L compass;

// Serial communication between STM & Arduino NANO
RawSerial device(PA_11, PA_12);  //TX RX

// Serial communication between STM & Raspberry PI
RawSerial devicePI(D10,D2);

//-----------Global Variables------------------//
char command = 0;
Timer calibrate;
Timer data;

int pwmMax = 1100;      // Reversed due to motor orientation
int pwmMin = 1500;

const int IDLE = 1500;
const int MAXYAW = 100;
const int MAXPITCH = 100;
const int MAXROLL = 100;

int thrust = 0;

int j = -1;      // j == 1 --> FWD ... j == -1 --> REV      


// PWM output variable for each motor
int m1pwm = pwmMin;
int m2pwm = pwmMin;
int m3pwm = pwmMin;
int m4pwm = pwmMin;
int Lthrustpwm = pwmMin;
int Rthrustpwm = pwmMin;

// Individual pid parameters
double myYaw, yawPoint, yawOut;
double myPitch, pitchPoint, pitchOut;
double myRoll, rollPoint, rollOut;
double myDepth, depthPoint, depthOut;

int depth = 1;

double kpVal = 0;
double kiVal = 0;
double kdVal = 0;

//-----------End Global Variables--------------//


            //----PID objects------//

// Input, Output, SetPoint, kp, ki, kd, Controller Direction
PID pidy(&myYaw, &yawOut, &yawPoint, 1, 1, 1, DIRECT);
PID pidp(&myPitch, &pitchOut, &pitchPoint, 1, 1, 1, DIRECT);
PID pidr(&myRoll, &rollOut, &rollPoint, 1, 1, 1, DIRECT);
PID pidd(&myDepth, &depthOut, &depthPoint, 1, 1, 1, DIRECT);

            //----End PID objects--//


//-----------Helper functions------------------//
void calibrateFilter()
{
    calibrate.start();
    while(calibrate.read() < 5)
    {
        IMUPrintData(mpu1, compass);
        
        char text[90];
        sprintf(text, "%f,%f,%f \n", yaw, pitch, roll);
        pc.printf("%s", text);
    }
    calibrate.stop();    
}

void updateMotors()
{
    m1.pulsewidth_us(m1pwm);
    m2.pulsewidth_us(m2pwm);
    m3.pulsewidth_us(m3pwm);
    m4.pulsewidth_us(m4pwm);
    Lthrust.pulsewidth_us(Lthrustpwm);
    Rthrust.pulsewidth_us(Rthrustpwm);
}

void neutralizeMotors()
{
    m1pwm = pwmMin;
    m2pwm = pwmMin;
    m3pwm = pwmMin;
    m4pwm = pwmMin;
    Lthrustpwm = pwmMin;
    Rthrustpwm = pwmMin;
    updateMotors(); 
}

void vesselGo()
{
    thrust = 200;   
}
void vesselHover()
{
    thrust = 0;
}

void readUserInput()
{
    if(pc.readable())
    {
         command = pc.getc();
    }  
}

void updateYawThrottle()
{
    Lthrustpwm = IDLE + (j*(thrust + yawOut));
    Rthrustpwm = IDLE + (j*(thrust + (MAXYAW-yawOut)));
}

void initializePIDs()
{
    pidy.SetMode(AUTOMATIC);            // Yaw PID
    pidy.SetOutputLimits(0, MAXYAW);
    
    pidp.SetMode(AUTOMATIC);            // Pitch PID
    pidp.SetOutputLimits(0, MAXPITCH); 
    
    pidr.SetMode(AUTOMATIC);            // Roll PID
    pidr.SetOutputLimits(0, MAXROLL);
    
    pidd.SetMode(AUTOMATIC);            // Depth PID
    pidd.SetOutputLimits(0,300); 
    
    depthPoint = 0;
    pitchPoint = 0;
    rollPoint = 0;
}

void readDepth()
{
    // Read pressure sensor data if available
    if (device.readable())
    {
        // Receive depth in inches as an integer
        depth = device.getc();
            
        // Convert to feet
    }
}

void displayTelemetry()
{
    char text[90];
    sprintf(text, "%f,%f,%f,%d,%f \n", yaw, pitch, roll, depth, depthPoint);
    pc.printf("%s", text);
}

//-----------End Helper functions--------------//



//-----------Interface States------------------//
enum controlStates { init, idle, manual, preparePid, beginTune } controlState;

void controlInterface(){
    
    switch(controlState)            //Actions
    {
        case init:
            pc.printf("Initialize sensors \n");
            IMUinit(mpu1);
            compass.init();
            
            pc.printf("Initialirze motors  \n");
            neutralizeMotors();
            
            pc.printf("Initialize PID objects \n");
            initializePIDs();
            
            pc.printf("Calibrate MARG Filter \n");
            calibrateFilter();
        break;
        
        case idle:
            IMUPrintData(mpu1, compass);
            readDepth();
            displayTelemetry();
        break;
        
        case manual:
            //pc.printf("Manual Control \n");
            switch(command)
            {
                case 'N':
                    //pc.printf("Neutralize motors\n");
                    neutralizeMotors();
                break;
                
                case 'R':
                    //pc.printf("Reduce Thrust\n");
                    if (m1pwm < pwmMin)
                    {
                        m1pwm++;   
                        m2pwm++;
                        m3pwm++;
                        m4pwm++;
                    }
                break;
                
                case 'I':
                    //pc.printf("Increase Thrust\n");
                    if (m1pwm > pwmMax)
                    {
                        m1pwm--; 
                        m2pwm--;
                        m3pwm--;
                        m4pwm--;
                    }
                break;
                
                default:
                break;
            }
            updateMotors();
            displayTelemetry();
        break;
        
        case preparePid:
           // pc.printf("Prepare PID \n");
            switch(command)
            {
                case 'K':           // Increase Kp gain
                    kpVal += .1;
                break;
                
                case 'k':
                    if (kpVal > 0 ) // Decreae Kp gain
                    {
                        kpVal -= .1;  
                    }
                break;
                
                case 'I':
                    kiVal += .1;
                break;
                
                case 'i':
                    if (kiVal > 0)
                    {
                        kiVal -= .1;
                    }
                break;
                
                case 'D':
                    kdVal += .1;
                break;
                
                case 'd':
                    if (kdVal > 0)
                    {
                        kdVal -= .1;
                    }                    
                break;
                
                case 'S':           // Increase depth
                    depthPoint++;
                
                case 's':           // Decrease setpoint
                    if(depthPoint > 0)
                    {
                        depthPoint--;   
                    }
                break;
                
                default:
                break;
            }
            
            // Set tunings
            pidd.SetTunings(kpVal,kiVal,kdVal);     //Set Ki Kd = 0 
            //pidy.SetTunings(kpVal,kiVal,kdVal);
            //pidp.SetTunings(kpVal,kiVal,kdVal);
            //pidr.SetTunings(kpVal,kiVal,kdVal);
            
            if (data.read_ms() % 500 == 0)
            {
                // Print Set parameters
                pc.printf("DepthPoint: %d \n", depthPoint);
                pc.printf("Kp gain: %d \n", pidd.GetKp());
                pc.printf("Ki gain: %d \n", pidd.GetKi());
                pc.printf("Kd gain: %d \n", pidd.GetKd());
            }
        break;
        
        case beginTune:
            //pc.printf("Tuning process begins.. \n");
            
            // Sense inertial / depth data
            IMUPrintData(mpu1, compass);
            readDepth();
            
            // Assign feedback variables
            myDepth = depth;
            //myYaw = yaw;
            //myPitch = pitch;
            //myRoll = Roll;
            
            // Compute PID
            pidd.Compute();
            //pidy.Compute();
            //pidp.Compute();
            //pidr.Compute();
            
            // Output pwm to motors                     // FIXME : account for pitch, roll and maybe yaw depending on motor configuration
            m1pwm = IDLE + depthOut; // + pitchOut 
            m2pwm = IDLE + depthOut; // + (MAXPITCH - pitchOut)
            m3pwm = IDLE + depthOut; // + pitchOut
            m4pwm = IDLE + depthOut; // + (MAXPITCH - pitchOut)
            
            //updateYawThrottle();
            updateMotors();
            
            // Display telemetry
            displayTelemetry();
            
        break;
        
        default:
            pc.printf("Error! \n");
        break;
    }
    
    switch(controlState)            //Transitions
    {
        case init:
            controlState = idle;
        break;
        
        case idle:
            controlState = (command == 'm') ? manual : ((command == 'p') ? preparePid : idle);
        break;
        
        case manual:
            controlState = (command == 'p') ? preparePid : manual;
            if (controlState == preparePid)
            {
                //pc.printf("Neutralize Motors \n"); 
                neutralizeMotors();  
            }
        break;
        
        case preparePid:
            controlState = (command == 'g') ? beginTune : preparePid;
        break;
        
        case beginTune:
            controlState = (command == 't') ? preparePid : beginTune;
            if (controlState == preparePid)
            {
                //pc.printf("Neutralize Motors \n");
                neutralizeMotors();
            }
        break;
        
        default:
        break;
    }
}

//-----------End Interface---------------------//

int main() {
    // Initialize control state
    controlState = init;
    data.start();
    while(1)
    {
        // Read user input
        readUserInput();
        
        // Begin Interface
        controlInterface();
    }
}