통신 알고리즘 - HC12 테스트0

*Flight

#include <SoftwareSerial.h>
#include <string.h>
SoftwareSerial HC12(2,3);
//https://rasino.tistory.com/326
//https://docs.arduino.cc/learn/built-in-libraries/software-serial
//https://blog.naver.com/darknisia/220808977305
//https://www.youtube.com/watch?v=B6qdNjtGfXE
/*
listen() : 수신 대기 상태
소프트웨어 시리얼 통신의 Rx핀으로 데이터 수신이 올 때 까지 대기한다
여러 소프트웨어 시리얼을 사용하는 경우 앞서 말했듯이 한 번에 하나의 통신만이
가능하므로 반드시 원하는 포트를 수신 대기 생태로 지정해 주어야 한다.
그리고 데이터 수신이 오게 되며 이 함수는 true를 반환하고 대기상태라면 
false 를 반환한다.

isListening()  : 수신 대기 여부 반환
소프트웨어 시리얼이 수신 대기 상태인지 아닌지를 반환하는 함수다.

read()
데이터 수신 버퍼의 첫 번째 문자를 반환하는 함수로 앞의 peek 함수랑 같은 기능을 가진다.
하지만 name.read() 함수는 수신 버퍼에서 반환한 데이터를 제거한다 

name.readBytes(buffer, length)
이 함수는 지정한 개수만큼 읽어와 버퍼에 저장하는 함수다.
길이가 길거나 정해진 데이터를 수신할 때 유용하다
만약 지정한 개수만큼 데이터가 수신되지 않을 경우 수신된 개수만큼만 저장하고
그 개수를 반환한다.

name.write(character)
바이트 단위의 데이터를 전송한다. 
하지만 Serial 클래스와 달리 바이트 배열을 한 번에 전송하는 기능은 없어 한 번에 
하나의 데이터만을 전송할 수 있다. 
그렇기 때문에 반환되는 값이 전송한 바이트 수인데 0과 1만이 반환된다. 

name.avaiable() 
수신 버퍼에 저장되어있는 데이터의 바이트수를 반환한다. 
만약 소프트웨어 시리얼이 수신 대기 상태에 있지 않거나 버퍼가 비어 있는
경우 0을 반환한다. 
일반적으로 데이터를 받기를 기다릴 때 if 문을 이용하여 많이 사용된다. 

Serial.end();
HC12.end(); 있다
*/

struct DataProtocol {
  int motorSpeed=0; //pc수신의 경우 N
  int servoPos=0;  //pc수신의 경우 N
  double altitude=0; //무인기가 수신의 경우 N
  double battery=0; //무인기가 수신의 경우 N
  double xiro=0; //무인기가 수신의 경우 N
};


struct DataProtocol rcvData;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  HC12.begin(9600);
  Serial.println("Flight Site");


}



