Collecting sensor info over i2c and SoftwareSerial

Hi,
I'm relatively new to Arduino and electronics so feel free to correct me. I just got my Adafruit GPS breakout board the other day to work with a FreeIMU for an autopilot on the Uno. It is my understanding that the FreeIMU communicates over I2C with the analog pins. The GPS is connected through the digital pins on the Uno to the tx and rx pins on the GPS. Is it possible to use both of these at the same time? Right now I'm trying to view the data from both sensors on the Serial monitor but it only lets me view one or the other. I commented out all of the IMU code and uncommented and then uploaded line by line and it seems that line 105 causes an issue with the serial monitor. I have attached my code below. Is there a way to sync the data from both sensors? I need to read the IMU and GPS values as quickly as possible. I've been using open source libraries for this project (which has been awesome!) and, as you can see, there are a lot of them. If you need to see one of these please let me know.

#include <ADXL345.h>
#include <bma180.h>
#include <HMC58X3.h>
#include <ITG3200.h>
#include <MS561101BA.h>
#include <I2Cdev.h>
#include <MPU60X0.h>
#include <EEPROM.h>
#include <Servo.h>

//#define DEBUG
#include "DebugUtils.h"
#include "CommunicationUtils.h"

#include "FreeIMU.h"
#include <Wire.h>
#include <SPI.h>

#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>


// If you're using the Adafruit GPS shield, change 
// SoftwareSerial mySerial(3, 2); -> SoftwareSerial mySerial(8, 7);
// and make sure the switch is set to SoftSerial

// If using software serial, keep these lines enabled
// (you can change the pin numbers to match your wiring):
SoftwareSerial mySerial(3, 2);

Adafruit_GPS GPS(&mySerial);
// If using hardware serial (e.g. Arduino Mega), comment
// out the above six lines and enable this line instead:
//Adafruit_GPS GPS(&Serial1);


// Set GPSECHO to 'false' to turn off echoing the GPS data to the Serial console
// Set to 'true' if you want to debug and listen to the raw GPS sentences. 
#define GPSECHO  false

// this keeps track of whether we're using the interrupt
// off by default!
boolean usingInterrupt = false;
void useInterrupt(boolean); // Func prototype keeps Arduino 0023 happy





const int pingPin = 9;
const int yawled = 10;
const int rollled = 11;
const int pitchled = 12;
const int detectionpin = 13;
long duration, in,cm;
int raw_values[9];
//char str[512];
float ypr[3]; // yaw pitch roll
float val[9];
Servo sweepserv; 
int pos = 0;
//// Set the FreeIMU object
FreeIMU my3IMU = FreeIMU();
void setup() { 
  Serial.begin(9600);
  Wire.begin();
  
  
    // connect at 115200 so we can read the GPS fast enough and echo without dropping chars
  // also spit it out

  Serial.println("Adafruit GPS library basic test!");

  // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
  GPS.begin(9600);
  
  // uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
  // uncomment this line to turn on only the "minimum recommended" data
  //GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
  // For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
  // the parser doesn't care about other sentences at this time
  
  // Set the update rate
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);   // 1 Hz update rate
  // For the parsing code to work nicely and have time to sort thru the data, and
  // print it out we don't suggest using anything higher than 1 Hz

  // Request updates on antenna status, comment out to keep quiet
  GPS.sendCommand(PGCMD_ANTENNA);

  // the nice thing about this code is you can have a timer0 interrupt go off
  // every 1 millisecond, and read data from the GPS for you. that makes the
  // loop code a heck of a lot easier!
  useInterrupt(true);

  delay(1000);
  // Ask for firmware version
  mySerial.println(PMTK_Q_RELEASE);
  
  delay(10);
  my3IMU.init(); // the parameter enable or disable fast mode
  delay(10);
  pinMode(yawled,OUTPUT);
  pinMode(rollled,OUTPUT);
  pinMode(pitchled,OUTPUT);
  pinMode(detectionpin,OUTPUT);
 
  sweepserv.attach(14);
  
}
// Interrupt is called once a millisecond, looks for any new GPS data, and stores it
SIGNAL(TIMER0_COMPA_vect) {
  char c = GPS.read();
  // if you want to debug, this is a good time to do it!
#ifdef UDR0
  if (GPSECHO)
    if (c) UDR0 = c;  
    // writing direct to UDR0 is much much faster than Serial.print 
    // but only one character can be written at a time. 
#endif
}

void useInterrupt(boolean v) {
  if (v) {
    // Timer0 is already used for millis() - we'll just interrupt somewhere
    // in the middle and call the "Compare A" function above
    OCR0A = 0xAF;
    TIMSK0 |= _BV(OCIE0A);
    usingInterrupt = true;
  } else {
    // do not call the interrupt function COMPA anymore
    TIMSK0 &= ~_BV(OCIE0A);
    usingInterrupt = false;
  }
}

uint16_t timer = millis();

