Project_3

Comment: This project was definitely the hardest thing I did in ENGR6. I did learn a lot about using libraries in arduino, which are basically header file in C/C++. It was nice that Mason provided everyone with the libraries, and code to run all the  modules. Our group replaced two of the nano, since they were buggy and had difficulty uploading code to the arduino. Compiling everything wasn't too bad, it was more the dynamic memory allocation that we were worried about. Fortunately, David told us that we could delete a part of code for the MPU module that saved us a lot of memory.We did get everything to work, except the python is buggy. We also, had trouble on the launch day, with the battery dying, and my group not having a hole for the switch to turn on the arduino. Fortunately,we finished in just the nick of time, and got back good data. 
//Reading Acceleration, Gyro, and Pressure Data ---> writing to SD card
//#include <TimerOne.h>
//#include <SD.h>
#include <Wire.h>
#include "BMP180Lib.c"
#include "quaternionFilters.h"
#include "MPU9250.h"
#include <SdFat.h>

SdFat SD;

#define BMP180_ADDRESS 0x77 // I2C address of BMP180

MPU9250 myIMU;

File dataFile;

long timeStamp; //Keeps track of when data is recorded
bool Trigger = 0; //becomes true if Acc is above threshold

const int buzzer = 2;
// Use these for altitude conversions
const float p0 = 101325; // Pressure at sea level (Pa)

// Accelerometer and Gyro variables
const long refresh_rate = 50; // in ms

void setup()
{
  Serial.begin(57600);
  Wire.begin();
  Serial.println("CD"); //Calibrating Devices
    bmp180Calibration();
    //myIMU.MPU9250SelfTest(myIMU.SelfTest); // Start by performing self test and reporting values
    myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
    myIMU.initMPU9250(); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
    myIMU.initAK8963(myIMU.magCalibration); // Initialize device for active mode read of magnetometer
  Serial.println("IC"); //Initializing Card
    tone (buzzer, 1000, 2000);
    delay(2000);
    //Set by default for the SD card library
    const int CS_PIN = 10;
    const int POW_PIN = 8;
    pinMode(CS_PIN, OUTPUT); //CS pin is an output
    pinMode(POW_PIN, OUTPUT);
    digitalWrite(POW_PIN, HIGH);

//  while (!SD.begin(CS_PIN))
//  {
//    Serial.println("CF"); //Card Failed
//    tone (buzzer, 500, 1000);
//    return;
//  }

if (!SD.begin(CS_PIN))
{
    Serial.println("CF");
    // don't do anything more:
    while (1);
  }
  Serial.println("card initialized.");
  dataFile = SD.open("MPU9255.csv", FILE_WRITE);
    dataFile.println("~~~~~~~~~~~~~~NEW DATA SET~~~~~~~~~~~~~~");
    dataFile.println("t,Ax,Ay,Az,Gx,Gy,Gz,Mx,My,Mz,Y,P,R,Pre,Alt");
//    dataFile.print("Time(ms)"); dataFile.print(",");
// 
//    dataFile.print("AccX (mg)"); dataFile.print(",");
//    dataFile.print("AccY"); dataFile.print(",");
//    dataFile.print("AccZ"); dataFile.print(",");
// 
//    dataFile.print("GyroX (deg/s)"); dataFile.print(",");
//    dataFile.print("GyroY"); dataFile.print(",");
//    dataFile.print("GyroZ"); dataFile.print(",");
// 
//    dataFile.print("MagX (mG)"); dataFile.print(",");
//    dataFile.print("MagY"); dataFile.print(",");
//    dataFile.print("MagZ"); dataFile.print(",");
// 
//    dataFile.print("Yaw (deg)"); dataFile.print(",");
//    dataFile.print("Pitch"); dataFile.print(",");
//    dataFile.print("Roll"); dataFile.print(",");
// 
//    dataFile.print("Pressure (Pa)"); dataFile.print(",");
//    dataFile.println("Altitude (m)");
    dataFile.close();
  Serial.println("CR"); //Card Ready
  Serial.print("RR= "); //Refresh Rate
  Serial.print(refresh_rate);
  Serial.println("ms");

    Serial.println("~~~~~~~~~~~~~~NEW DATA SET~~~~~~~~~~~~~~");
    Serial.println("t,Ax,Ay,Az,Gx,Gy,Gz,Mx,My,Mz,Y,P,R,Pre,Alt");
//    Serial.print("Time(ms)"); Serial.print(",");
// 
//    Serial.print("AccX(mg)"); Serial.print(",");
//    Serial.print("AccY"); Serial.print(",");
//    Serial.print("AccZ"); Serial.print(",");
// 
//    Serial.print("GyroX(deg/s)"); Serial.print(",");
//    Serial.print("GyroY"); Serial.print(",");
//    Serial.print("GyroZ"); Serial.print(",");
// 
//    Serial.print("MagX(mG)"); Serial.print(",");
//    Serial.print("MagY"); Serial.print(",");
//    Serial.print("MagZ"); Serial.print(",");
// 
//    Serial.print("Yaw(deg)"); Serial.print(",");
//    Serial.print("Pitch"); Serial.print(",");
//    Serial.print("Roll"); Serial.print(",");
// 
//    Serial.print("Pressure(Pa)"); Serial.print(",");
//    Serial.println("Altitude(m)");
    tone (buzzer, 3000, 300);
    delay (300);
    tone (buzzer, 3000, 300);
    delay(300);

  //Set up timer interrupt
//  Timer1.initialize(refresh_rate*1000); //Set a timer of length 50 ms
//  Timer1.attachInterrupt(updateClock); //Runs "updateClock function" on each time interrupt
}