void loop() {//
  // put your main code here, to run repeatedly:
  int loop1Count=0;
  int loop2Count=0;

  String input = HC12.readString(); //1번신호 받음
  Serial.println(input);   
  if(input =="RDYJOY1\r\n"){
    Serial.println("Receive : RDYJOY1\r\n");
    for(int i=0;i<5;i++){
      HC12.println("RDYJOY2");//2번 신호 보냄
      Serial.println("Sent RDYJOY2"); //2번 신호 보낸걸 시리얼에 띄움
      delay(20);
    }
    int loop1=1;
    while(loop1>0 && loop1<6){//5회 동작
      String input = HC12.readString(); //조이스틱 값 받음
      Serial.println(input); 

      if(input== "JoystickValue"){//조이스틱 값이 맞으면
        /*
        모터및 서보 값 구조체에 데이터 저장 
        */
        Serial.println("Receive Correct JoystickValue"); 
        loop1=0;//루프 탈출

      }
      else{
        Serial.println("Bad JoystickValue");
        loop1++;
      }
      delay(20);
    }
  }
  else if(input == "RDYSND\r\n"){
    Serial.println("Receive : RDYSND\r\n");
    for(int i=0;i<5;i++){
      HC12.println("SensorValue");//2번 신호 보냄
      Serial.println("Sent RDYJOY2"); //2번 신호 보낸걸 시리얼에 띄움
      delay(20);
    }
    for(int i=0;i<5;i++){
      String input = HC12.readString(); //조이스틱 값 받음
      Serial.println(input); 
      delay(20);
    }
  }
  /*
  모터동작
  */
}
/*
  if(step ==1){
    String input = HC12.readString(); //1번신호 받음
    Serial.println(input);     

    if(input == "RDYJOY1\r\n"){//1번 신호 맞으면 
      Serial.println("Receive : RDYJOY1\r\n");
      HC12.println("RDYJOY2");//2번 신호 보냄
      Serial.println("Sent RDYJOY2"); //2번 신호 보낸걸 시리얼에 띄움

      step==2
    }
    else {
      Serial.println("NOT Receive : RDYJOY1\r\n");// 1번 신호 못 받음
      
    }
  }

  */
  /*
  else if(step ==2){
    String input = HC12.readString(); //3번신호 받음
    Serial.println(input);

    if(input == "Joustick Value\r\n"){//3번 신호 : Joystick Value맞으면 
      Serial.println("Receive : Joytick Value\r\n");// 3번 신호 받음
      HC12.println("RCVDT");//4번 신호(옳은 수신인 경우) 보냄
      Serial.println("Sent RCVDT"); //2번 신호 보낸걸 시리얼에 띄움

      step==2
    }
    else{
      Serial.println("NOT Receive : RDYJOY1\r\n");// 3번 신호 잘못 받음
      HC12.println("WRGDT");//4번 신호(옳은 수신인 경우) 보냄
      Serial.println("Sent WRGDT"); //2번 신호 보낸걸 시리얼에 띄움
    }
  }
  */


int rcvJoyData(struct DataProtocol* joyData){//joyData에 분리한 값을 넣는 것이다.  
  //리턴값 1:데이터 전송 성공, 0:데이터 전송 실패 
  int trigger=1;
  int step=1;
  while(trigger){

    if(step ==1){
      if(HC12.available()){
        String input = HC12.readString();
        Serial.println(input);     
        if(input == "RDYJOY1\r\n"){
          Serial.println("good");
          HC12.println("stop good");
        }
        else{
          Serial.println("bad");
          HC12.println("stop bad");
        }

        /*
        if(input == "RDYJOY1\0"){
          Serial.println("<<<  Correct Data = RDYJOY1 >>>");
          HC12.println("RDYJOY2");
          Serial.println("<<<  Send Data = RDYJOY2 >>>");
          //HC12.listen();
          step=2;
        }
        else {
          Serial.println("<<<  WRONG DATA : Correct Data = RDYJOY1 >>>");
          trigger =0;
          //함수 종료
          return 0;
        }
        */
      }
    }
   /* else if(step ==2 ){
      int condition =-1;
      if(HC12.available()){
        String input = HC12.readString();
        Serial.println(input);

        condition = dataFormatCheck(1,input,joyData);  //데이터의 포맷이 맞는지 여부 파악하고 맞으면 데이터 저장해둠

        if(condition==1){// 받은 데이터가 옳은 형식이면  
          Serial.println("<<<  Correct Data = RCVDT\r\n >>>");     
          HC12.println("RCVDT\r\n");
          trigger =0;
          //함수 종료
          return 1;
        }
        else if(condition ==0){// 받은 데이터가 틀린 형식이면 
          Serial.println("<<<  WRONG DATA : Correct Data = WRFGDT\r\n >>>");
          HC12.println("WRGDT\r\n");
          trigger=0;
          //함수 종료
          return 0;
        }
      }
    }*/
  }
}