void loop() { 
 

//    for(pos = 0; pos < 150; pos += 1)  // goes from 0 degrees to 180 degrees 
//  { 
//      sweepserv.write(pos);
      
       // in case you are not using the interrupt above, you'll
  // need to 'hand query' the GPS, not suggested :(
  if (! usingInterrupt) {
    // read data from the GPS in the 'main loop'
    char c = GPS.read();
    // if you want to debug, this is a good time to do it!
    if (GPSECHO)
      if (c) Serial.print(c);
  }
  
  // if a sentence is received, we can check the checksum, parse it...
  if (GPS.newNMEAreceived()) {
    // a tricky thing here is if we print the NMEA sentence, or data
    // we end up not listening and catching other sentences! 
    // so be very wary if using OUTPUT_ALLDATA and trytng to print out data
    //Serial.println(GPS.lastNMEA());   // this also sets the newNMEAreceived() flag to false
  
    if (!GPS.parse(GPS.lastNMEA()))   // this also sets the newNMEAreceived() flag to false
      return;  // we can fail to parse a sentence in which case we should just wait for another
  }

  // if millis() or timer wraps around, we'll just reset it
  if (timer > millis()) { timer = millis();}

  // approximately every 2 seconds or so, print out the current stats
  if (millis() - timer > 2000) { 
    timer = millis(); // reset the timer
    
    Serial.print("\nTime: ");
    Serial.print(GPS.hour, DEC); Serial.print(':');
    Serial.print(GPS.minute, DEC); Serial.print(':');
    Serial.print(GPS.seconds, DEC); Serial.print('.');
    Serial.println(GPS.milliseconds);
    Serial.print("Date: ");
    Serial.print(GPS.day, DEC); Serial.print('/');
    Serial.print(GPS.month, DEC); Serial.print("/20");
    Serial.println(GPS.year, DEC);
    Serial.print("Fix: "); Serial.print((int)GPS.fix);
    Serial.print(" quality: "); Serial.println((int)GPS.fixquality); 
    if (GPS.fix) {
      Serial.print("Location: ");
      Serial.print(GPS.latitude, 4); Serial.print(GPS.lat);
      Serial.print(", "); 
      Serial.print(GPS.longitude, 4); Serial.println(GPS.lon);
      
      Serial.print("Speed (knots): "); Serial.println(GPS.speed);
      Serial.print("Angle: "); Serial.println(GPS.angle);
      Serial.print("Altitude: "); Serial.println(GPS.altitude);
      Serial.print("Satellites: "); Serial.println((int)GPS.satellites);
      

    }
  }
  

      my3IMU.getYawPitchRoll(ypr);
      Serial.print(" Yaw: ");
      Serial.print(ypr[0]);

      Serial.print(" Roll: ");
      Serial.print(ypr[2]);

Sorry, in that code, the line at the beginning of setup has Serial.begin(9600), which I thought matching that to GPS.begin(9600) would work and and it did not. It was originally Serial.begin(115200). Thanks

Deleting the commented out code would be a first step.

Using Tools + Auto Format would be a good second step.

Posting code that actually compiles would be a necessary third step.

I apologize. I was in a bit of a frenzy last night (you can see my code was as well). I made some changes this morning so this is different than the original code I posted. As of now, a servo sweeps 150 degrees while using an ultrasonic sensor to detect objects that are 6 inches away. Once the servo gets to 150 degrees, GPS coordinates are spit out, and then the servo sweeps back to 0 degrees. I found a solution to my original problem of not being able to see both serial outputs. However, now I am trying to use the Parallax PING))) sensor and for some reason it outputs a 0 distance every few seconds and the servo is quite spastic meaning it jumps between seemingly random positions while still (kind of) following the "sweeping" motion I have coded.

Let me know if you need more info!

#include <ADXL345.h>
#include <bma180.h>
#include <HMC58X3.h>
#include <ITG3200.h>
#include <MS561101BA.h>
#include <I2Cdev.h>
#include <MPU60X0.h>
#include <EEPROM.h>
#include <Servo.h>

//#define DEBUG
#include "DebugUtils.h"
#include "CommunicationUtils.h"

#include "FreeIMU.h"
#include <Wire.h>
#include <SPI.h>

#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>



SoftwareSerial mySerial(3, 2);

Adafruit_GPS GPS(&mySerial);



// Set GPSECHO to 'false' to turn off echoing the GPS data to the Serial console
// Set to 'true' if you want to debug and listen to the raw GPS sentences.
#define GPSECHO  false

// this keeps track of whether we're using the interrupt
// off by default!
boolean usingInterrupt = false;
void useInterrupt(boolean); // Func prototype keeps Arduino 0023 happy