// Stores all of the bmp180's calibration values into global variables
// Calibration values are required to calculate temp and pressure
// This function should be called at the beginning of the program
void bmp180Calibration()
{
 ac1 = bmp180ReadInt(0xAA);
 ac2 = bmp180ReadInt(0xAC);
 ac3 = bmp180ReadInt(0xAE);
 ac4 = bmp180ReadInt(0xB0);
 ac5 = bmp180ReadInt(0xB2);
 ac6 = bmp180ReadInt(0xB4);
 b1 = bmp180ReadInt(0xB6);
 b2 = bmp180ReadInt(0xB8);
 mb = bmp180ReadInt(0xBA);
 mc = bmp180ReadInt(0xBC);
 md = bmp180ReadInt(0xBE);
}

// Read 1 byte from the BMP180 at 'address'
char bmp180Read(unsigned char address)
{
 unsigned char data;
 Wire.beginTransmission(BMP180_ADDRESS);
 Wire.write(address);
 Wire.endTransmission();
 Wire.requestFrom(BMP180_ADDRESS, 1);
 while(!Wire.available())
 ;
 return Wire.read();
}

// Read 2 bytes from the BMP180
// First byte will be from 'address'
// Second byte will be from 'address'+1
int bmp180ReadInt(unsigned char address)
{
 unsigned char msb, lsb;
 Wire.beginTransmission(BMP180_ADDRESS);
 Wire.write(address);
 Wire.endTransmission();

 Wire.requestFrom(BMP180_ADDRESS, 2);
 while(Wire.available()<2)
 ;
 msb = Wire.read();
 lsb = Wire.read();

 return (int) msb<<8 | lsb;
}

// Read the uncompensated temperature value
unsigned int bmp180ReadUT()
{
 unsigned int ut;
 // Write 0x2E into Register 0xF4
 // This requests a temperature reading
 Wire.beginTransmission(BMP180_ADDRESS);
 Wire.write(0xF4);
 Wire.write(0x2E);
 Wire.endTransmission();
 // Wait at least 4.5ms
 delay(5);
 // Read two bytes from registers 0xF6 and 0xF7
 ut = bmp180ReadInt(0xF6);
 return ut;
}