void sendSensorVal(String data){// flightData 보내버림
  int trigger=1;

  while(trigger){
    if(HC12.available()){
      String input = HC12.readString();
      Serial.println(input);     
    
      if(input == "RDYSND\r\n"){
        HC12.println(data);// 센서 데이터값         
        trigger=0;
      }
    }
  }
}

String dataStringMaker(String motor, String servo, String height, String battery, String xiro ){
  //통신 규약에 맞게 데이터를 문자열에 담는 함수이다.
  //S A 모터세기 A 서보포즈 A 고도 A 배터리 A 자이로 A E
  String output = String("SA"+ motor + "A" +servo +"A"+height+"A"+battery+"A"+xiro+"AE");
  return output;

}

int dataFormatCheck(int dataType, String data, struct DataProtocol *protocol){//dataType은 1: PC 전송형식, 2: Flight 전송형식
  //리턴값  1: 전송 형식에 맞음, 0: 그 어느 형식에도 맞지 않음 
  //데이터 포멧 맞는지 확인하고 맞는 경우 받은 데이터 포멧 분리하여 구조체에 넣는 함수이다.
  //substring 하고 indexof사용해서 문자열을 분리한다.
  //String.toInt()
  
  // 일단 구분자 기준으로 분리한다.

  int first = data.indexOf("A");// 첫 번째 콤마 위치
  int second = data.indexOf("A",first+1); // 두 번째 콤마 위치
  int third = data.indexOf("A",second +1);
  int fourth = data.indexOf("A", third+1);
  int fifth = data.indexOf("A",fourth+1);
  int sixth =data.indexOf("A",fifth+1);

  String startByte2=data.substring(0,first);
  String motorSpeed2=data.substring(first+1,second); //pc수신의 경우 N
  String servoPos2=data.substring(second+1,third);  //pc수신의 경우 N
  String altitude2=data.substring(third+1,fourth); //무인기가 수신의 경우 N
  String battery2=data.substring(fourth+1,fifth); //무인기가 수신의 경우 N
  String xiro2=data.substring(fifth+1,sixth); //무
  String endByte2=data.substring(sixth+1,data.length());
 

  // 분리된 데이터를 가지고 판별한다.
  switch(dataType){
    case 1://PC전송 형식 체크 
    if(startByte2=="S"&&endByte2=="E"){
      if(xiro2 == "N"&& altitude2 =="N"&&battery2 == "N"){
        //맞는 형식 
        //맞는 경우에만 데이터를 넣는다. 
          protocol->motorSpeed =motorSpeed2.toInt();
          protocol->servoPos= servoPos2.toInt();
          //protocol->altitude= altitude2.toDouble();  //사용안함
          //protocol->battery= battery2.toDouble();    //사용안함
          //protocol->xiro= xiro2.toDouble();          //사용안함
          return 1;
      }
      else{
        Serial.println("Receive Joystick Data Without [N] ");
        return 0;
      }

    }
    else{
      Serial.println("Receive Joystick Data Without [S or E] ");
      return 0;
    }
    break;

    case 2://Flight 전송 형식 체크 
        if(startByte2=="S"&&endByte2=="E"){
          if(xiro2 == "N"&& altitude2 =="N"&&battery2 == "N"){
        //맞는 형식 
        //맞는 경우에만 데이터를 넣는다. 
          //protocol->motorSpeed =motorSpeed2.toInt(); //사용안함 
          //protocol->servoPos= servoPos2.toInt();     //사용안함 
          protocol->altitude= altitude2.toDouble();  
          protocol->battery= battery2.toDouble();    
          protocol->xiro= xiro2.toDouble();          
          return 1;
      }
      else{
        Serial.println("Send Sensor Data Without [N] ");
        return 0;
      }

    }
    else{
      Serial.println("Send Sensor Data Without [S or E\r\n] ");
      return 0;
    }
    break;

    default:
    break;
  }
}

 

 

 

*PC

