프로그램 : 아두이노 IDE

보드 : 아두이노 메가 

센서 : 초음파 모듈, 나침반 컴파스 모듈, gps모듈

모터 : DC모터 1개 , 스텝 모터 1개 , 모터 드라이버 2개

안녕하세요.. 제가 위의 부품을 가지고 작품을 하나 만들고 싶어서 아두이노IDE를 이용하여 코딩을 하였는데, 

각각의 센서에 대해서는 잘 작동을 하는데  모든 센서와 부품들을 합친 코드는 동작을 하지않습니다....

시리얼 모니터에서 GPS모듈 값을 못읽는것 같습니다.... 고수님들 도와주세요...

Void loop 에서 gps모듈 데이터 파싱관련하여 if문이 3개가 나오는데  확인결과 3번째 if문을 받아들이지 못합니다

분명 센서 따로따로 하였을땐 문제가 없었는데 왜 그럴까요??

소스코드는 밑에 첨부했습니다.

#include <Wire.h>
#include <MechaQMC5883.h>
#include <SoftwareSerial.h>
#include<Stepper.h>
#include<TinyGPS.h>

SoftwareSerial gpsSerial(11, 10); // TX , RX 순서
#define speedPIN 3 // ENA
#define LEFT_A1 4  // 모터 +
#define LEFT_B1 5  // 모터 -
#define IR_TRIG 6 //TRIG 
#define IR_ECHO 7 // ECHO 

const int stepsPerRevolution = 200; // 스텝모터 1각에 1.8도 총 360도
Stepper Rolling(stepsPerRevolution,8,9,12,13);   // 모터 드라이버 핀번호
MechaQMC5883 qmc;

char c = ""; 

String str = "";

String targetStr = "GPGGA"; 

void setup() {

  // put your setup code here, to run once:

  Serial.begin(9600);

  Serial.println("Start GPS... ");

  gpsSerial.begin(9600);
  Rolling.setSpeed(20); // 스텝모터 RPM
  qmc.init();
    Wire.begin();
    
    //  DC 모터 부분
  //모터 핀모드 설정
 pinMode(speedPIN, OUTPUT);
  pinMode(LEFT_A1, OUTPUT);
  pinMode(LEFT_B1, OUTPUT);

  //초음파 센서 핀모드 설정
  pinMode(IR_TRIG, OUTPUT);
  pinMode(IR_ECHO, INPUT);
  

}

void loop() {

  // put your main code here, to run repeatedly:

 float duration, distance;
  digitalWrite(IR_TRIG, HIGH);
  delay(10);
  digitalWrite(IR_TRIG, LOW);   // 
  int x, y, z;  // 
 int azimuth;  // 
 int random1 = 320;

duration = pulseIn(IR_ECHO, HIGH);
  // HIGH 
  distance = ((float)(340 * duration) / 10000) / 2;

  if(gpsSerial.available()) // gps 센서 통신 가능?

    {     
            Serial.println("1st good");              
      c=gpsSerial.read(); // 센서의 값 읽기

      if(c == '\n'){ // \n 값인지 구분.

            Serial.println("2nd good ");
        // \n 일시. 지금까지 저장된 str 값이 targetStr과 맞는지 구분

        if(targetStr.equals(str.substring(1, 6))){

          // NMEA 의 GPGGA 값일시

          // , 를 토큰으로서 파싱.

          int first = str.indexOf(",");

          int two = str.indexOf(",", first+1);

          int three = str.indexOf(",", two+1);

          int four = str.indexOf(",", three+1);

          int five = str.indexOf(",", four+1);

          // Lat과 Long 위치에 있는 값들을 index로 추출

          String Lat = str.substring(two+1, three);

          String Long = str.substring(four+1, five);

          // Lat의 앞값과 뒷값을 구분

          String Lat1 = Lat.substring(0, 2);

          String Lat2 = Lat.substring(2);

          // Long의 앞값과 뒷값을 구분

          String Long1 = Long.substring(0, 3);

          String Long2 = Long.substring(3);

          // 좌표 계산.

          double LatF = Lat1.toDouble() + Lat2.toDouble()/60;

          float LongF = Long1.toFloat() + Long2.toFloat()/60;

          // 좌표 출력.

             Serial.println("3rd good ");
          Serial.print("Lat : ");

          Serial.println(LatF, 15);

          Serial.print("Long : ");

          Serial.println(LongF, 15);
          delay(100);

          
           Serial.print("\nDIstance : "); // 초음파 거리
  Serial.println(distance);
   
   qmc.init();
  qmc.read(&x, &y, &z,&azimuth);
 azimuth = qmc.azimuth(&y,&x);
  Serial.print(" azimuth: ");  // 현재 방위각
 Serial.print(azimuth);
 Serial.println();
   
 if(distance > 30 || distance ==0) {         //장애물 감지 
     Serial.println("GO!GO!");
        gogo();
   }
      else if(distance>=15 && distance<=29){
                    Serial.println("slow");
                    slow();
 }  else{         
            Serial.println("stop");
                    stop();
    
   }

      if(azimuth <random1-1 ){
          Serial.print("The degree of azimuth is less than the goal . ");
            Rolling.step(-stepsPerRevolution);
      }
    else if (azimuth > random1+1){
       Serial.print("The degree of azimuth is more than the goal. ");
      Rolling.step(stepsPerRevolution);
   
    }
    else{
       Serial.print("The degree of azimuth is perfect to the goal. ");
       
    }

        }
    else{
       Serial.println("3rd bad");
    }
        // str 값 초기화 

        str = "";

      }else{ // \n 아닐시, str에 문자를 계속 더하기
        
        

        str += c;

      }

    }

 }

 void slow() { 
  digitalWrite(LEFT_A1,LOW);
  digitalWrite(LEFT_B1, HIGH);
  analogWrite(speedPIN,150);
  
}
void gogo(){
  digitalWrite(LEFT_A1, LOW);
  digitalWrite(LEFT_B1, HIGH);
  analogWrite(speedPIN,255);
  }
 void stop() {
  digitalWrite(LEFT_A1, LOW);
  digitalWrite(LEFT_B1, LOW);
  
 }