// Read the uncompensated pressure value
unsigned long bmp180ReadUP()
{
 unsigned char msb, lsb, xlsb;
 unsigned long up = 0;
 // Write 0x34+(OSS<<6) into register 0xF4
 // Request a pressure reading w/ oversampling setting
 Wire.beginTransmission(BMP180_ADDRESS);
 Wire.write(0xF4);
 Wire.write(0x34 + (OSS<<6));
 Wire.endTransmission();
 // Wait for conversion, delay time dependent on OSS
 delay(2 + (3<<OSS));
 // Read register 0xF6 (MSB), 0xF7 (LSB), and 0xF8 (XLSB)
 Wire.beginTransmission(BMP180_ADDRESS);
 Wire.write(0xF6);
 Wire.endTransmission();
 Wire.requestFrom(BMP180_ADDRESS, 3);
 // Wait for data to become available
 while(Wire.available() < 3)
 ;
 msb = Wire.read();
 lsb = Wire.read();
 xlsb = Wire.read();
 up = (((unsigned long) msb << 16) | ((unsigned long) lsb << 8) | (unsigned long) xlsb) >> (8-OSS);
 return up;
}

void loop()
{
 if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
 {
   myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values
   myIMU.getAres();
   // Now we'll calculate the accleration value into actual g's
   // This depends on scale being set
   myIMU.ax = (float)myIMU.accelCount[0]*myIMU.aRes; // - accelBias[0];
   myIMU.ay = (float)myIMU.accelCount[1]*myIMU.aRes; // - accelBias[1];
   myIMU.az = (float)myIMU.accelCount[2]*myIMU.aRes; // - accelBias[2];
   myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values
   myIMU.getGres();
   // Calculate the gyro value into actual degrees per second
   // This depends on scale being set
   myIMU.gx = (float)myIMU.gyroCount[0]*myIMU.gRes;
   myIMU.gy = (float)myIMU.gyroCount[1]*myIMU.gRes;
   myIMU.gz = (float)myIMU.gyroCount[2]*myIMU.gRes;
   myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values
   myIMU.getMres();
   myIMU.magbias[0] = +470.;
   // User environmental x-axis correction in milliGauss TODO axis??
   myIMU.magbias[1] = +120.;
   // User environmental x-axis correction in milliGauss
   myIMU.magbias[2] = +125.;
   myIMU.mx = (float)myIMU.magCount[0]*myIMU.mRes*myIMU.magCalibration[0] -
   myIMU.magbias[0];
   myIMU.my = (float)myIMU.magCount[1]*myIMU.mRes*myIMU.magCalibration[1] -
   myIMU.magbias[1];
   myIMU.mz = (float)myIMU.magCount[2]*myIMU.mRes*myIMU.magCalibration[2] -
   myIMU.magbias[2];
 } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)

 // Must be called before updating quaternions!
myIMU.updateTime();
MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx*DEG_TO_RAD,
                        myIMU.gy*DEG_TO_RAD, myIMU.gz*DEG_TO_RAD, myIMU.my,
                        myIMU.mx, myIMU.mz, myIMU.deltat);
      myIMU.yaw   = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() *
                    *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) * *(getQ()+1)
                    - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) * *(getQ()+3));
      myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() *
                    *(getQ()+2)));
      myIMU.roll  = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) *
                    *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1) * *(getQ()+1)
                    - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) * *(getQ()+3));
      myIMU.pitch *= RAD_TO_DEG;
      myIMU.yaw   *= RAD_TO_DEG;
      // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
      //  8° 30' E  ± 0° 21' (or 8.5°) on 2016-07-19
      // - http://www.ngdc.noaa.gov/geomag-web/#declination
      myIMU.yaw   -= 8.5;
      myIMU.roll  *= RAD_TO_DEG;
   
myIMU.delt_t = millis() - myIMU.count;