#include <SoftwareSerial.h>
#include <String.h>
SoftwareSerial HC12(2,3);
//https://rasino.tistory.com/326
//https://docs.arduino.cc/learn/built-in-libraries/software-serial
//https://blog.naver.com/darknisia/220808977305
//https://www.youtube.com/watch?v=B6qdNjtGfXE
/*
listen() : 수신 대기 상태
소프트웨어 시리얼 통신의 Rx핀으로 데이터 수신이 올 때 까지 대기한다
여러 소프트웨어 시리얼을 사용하는 경우 앞서 말했듯이 한 번에 하나의 통신만이
가능하므로 반드시 원하는 포트를 수신 대기 생태로 지정해 주어야 한다.
그리고 데이터 수신이 오게 되며 이 함수는 true를 반환하고 대기상태라면 
false 를 반환한다.

isListening()  : 수신 대기 여부 반환
소프트웨어 시리얼이 수신 대기 상태인지 아닌지를 반환하는 함수다.

read()
데이터 수신 버퍼의 첫 번째 문자를 반환하는 함수로 앞의 peek 함수랑 같은 기능을 가진다.
하지만 name.read() 함수는 수신 버퍼에서 반환한 데이터를 제거한다 

name.readBytes(buffer, length)
이 함수는 지정한 개수만큼 읽어와 버퍼에 저장하는 함수다.
길이가 길거나 정해진 데이터를 수신할 때 유용하다
만약 지정한 개수만큼 데이터가 수신되지 않을 경우 수신된 개수만큼만 저장하고
그 개수를 반환한다.

name.write(character)
바이트 단위의 데이터를 전송한다. 
하지만 Serial 클래스와 달리 바이트 배열을 한 번에 전송하는 기능은 없어 한 번에 
하나의 데이터만을 전송할 수 있다. 
그렇기 때문에 반환되는 값이 전송한 바이트 수인데 0과 1만이 반환된다. 

name.avaiable() 
수신 버퍼에 저장되어있는 데이터의 바이트수를 반환한다. 
만약 소프트웨어 시리얼이 수신 대기 상태에 있지 않거나 버퍼가 비어 있는
경우 0을 반환한다. 
일반적으로 데이터를 받기를 기다릴 때 if 문을 이용하여 많이 사용된다. 

Serial.end();
HC12.end(); 있다
*/
struct DataProtocol {
  int motorSpeed=0; //pc수신의 경우 N
  int servoPos=0;  //pc수신의 경우 N
  double altitude=0; //무인기가 수신의 경우 N
  double battery=0; //무인기가 수신의 경우 N
  double xiro=0; //무인기가 수신의 경우 N
};

int loop1;
int loop2;
int loop3;
struct DataProtocol rcvData;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  HC12.begin(9600);
  Serial.println("PC Site");
  loop1=1;
  loop2=0;
  loop3=0;
}


int loopCount=0;