const int pingPin = 9;
const int yawled = 10;
const int rollled = 11;
const int pitchled = 12;
const int detectionpin = 13;
long duration, in, cm;
int raw_values[9];
//char str[512];
float ypr[3]; // yaw pitch roll
float val[9];
Servo sweepserv;
int pos = 0;
//// Set the FreeIMU object
FreeIMU my3IMU = FreeIMU();
void setup() {
  Serial.begin(115200);
  Wire.begin();


  // connect at 115200 so we can read the GPS fast enough and echo without dropping chars
  // also spit it out

  Serial.println("Adafruit GPS library basic test!");

  // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
  GPS.begin(9600);


  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);


  // Set the update rate
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);   // 1 Hz update rate


  GPS.sendCommand(PGCMD_ANTENNA);


  useInterrupt(true);

  delay(1000);
  // Ask for firmware version
  mySerial.println(PMTK_Q_RELEASE);

  delay(10);
  //  my3IMU.init(); // the parameter enable or disable fast mode
  //  delay(10);
  //  pinMode(yawled,OUTPUT);
  //  pinMode(rollled,OUTPUT);
  //  pinMode(pitchled,OUTPUT);
  pinMode(detectionpin, OUTPUT);
  digitalWrite(detectionpin, LOW);
  sweepserv.attach(14);

}
// Interrupt is called once a millisecond, looks for any new GPS data, and stores it
SIGNAL(TIMER0_COMPA_vect) {
  char c = GPS.read();
  // if you want to debug, this is a good time to do it!
#ifdef UDR0
  if (GPSECHO)
    if (c) UDR0 = c;
  // writing direct to UDR0 is much much faster than Serial.print
  // but only one character can be written at a time.
#endif
}

void useInterrupt(boolean v) {
  if (v) {
    // Timer0 is already used for millis() - we'll just interrupt somewhere
    // in the middle and call the "Compare A" function above
    OCR0A = 0xAF;
    TIMSK0 |= _BV(OCIE0A);
    usingInterrupt = true;
  } else {
    // do not call the interrupt function COMPA anymore
    TIMSK0 &= ~_BV(OCIE0A);
    usingInterrupt = false;
  }
}

uint16_t timer = millis();

void loop() {
  for (pos = 0; pos < 150; pos += 1)  { // goes from 0 degrees to 150 degrees
    sweepserv.write(pos);


    pinMode(pingPin, OUTPUT);
    digitalWrite(pingPin, LOW); //warmup
    delayMicroseconds(2);
    digitalWrite(pingPin, HIGH); //sending ping
    delayMicroseconds(5);
    digitalWrite(pingPin, LOW);

    pinMode(pingPin, INPUT);
    duration = pulseIn(pingPin, HIGH); //time till ping return
    in = duration / 74 / 2;
    cm = duration / 29 / 2;
    Serial.print(" Dist: ");
    Serial.print(in);
    Serial.println("");
    if (in <= 6)
    {
      digitalWrite(detectionpin, HIGH);
    }
    else
    {
      digitalWrite(detectionpin, LOW);
    }

  }
  // in case you are not using the interrupt above, you'll
  // need to 'hand query' the GPS, not suggested :(
  if (! usingInterrupt) {
    // read data from the GPS in the 'main loop'
    char c = GPS.read();
    // if you want to debug, this is a good time to do it!
    if (GPSECHO)
      if (c) Serial.print(c);
  }

  // if a sentence is received, we can check the checksum, parse it...
  if (GPS.newNMEAreceived()) {
    // a tricky thing here is if we print the NMEA sentence, or data
    // we end up not listening and catching other sentences!
    // so be very wary if using OUTPUT_ALLDATA and trytng to print out data
    //Serial.println(GPS.lastNMEA());   // this also sets the newNMEAreceived() flag to false

    if (!GPS.parse(GPS.lastNMEA()))   // this also sets the newNMEAreceived() flag to false
      return;  // we can fail to parse a sentence in which case we should just wait for another
  }

  // if millis() or timer wraps around, we'll just reset it
  if (timer > millis()) {
    timer = millis();
  }
  if (GPS.fix) {
    Serial.print("Latitude: ");
    Serial.print(GPS.latitude, 4);
    Serial.print(" Longitude: ");
    Serial.print(GPS.longitude, 4);
    Serial.print( " Altitude: ");
    Serial.print(GPS.altitude);
    Serial.print( " Satellites: ");
    Serial.print(GPS.satellites);
    Serial.println("");
  }

  for (pos = 150; pos >= 1; pos -= 1) // goes from 150 degrees to 0 degrees
  {
    sweepserv.write(pos);


    pinMode(pingPin, OUTPUT);
    digitalWrite(pingPin, LOW); //warmup
    delayMicroseconds(2);
    digitalWrite(pingPin, HIGH); //sending ping
    delayMicroseconds(5);
    digitalWrite(pingPin, LOW);

    pinMode(pingPin, INPUT);
    duration = pulseIn(pingPin, HIGH); //time till ping return
    in = duration / 74 / 2;
    cm = duration / 29 / 2;
    Serial.print(" Dist: ");
    Serial.print(in);
    Serial.println("");
    if (in <= 6)
    {
      digitalWrite(detectionpin, HIGH);
    }
    else
    {
      digitalWrite(detectionpin, LOW);
    }
  }
}