File content as of revision 3:394c971eab83:

#include "IMU.h"
#include "PID.h"
#include "MS5837.h"
#include "Motor.h"
#include "HMC5883L.h"

// Maps value from incoming analog signal to correct range
// to be sent out to as pwm signal to motors
float map(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

double myPitch, sOut, setPoint;
double k_p, k_i, k_d;

//Declare digital button input
DigitalIn tuningButton(USER_BUTTON);


// Declare mpu object
MPU6050 mpu1;
HMC5883L compass;

MS5837 pressure_sensor = MS5837(D14, D15, ms5837_addr_no_CS);

// Declare motor objects
Motor mBlack(D3,D2,D9); // pwm, fwd, rev
Motor mWhite(D4,D5,D8);

// Declare analog input pin 
AnalogIn    kpKnob(A0);
AnalogIn    kiKnob(A1);
AnalogIn    kdKnob(A2);

// Input, Output, SetPoint, kp, ki, kd, Controller Direction
PID pidp(&myPitch, &sOut, &setPoint, 1, 1, 1, DIRECT);

//Serial pc(USBTX, USBRX);

int main()
{   
    // Initialize IMU
    IMUinit(mpu1);
    compass.init();
    // Initialize pressure sensor
 //   pressure_sensor.MS5837Init();
    
    // Initialize PID 
    pidp.SetMode(AUTOMATIC);    
    pidp.SetOutputLimits(0.5, 1);
    
    //Default kp ki kd parameters
    float kpKnobVal = .028;
    float kiKnobVal = .01;
    float kdKnobVal = .025;
    pidp.SetTunings(kpKnobVal, kiKnobVal, kdKnobVal);
    setPoint = 0;
    
   // float pressure;

    while(1)
    {    
    
        // Read pressure sensor data
       // pressure_sensor.Barometer_MS5837();
       // pressure = pressure_sensor.MS5837_Temperature();
       // pc.printf("pressure: %f \n", pressure);
        
        // If button is pressed kp ki kd values can be adjusted
        if(!tuningButton)
        {
            // Read raw potentiometer values from k-knob and map to kpknobVal
            kpKnobVal = map(kpKnob.read(), 0.000, 1.000, 0.000, .050);
            kiKnobVal = map(kiKnob.read(), 0.000, 1.000, 0.000, 0.008);
            kdKnobVal = map(kdKnob.read(), 0.000, 1.000, 0.000, .040);
            
            // Adjust tunings
            pidp.SetTunings(kpKnobVal,kiKnobVal,kdKnobVal);
            
        }
        //print mapped joystick values
       // pc.printf("kp: %f -- ki: %f -- kd %f \n", kpKnobVal, kiKnobVal, kdKnobVal);  
        
        // Obtain mpu data -> pass through filter -> obtain yaw pitch roll
        IMUPrintData(mpu1, compass);
        //myPitch = pitch;
        
       // char textA[90];
       // sprintf(textA, "%f,%f,%f,%f \n", heading, magdata[0], magdata[1], heading);
       // pc.printf("%s", textA);
        
        // Compute output using pid controller
        //pidp.Compute();
        
        // Send pwm output to Motors
        //float s2Out = 1.5 - sOut;
        //mWhite.speed(s2Out); 
        //mBlack.speed(-sOut);
                
        //Display telemetry
        char text[90];
        sprintf(text, "%f,%f,%f \n", yaw, pitch, roll);
        pc.printf("%s", text);
        
        wait(.01);
    }
}