File content as of revision 0:048a74dd7c3a:

//Robosub Control Interface

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

//------------Declare Objects------------------//
PwmOut      m1(D3);
PwmOut      m2(D4);
PwmOut      m3(D5);
PwmOut      m4(D6);

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

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

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

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

// PWM output variable for each motor
int m1pwm = pwmMin;
int m2pwm = pwmMin;
int m3pwm = pwmMin;
int m4pwm = 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;

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

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

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

void initializePIDs()
{
    pidy.SetMode(AUTOMATIC);            // Yaw PID
    pidy.SetOutputLimits(pwmMin, pwmMax);
    
    pidp.SetMode(AUTOMATIC);            // Pitch PID
    pidp.SetOutputLimits(pwmMin, pwmMax); 
    
    pidr.SetMode(AUTOMATIC);            // Roll PID
    pidr.SetOutputLimits(pwmMin, pwmMax);
    
    pidd.SetMode(AUTOMATIC);            // Depth PID
    pidd.SetOutputLimits(pwmMin, pwmMax); 
    
    depthPoint = 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 \n", yaw, pitch, roll, depth);
    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("Initialize 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();
        break;
        
        case preparePid:
           // pc.printf("Prepare PID \n");
            switch(command)
            {
                case 'I':           // Increase Kp gain
                    kpVal += .1;
                break;
                
                case 'R':
                    if (kpVal > 0 ) // Decreae Kp gain
                    {
                        kpVal -= .1;  
                    }
                break;
                
                case 'S':           // Increase depth
                    depthPoint++;
                
                case 's':           // Decrease setpoint
                    if(depthPoint > 0)
                    {
                        depthPoint--;   
                    }
                break;
                
                default:
                break;
            }
            
            // Set tunings
            pidd.SetTunings(kpVal,0,0);     //Set Ki Kd = 0 
            
            // Print Set parameters
            pc.printf("DepthPoint: %d \n", depthPoint);
            pc.printf("Kp gain: %d \n", pidd.GetKp());
            
        break;
        
        case beginTune:
            //pc.printf("Tuning process begins.. \n");
            
            // Sense inertial / depth data
            IMUPrintData(mpu1, compass);
            readDepth();
            
            // Assign feedback variables
            myDepth = depth;
            
            // Compute PID
            pidd.Compute();
            
            // Output pwm to motors                     // FIXME : account for pitch, roll and maybe yaw depending on motor configuration
            m1pwm = m2pwm = m3pwm = m4pwm = depthOut;
            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;
    
    while(1)
    {
        // Read user input
        readUserInput();
        
        // Begin Interface
        controlInterface();
    }
}