//  float AccX = 1000*myIMU.ax;
//  float AccY = 1000*myIMU.ay;
//  float AccZ = 1000*myIMU.az;
//
//  float GyroX = myIMU.gx;
//  float GyroY = myIMU.gy;
//  float GyroZ = myIMU.gz;
//
//  float BfieldX = myIMU.mx;
//  float BfieldY = myIMU.my;
//  float BfieldZ = myIMU.mz;

  myIMU.count = millis();

//  float Yaw = myIMU.yaw;
//  float Pitch = myIMU.pitch;
//  float Roll = myIMU.roll;

//  short temperature;
//  long pressure;
//float altitude;
  short temperature = bmp180GetTemperature(bmp180ReadUT());
  long pressure = bmp180GetPressure(bmp180ReadUP());
  float altitude = (float)44330 * (1 - pow(((float) pressure/p0), 0.190295));

   //Open a file and write to it.
if ((abs(1000*myIMU.ax) < 2000) || (abs(1000*myIMU.ay) < 2000) || (abs(1000*myIMU.ay) < 2000))
{
    Trigger = 1;
    if (altitude < 873)
    {
    Serial.println("Data not being collected");
    delay(50);
}
else if ((Trigger==1) && (altitude > 873))
{
dataFile = SD.open("MPU9255.csv", FILE_WRITE);
//while (!dataFile)
//{
//  dataFile = SD.open("MPU9255.csv", FILE_WRITE);
//}
  if (dataFile)
  {
    updateClock();
    dataFile.print(timeStamp);dataFile.print(",");
 
    dataFile.print(1000*myIMU.ax); dataFile.print(",");
    dataFile.print(1000*myIMU.ay); dataFile.print(",");
    dataFile.print(1000*myIMU.az); dataFile.print(",");
 
    dataFile.print(myIMU.gx, 3); dataFile.print(",");
    dataFile.print(myIMU.gy, 3); dataFile.print(",");
    dataFile.print(myIMU.gz, 3); dataFile.print(",");
 
    dataFile.print(myIMU.mx); dataFile.print(",");
    dataFile.print(myIMU.my); dataFile.print(",");
    dataFile.print(myIMU.mz); dataFile.print(",");
 
    dataFile.print(myIMU.yaw, 2); dataFile.print(",");
    dataFile.print(myIMU.pitch, 2); dataFile.print(",");
    dataFile.print(myIMU.roll, 2); dataFile.print(",");
 
    dataFile.print(pressure); dataFile.print(",");
    dataFile.println(altitude);
    dataFile.close(); //Data isnt actually written until we close the connection!

    //Print same thing to the screen for debugging
    Serial.print(timeStamp); Serial.print(" ms,");
 
    Serial.print(1000*myIMU.ax); Serial.print(",");
    Serial.print(1000*myIMU.ay); Serial.print(",");
    Serial.println(1000*myIMU.az); //Serial.print(",");
 
//    Serial.print(myIMU.gx, 3); Serial.print(",");
//    Serial.print(myIMU.gy, 3); Serial.print(",");
//    Serial.print(myIMU.gz, 3); Serial.print(",");
// 
//    Serial.print(myIMU.mx); Serial.print(",");
//    Serial.print(myIMU.my); Serial.print(",");
//    Serial.print(myIMU.mz); Serial.print(",");
// 
//    Serial.print(myIMU.yaw, 2); Serial.print(",");
//    Serial.print(myIMU.pitch, 2); Serial.print(",");
//    Serial.print(myIMU.roll, 2); Serial.print(",");
// 
//    Serial.print(pressure); Serial.print(",");
//    Serial.print(altitude); Serial.println(" m");
    //delay(refresh_rate);
  }
    else
  {
    Serial.println("LFF"); // Couldn't open log file aka "Log File Failed"
    return 0;
  }
}
else
  {
    Serial.println("Data not being collected");
    delay(50);
  }
}
}

void updateClock()
{
  timeStamp = millis();
}