void loop() {
  // put your main code here, to run repeatedly:
  int loop1Count=0;
  int loop2Count=0;
  //조이스틱 값 시작 명령
  while(loop1==1){
    HC12.println("RDYJOY1");//1번 신호 보냄 : 통신확인 신호
    Serial.println("Sent RDYJOY1");
    delay(20);
    String getto = HC12.readString();
    if(getto == "RDYJOY2\r\n"){//2번 신호 옳게 받음 
      Serial.println("Received RDYJOY2\r\n");//
      loop1=3;//loop1 끝 {1:시작, 0:루프 초기, }
      loop2=1;//loop2 시작   
    }
    else{
      Serial.println("NOT Receive : RDYJOY2\r\n");// 1번 신호 못 받음
      loop1=1; //다시 1번 신호 보내
    }
    Serial.print("loop1Count : ");
    Serial.println(loop1Count);
    loop1Count++;

  }

  //조이스틱 값보내기
  /*
  while(loop2==1){
    HC12.println("JoystickValue");//3번 신호 보냄
    Serial.println("Sent JoystickValue");
    delay(20);
    String getto = HC12.readString();//4번신호 받음
    Serial.println(getto);

    if(getto =="RCVDT\r\n"){
      Serial.println("Good Delivery");//잘 전달됨
      loop2=3;//loop2 끝
      loop3=1;//loop3 시작
    }
    else if(getto =="WRGDT\r\n"){
      Serial.println("Bad Delivery");//잘못 전달됨
      loop2=1;
    }
    else{
      Serial.println("No Delivery");//전달받은게 없다.
      loop2=1;

    }
    Serial.print("loop2Count : ");
    Serial.println(loop2Count);
    loop2Count++;
  }
  */

  /*

  //센서값 받기 : 변수 처음처럼 설정해야함
  while(loop3>0 && loop3<5){
    HC12.println("RDYSND");//센서 1번 신호 보냄    
      Serial.println("Sent RDYSND");
      delay(20);
      String getto = HC12.readString();
      if(getto=="SensorValue\r\n"){//
        Serial.println("Receive : Correct Format Data");//
        loop1=1;
        loop2=0;
        loop3=0;
        
      }
      else{
        Serial.println("NOT Receive : Correct Format Data");//잘못된 형식의 데이터 받음 
        loop3++;
        if(loop3==5){
          loop3=0;
        }
      }
      
  }
  */
  Serial.print("loopCount : ");
  Serial.println(loopCount);
  loopCount++;
}

  /*
  int step =1;
  int next =1;
  int count =0;


    if(step ==1 ){
    do{
      HC12.println("RDYJOY1");//1번 신호 보냄    
      Serial.println("Sent RDYJOY1");
      delay(20);
      String getto = HC12.readString();
      if(getto == "RDYJOY2\r\n"){//2번 신호 옳게 받음 
        Serial.println("Received RDYJOY2\r\n");//
        step=2;
        
      }
      else{
        Serial.println("NOT Receive : RDYJOY2\r\n");// 1번 신호 못 받음
        step=1; //조이스틱 데이터 과정 그만둠.
      }

    }while(step==1);
       +
    
    }
   else if(step==2){  
      HC12.println("Joystick Value");//3번 신호 보냄
      Serial.println("Sent Joystick Value");

      String getto = HC12.readString();//4번신호 받음
      Serial.println(getto);
      if(getto =="RCVDT"){
        Serial.println("Good Delivery");//잘 전달됨
        step=3;//조이스틱 데이터 과정 그만둠.
      }
      else if(getto =="WRGDT"){
        Serial.println("Bad Delivery");//잘못 전달됨
        step=1;
      }
    }

 
  */


int sndJoyData(){//조이스틱 데이터 전송 : 파라미터>> 조이스틱값을 서보모터 세기에 mapping 해여한다

  //리턴값 1:데이터 전송 성공, 0:데이터 전송 실패 
  int trigger =1;
  int step =1;
  while(trigger){
    if(step ==1 ){
      
      HC12.println("RDYJOY1");
      Serial.println("RDYJOY1");
      String getto = HC12.readString();
      Serial.println(getto);
      delay(60);
      
      /*
      if(HC12.available()){
        String input = HC12.readString();
        Serial.println(input);
        if(input == "RDYJOY2\r\n"){ // Flight 응답이 올바를때
          Serial.println("<<<  Correct Data = RDYJOY2 >>>");
          step=2;
        }
        else{ //Flight 응답이 잘못되었을 때 
          Serial.println("<<<  WRONG DATA : Correct Data = RDYJOY2 >>>");
          trigger =0;
          //함수 종료 
          return 0;
        }
      }

      */
    }
    /*
    else if(step ==2){
      String joyFormat = dataStringMaker("10","V","N","N","N");//조이스틱 관련 데이터 넣으면 포맷맞게 문자열 제시함  
      HC12.println(joyFormat);// 조이스틱 값 보냄
      
      if(HC12.available()){
        String input = HC12.readString();
        Serial.println(input);
        if(input == "RCVDT\r\n"){//Flight 응답이 올바를때
          Serial.println("<<<  Correct Data = RCVDT >>>");
          trigger=0;
          //함수 종료
          return 1;
        }
        else if(input == "WRGDT\r\n"){
          Serial.println("<<<  WRONG DATA : Correct Data = RCVDT >>>");
          trigger =0;
          //함수 종료
          return 0;

        }

      }

    }

    */
  }
  
}



