Revision 7:396fa2a8648d, committed 2016-07-29

Comitter:

aolgu003

Date:

Fri Jul 29 15:34:59 2016 +0000

Parent:

6:b45b74fd6a07

Commit message:

Fixed issues and integrated with the sub.

Changed in this revision

--- a/IMU.h	Wed Jul 27 02:45:45 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,219 +0,0 @@
-
-#include "MPU6050.h"
-#include "communication.h"
-
-float sum = 0;
-uint32_t sumCount = 0;
-
-Timer t;
-
-void IMUinit(MPU6050 &mpu6050)
-{
-    t.start();
-
-// Read the WHO_AM_I register, this is a good test of communication
-    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
-    pc.printf("I AM 0x%x\n\r", whoami);
-    pc.printf("I SHOULD BE 0x68\n\r");
-
-    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
-        pc.printf("MPU6050 is online...");
-        wait(1);
-        //lcd.clear();
-        //lcd.printString("MPU6050 OK", 0, 0);
-
-
-        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
-        pc.printf("x-axis self test: acceleration trim within : ");
-        pc.printf("%f", SelfTest[0]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("y-axis self test: acceleration trim within : ");
-        pc.printf("%f", SelfTest[1]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("z-axis self test: acceleration trim within : ");
-        pc.printf("%f", SelfTest[2]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("x-axis self test: gyration trim within : ");
-        pc.printf("%f", SelfTest[3]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("y-axis self test: gyration trim within : ");
-        pc.printf("%f", SelfTest[4]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("z-axis self test: gyration trim within : ");
-        pc.printf("%f", SelfTest[5]);
-        pc.printf("% of factory value \n\r");
-        wait(1);
-
-        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
-            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
-            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
-            mpu6050.initMPU6050();
-            pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
-            wait(2);
-        } else {
-            pc.printf("Device did not the pass self-test!\n\r");
-        }
-    } else {
-        pc.printf("Could not connect to MPU6050: \n\r");
-        pc.printf("%#x \n",  whoami);
-
-        while(1) ; // Loop forever if communication doesn't happen
-    }
-}
-
-
-void IMUPrintData(MPU6050 &mpu6050)
-{
-// If data ready bit set, all data registers have new data
-    if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
-        mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
-        mpu6050.getAres();
-
-        // Now we'll calculate the accleration value into actual g's
-        ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
-        ay = (float)accelCount[1]*aRes - accelBias[1];
-        az = (float)accelCount[2]*aRes - accelBias[2];
-
-        mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
-        mpu6050.getGres();
-
-        // Calculate the gyro value into actual degrees per second
-        gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
-        gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
-        gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
-
-        tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
-        temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
-    }
-
-    Now = t.read_us();
-    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
-    lastUpdate = Now;
-
-    sum += deltat;
-    sumCount++;
-
-    if(lastUpdate - firstUpdate > 10000000.0f) {
-        beta = 0.04;  // decrease filter gain after stabilized
-        zeta = 0.015; // increasey bias drift gain after stabilized
-    }
-
-    // Pass gyro rate as rad/s
-    mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
-
-    // Serial print and/or display at 0.5 s rate independent of data rates
-    delt_t = t.read_ms() - count;
-    if (delt_t > 500) { // update LCD once per half-second independent of read rate
-
-        pc.printf("ax = %f", 1000*ax);
-        pc.printf(" ay = %f", 1000*ay);
-        pc.printf(" az = %f  mg\n\r", 1000*az);
-
-        pc.printf("gx = %f", gx);
-        pc.printf(" gy = %f", gy);
-        pc.printf(" gz = %f  deg/s\n\r", gz);
-
-        pc.printf(" temperature = %f  C\n\r", temperature);
-
-        pc.printf("q0 = %f\n\r", q[0]);
-        pc.printf("q1 = %f\n\r", q[1]);
-        pc.printf("q2 = %f\n\r", q[2]);
-        pc.printf("q3 = %f\n\r", q[3]);
-
-        // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
-        // In this coordinate system, the positive z-axis is down toward Earth.
-        // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
-        // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
-        // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
-        // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
-        // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
-        // applied in the correct order which for this configuration is yaw, pitch, and then roll.
-        // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
-        yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-        pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-        roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-        pitch *= 180.0f / PI;
-        yaw   *= 180.0f / PI;
-        roll  *= 180.0f / PI;
-
-//    pc.printf("Yaw, Pitch, Roll: \n\r");
-//    pc.printf("%f", yaw);
-//    pc.printf(", ");
-//    pc.printf("%f", pitch);
-//    pc.printf(", ");
-//    pc.printf("%f\n\r", roll);
-//    pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
-
-        pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-        pc.printf("average rate = %f\n\r", (float) sumCount/sum);
-
-        //myled= !myled;
-        count = t.read_ms();
-        sum = 0;
-        sumCount = 0;
-    }
-}
-
-void IMUUpdate(MPU6050 &mpu6050)
-{
-    // If data ready bit set, all data registers have new data
-    if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
-        mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
-        mpu6050.getAres();
-
-        // Now we'll calculate the accleration value into actual g's
-        ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
-        ay = (float)accelCount[1]*aRes - accelBias[1];
-        az = (float)accelCount[2]*aRes - accelBias[2];
-
-        mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
-        mpu6050.getGres();
-
-        // Calculate the gyro value into actual degrees per second
-        gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
-        gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
-        gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
-
-        tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
-        temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
-    }
-
-    Now = t.read_us();
-    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
-    lastUpdate = Now;
-
-    sum += deltat;
-    sumCount++;
-
-    if(lastUpdate - firstUpdate > 10000000.0f) {
-        beta = 0.04;  // decrease filter gain after stabilized
-        zeta = 0.015; // increasey bias drift gain after stabilized
-    }
-
-    // Pass gyro rate as rad/s
-    mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
-
-    // Serial print and/or display at 0.5 s rate independent of data rates
-    delt_t = t.read_ms() - count;
-
-    // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
-    // In this coordinate system, the positive z-axis is down toward Earth.
-    // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
-    // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
-    // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
-    // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
-    // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
-    // applied in the correct order which for this configuration is yaw, pitch, and then roll.
-    // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
-    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-    pitch *= 180.0f / PI;
-    yaw   *= 180.0f / PI;
-    roll  *= 180.0f / PI;
-
-    count = t.read_ms();
-    sum = 0;
-    sumCount = 0;
-
-}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.lib	Fri Jul 29 15:34:59 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/aolgu003/code/IMU/#9a71704df32d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MS5837/MS5837.cpp	Fri Jul 29 15:34:59 2016 +0000
@@ -0,0 +1,100 @@
+#include <stdlib.h>
+#include "MS5837.h"
+
+
+/*
+ * Sensor operating function according data sheet
+ */
+
+void MS5837::MS5837Init(void)
+{
+    MS5837Reset();
+    MS5837ReadProm();
+    return;
+}
+
+/* Send soft reset to the sensor */
+void MS5837::MS5837Reset(void)
+{
+    /* transmit out 1 byte reset command */
+    ms5837_tx_data[0] = ms5837_reset;
+    if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
+    //printf("send soft reset");
+    wait_ms(20);
+}
+
+/* read the sensor calibration data from rom */
+void MS5837::MS5837ReadProm(void)
+{
+    uint8_t i,j;
+    for (i=0; i<8; i++) {
+        j = i;
+        ms5837_tx_data[0] = ms5837_PROMread + (j<<1);
+        if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
+        if ( i2c.read( device_address,  ms5837_rx_data, 2 ) );
+        C[i]   = ms5837_rx_data[1] + (ms5837_rx_data[0]<<8);
+    }
+}
+
+/* Start the sensor pressure conversion */
+void MS5837::MS5837ConvertD1(void)
+{
+    ms5837_tx_data[0] = ms5837_convD1;
+    if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
+}
+
+/* Start the sensor temperature conversion */
+void MS5837:: MS5837ConvertD2(void)
+{
+    ms5837_tx_data[0] = ms5837_convD2;
+    if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
+}
+
+/* Read the previous started conversion results */
+int32_t MS5837::MS5837ReadADC(void)
+{
+    int32_t adc;
+    wait_ms(150);
+    ms5837_tx_data[0] = ms5837_ADCread;
+    if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
+    if ( i2c.read( device_address,  ms5837_rx_data, 3 ) );
+    adc = ms5837_rx_data[2] + (ms5837_rx_data[1]<<8) + (ms5837_rx_data[0]<<16);
+    return (adc);
+}
+
+/* return the results */
+float MS5837::MS5837_Pressure (void)
+{
+    return P_MS5837;
+}
+float MS5837::MS5837_Temperature (void)
+{
+    return T_MS5837;
+}
+
+/* Sensor reading and calculation procedure */
+void MS5837::Barometer_MS5837(void)
+{
+    int32_t dT, temp;
+    int64_t OFF, SENS, press;
+
+    //no need to do this everytime!
+    //MS5837Reset();                 // reset the sensor
+    //MS5837ReadProm();             // read the calibration values
+    
+    
+    MS5837ConvertD1();             // start pressure conversion
+    D1 = MS5837ReadADC();        // read the pressure value
+    MS5837ConvertD2();             // start temperature conversion
+    D2 = MS5837ReadADC();         // read the temperature value
+
+    /* calculation according MS5837-01BA data sheet DA5837-01BA_006 */
+    dT       = D2 - (C[5]* 256);
+    OFF      = (int64_t)C[2] * (1<<16) + ((int64_t)dT * (int64_t)C[4]) / (1<<7);
+    SENS     = (int64_t)C[1] * (1<<15) + ((int64_t)dT * (int64_t)C[3]) / (1<<8);
+
+    temp     = 2000 + (dT * C[6]) / (1<<23);
+    T_MS5837 = (float) temp / 100.0f;                 // result of temperature in deg C in this var
+    press    = (((int64_t)D1 * SENS) / (1<<21) - OFF) / (1<<13);
+    P_MS5837 = (float) press / 10.0f;                 // result of pressure in mBar in this var
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MS5837/MS5837.h	Fri Jul 29 15:34:59 2016 +0000
@@ -0,0 +1,64 @@
+#include "mbed.h"
+
+#ifndef MS5837_H
+#define MS5837_H
+
+#define MS5837_RX_DEPTH 3 //
+#define MS5837_TX_DEPTH 2 //
+
+// choose your connection here
+#define ms5837_addr_no_CS  0x76 //0b1110110
+
+#define ms5837_reset       0x1E // Sensor Reset
+
+#define ms5837_convD1_256  0x40 // Convert D1 OSR 256
+#define ms5837_convD1_512  0x42 // Convert D1 OSR 512
+#define ms5837_convD1_1024 0x44 // Convert D1 OSR 1024
+#define ms5837_convD1_2048 0x46 // Convert D1 OSR 2048
+#define ms5837_convD1_4096 0x48 // Convert D1 OSR 4096
+#define ms5837_convD1_8192 0x4A // Convert D1 OSR 8192
+
+#define ms5837_convD1 ms5837_convD1_4096 // choose your sampling rate here
+
+#define ms5837_convD2_256  0x50 // Convert D2 OSR  256
+#define ms5837_convD2_512  0x52 // Convert D2 OSR  512
+#define ms5837_convD2_1024 0x54 // Convert D2 OSR 1024
+#define ms5837_convD2_2048 0x56 // Convert D2 OSR 2048
+#define ms5837_convD2_4096 0x58 // Convert D2 OSR 4096
+#define ms5837_convD2_8192 0x5A // Convert D2 OSR 8192
+
+#define ms5837_convD2 ms5837_convD2_4096 // choose your sampling rate here
+
+#define ms5837_ADCread     0x00 // read ADC command
+#define ms5837_PROMread    0xA0 // read PROM command base address
+
+class MS5837{
+private:
+    int D1, D2, Temp, C[8];
+    float T_MS5837, P_MS5837;
+    /* Data buffers */
+    char ms5837_rx_data[MS5837_RX_DEPTH];
+    char ms5837_tx_data[MS5837_TX_DEPTH];
+
+public:
+    MS5837 (PinName sda, PinName scl,
+            char ms5837_addr = ms5837_addr_no_CS  )
+            : i2c( sda, scl ), device_address( ms5837_addr << 1 ) {
+    }
+    void MS5837Init(void);
+    void MS5837Reset(void);
+    void MS5837ReadProm(void);
+    void MS5837ConvertD1(void);
+    void MS5837ConvertD2(void);
+    int32_t MS5837ReadADC(void);
+    float MS5837_Pressure (void);
+    float MS5837_Temperature (void);
+    void Barometer_MS5837(void);
+
+
+private:
+    I2C     i2c;
+    char    device_address;
+
+};
+#endif

--- a/main.cpp	Wed Jul 27 02:45:45 2016 +0000
+++ b/main.cpp	Fri Jul 29 15:34:59 2016 +0000
@@ -1,22 +1,23 @@
 // Continuously sweep the servo through it's full range
 #include "mbed.h"
 #include "vessel.h"
-
+//#include "MS5803.h"
+//MS5803 big_sensor = MS5803(I2C_SDA, I2C_SCL, ms5803_addrCH);
 
 int main()
 {  
     Timer t;
     wait(3);
     Vessel seagoat; //Starts the seagoat
-    seagoat.SetYawPID(4,0,0);  
-    seagoat.SetRollPID(4,0,0);    
-    seagoat.SetPitchPID(4,0,0);    
+    seagoat.SetYawPID(1,0,0);  
+    seagoat.SetRollPID(1,0,0);    
+    seagoat.SetPitchPID(1,0,0);    
     seagoat.SetXPID(0,0,0);    
     seagoat.SetYPID(0,0,0);    
     seagoat.SetZPID(0,0,0);    
     
     t.start();
-    while(t.read() < 5){
+    while(t.read() < 4){
          seagoat.calibrate();
          
          //pc.printf("%f \n", t.read());
@@ -24,9 +25,8 @@
     t.stop();
     pc.printf("Seagoat Ready to Go\n\r");
     //seagoat.motorTest();
-    
+    float depth = 0;
     while(1) {
-        
         //seagoat.motorTest();
         seagoat.update();
         seagoat.updateCommand();

--- a/vessel.h	Wed Jul 27 02:45:45 2016 +0000
+++ b/vessel.h	Fri Jul 29 15:34:59 2016 +0000
@@ -8,6 +8,10 @@
 #include "IMU.h"
 #include "PID.h"
 #include <string>
+#include "MS5837.h"
+
+MS5837 pressure_sensor = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS);
+
 /*
             Cameras
       FL ----- F ->--- FR
@@ -36,41 +40,48 @@
 {
 
 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;
+    Servo m0;
+    Servo m1;
+    Servo m2;
+    Servo m3;
+    Servo m4;
+    Servo m5;
+    Servo m6;
+    Servo m7;
+
+    AnalogIn powerPin;
+    int motorState;
+//    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 dPoint, dIn, dOut;
     double p_gain, i_gain, d_gain;
-    PID pidy, pidr, pidp, pidX, pidY, pidZ;
+    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);
     }
 
@@ -78,11 +89,13 @@
     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), 
+        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)
-        {
+        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);
@@ -102,7 +115,11 @@
         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;
+
         m0 = 0.5;
         m1 = 0.5;
         m2 = 0.5;
@@ -112,6 +129,8 @@
         m6 = 0.5;
         m7 = 0.5;
         
+        motorState = 1;
+
         Start_IMU();
         pc.printf("Seagoat Initialized \n\r");
     }
@@ -139,44 +158,67 @@
     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);
+    }
+
     //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--){
+//    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);
-//        }   
-    }
-    void calibrate(){
+//        }
+//   //     for(float i = 80; i >= 0; i--){
+////            motor = i/255;
+////            wait(0.002);
+////        }
+//    }
+    void calibrate() {
         IMUUpdate(mpu6050);
         pc.printf("Calibrating...\n\r");
+        //pressure_sensor.Barometer_MS5837();
+        //depth = pressure_sensor.MS5837_Pressure();
     }
-    
-    void update(){
+
+    void update() {
         //Update IMU Values
         IMUUpdate(mpu6050);
+
+        //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){
+            initMotors(); 
+            motorState = 1;  
+            pc.printf("Motors Detected");
+            
+             yawPoint = yaw;
+        }
+        else if(powerPin.read() != 1){
+            motorState = 0;
+            neutralizeMotors();
+        }
+
         yawIn = yaw;
         rollIn = roll;
         pitchIn = pitch;
         xIn = ax;
         yIn = ay;
         zIn = az;
-        
+
         //Calculate PID values
         pidy.Compute();
         pidr.Compute();
@@ -184,7 +226,7 @@
         pidX.Compute();
         pidY.Compute();
         pidZ.Compute();
-        
+
         /*
                 Cameras
           FL ----- F ->--- FR
@@ -196,7 +238,7 @@
           |        |       ˅
           |        |       |
           BL ---<- B ----- BR
-    
+
           0  ----- 1 ->--- 2
           |        |       |
           ˄        |       |
@@ -206,43 +248,44 @@
           |        |       ˅
           |        |       |
           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 depths = dOut * dOut;
         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 zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut) + abs(dOut)) * 255;
         float yy  = (abs(yOut) + abs(yawOut)) * 255;
-        float xy  = (abs(xOut) + 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; 
-        
+//        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(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(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);//
@@ -251,67 +294,92 @@
 //        m5 = abs((accys + yaws) / yy);
 //        m6 = abs((acczs - pitchs + rolls) / zpr);//
 //        m7 = abs((accxs + yaws) / yy);
-        
-        m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
+
+        m0 = (acczs + pitchs + rolls - depths) / 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;//
+        m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
+        m3 = (accxs + yaws) / -xy + 0.5;
+        m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
         m5 = (accys + yaws) / yy + 0.5;
-        m6 = (acczs - pitchs + rolls) / zpr + 0.5;//
+        m6 = (acczs - pitchs + rolls - depths) / -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("%f,%f,%f, %f,%f,%f, %f, %f \n\r",powerPin.read(), acczs, yaws, pitchs, rolls, zpr, depths, (acczs + pitchs + rolls - depths) / 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);
+        //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
+    }
+
+    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);
+        }
+    }
+
+    void initMotors(){
+        
+        neutralizeMotors();
+        m0.calibrate();
+        m1.calibrate();
+        m2.calibrate();
+        m3.calibrate();
+        m4.calibrate();
+        m5.calibrate();
+        m6.calibrate();
+        m7.calibrate();
     }
     
-void updateCommand() {
-    char c = 0;
-    string command;
-    char buffer[BUFFER_SIZE] = {' '};
-    int buffer_iter = 0;    
-    pc.printf("Checking for command\n");
+    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;
+    }
 
-    // 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
\ No newline at end of file