String dataStringMaker(String motor, String servo, String height, String battery, String xiro ){
  //통신 규약에 맞게 데이터를 문자열에 담는 함수이다.
  //S A 모터세기 A 서보포즈 A 고도 A 배터리 A 자이로 A E
  String output = String("SA"+ motor + "A" +servo +"A"+height+"A"+battery+"A"+xiro+"AE");
  return output;

}

int dataFormatCheck(int dataType, String data, struct DataProtocol *protocol){//dataType은 1: PC 전송형식, 2: Flight 전송형식
  //리턴값  1: 전송 형식에 맞음, 0: 그 어느 형식에도 맞지 않음 
  //데이터 포멧 맞는지 확인하고 맞는 경우 받은 데이터 포멧 분리하여 구조체에 넣는 함수이다.
  //substring 하고 indexof사용해서 문자열을 분리한다.
  //String.toInt()
  
  // 일단 구분자 기준으로 분리한다.

  int first = data.indexOf("A");// 첫 번째 콤마 위치
  int second = data.indexOf("A",first+1); // 두 번째 콤마 위치
  int third = data.indexOf("A",second +1);
  int fourth = data.indexOf("A", third+1);
  int fifth = data.indexOf("A",fourth+1);
  int sixth =data.indexOf("A",fifth+1);

  String startByte2=data.substring(0,first);
  String motorSpeed2=data.substring(first+1,second); //pc수신의 경우 N
  String servoPos2=data.substring(second+1,third);  //pc수신의 경우 N
  String altitude2=data.substring(third+1,fourth); //무인기가 수신의 경우 N
  String battery2=data.substring(fourth+1,fifth); //무인기가 수신의 경우 N
  String xiro2=data.substring(fifth+1,sixth); //무
  String endByte2=data.substring(sixth+1,data.length());
 

  // 분리된 데이터를 가지고 판별한다.
  switch(dataType){
    case 1://PC전송 형식 체크 
    if(startByte2=="S"&&endByte2=="E"){
      if(xiro2 == "N"&& altitude2 =="N"&&battery2 == "N"){
        //맞는 형식 
        //맞는 경우에만 데이터를 넣는다. 
          protocol->motorSpeed =motorSpeed2.toInt();
          protocol->servoPos= servoPos2.toInt();
          //protocol->altitude= altitude2.toDouble();  //사용안함
          //protocol->battery= battery2.toDouble();    //사용안함
          //protocol->xiro= xiro2.toDouble();          //사용안함
          return 1;
      }
      else{
        Serial.println("Receive Joystick Data Without [N] ");
        return 0;
      }

    }
    else{
      Serial.println("Receive Joystick Data Without [S or E] ");
      return 0;
    }
    break;

    case 2://Flight 전송 형식 체크 
        if(startByte2=="S"&&endByte2=="E"){
          if(xiro2 == "N"&& altitude2 =="N"&&battery2 == "N"){
        //맞는 형식 
        //맞는 경우에만 데이터를 넣는다. 
          //protocol->motorSpeed =motorSpeed2.toInt(); //사용안함 
          //protocol->servoPos= servoPos2.toInt();     //사용안함 
          protocol->altitude= altitude2.toDouble();  
          protocol->battery= battery2.toDouble();    
          protocol->xiro= xiro2.toDouble();          
          return 1;
      }
      else{
        Serial.println("Send Sensor Data Without [N] ");
        return 0;
      }

    }
    else{
      Serial.println("Send Sensor Data Without [S or E\r\n] ");
      return 0;
    }
    break;

    default:
    break;
  }
}