//MPU6050相关设置

int p2 = 0; //状态开关值
int p3 = 0; //状态开关值
int p4 = 0; //状态开关值
int p5 = 0; //状态开关值
int p6 = 0; //状态开关值
int p7 = 0; //状态开关值
int p8 = 0; //状态开关值
int p9 = 0; //状态开关值
/**控制接收端变量设置start***/
int i=0;
int iii=0;
int ii[3];
int ss[4];
int s=1;//speed
int a=0;// RX init
int h=0;
int p=1;
int op=1;
int staus=0;// 当前状态，为1时可飞
int pp=0;//限飞电压
int limit=90;//pwm最大输出（0-255）
/**控制接收端变量设置end***/


void setup() {

    /**MPU6050初始化 end***/

    /**串口初始化 start***/
   Serial2.begin(9600);
   Serial.begin(9600);
   /**串口初始化 end***/

    /**电机输出口初始化 start***/
   pinMode(22,OUTPUT);
   pinMode(23,OUTPUT);
   pinMode(24,OUTPUT);
   pinMode(25,OUTPUT);
    pinMode(26,OUTPUT);
   pinMode(27,OUTPUT);
   pinMode(28,OUTPUT);
   pinMode(29,OUTPUT);
    
     /**电机输出口初始化 end***/
  pinMode(2, INPUT); //遥控启动引脚初始化
  pinMode(3, INPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT);
  pinMode(7, INPUT);


}

void loop() {
  /**遥控启动 start***/
if ((digitalRead(2) == 0) && (p2 == 0))
  {
    delay(150);
    p2 = 1;
    pt(12);

  }
  /**遥控启动 end***/

  /**拨码开关遥控停止 start***/
  if ((digitalRead(2) == 1) && (p2 == 1))
  { p2 = 0;
    pt(22);


  }
  if ((digitalRead(3) == 0) && (p3 == 0))
  { delay(150);
    p3 = 1;
    pt(13);

  }
  /**遥控启动 end***/

  /**拨码开关遥控停止 start***/
  if ((digitalRead(3) == 1) && (p3 == 1))
  { p3 = 0;
    pt(23);


  }
  if ((digitalRead(4) == 0) && (p4 == 0))
  { delay(150);
    p4 = 1;
    pt(14);

  }
  /**遥控启动 end***/

  /**拨码开关遥控停止 start***/
  if ((digitalRead(4) == 1) && (p4 == 1))
  { p4 = 0;
    pt(24);


  }
  if ((digitalRead(5) == 0) && (p5 == 0))
  { delay(150);
    p5 = 1;
    pt(15);

  }
  /**遥控启动 end***/

  /**拨码开关遥控停止 start***/
  if ((digitalRead(5) == 1) && (p5 == 1))
  { p5 = 0;
    pt(25);


  }
  if ((digitalRead(6) == 0) && (p6 == 0))
  { delay(150);
    p6 = 1;
    pt(16);

  }
  /**遥控启动 end***/

  /**拨码开关遥控停止 start***/
  if ((digitalRead(6) == 1) && (p6 == 1))
  { p6 = 0;
    pt(26);


  }
  if ((digitalRead(7) == 0) && (p7 == 0))
  { delay(150);
    p7 = 1;

    pt(17);

  }
  /**遥控启动 end***/

  /**拨码开关遥控停止 start***/
  if ((digitalRead(7) == 1) && (p7 == 1))
  {
    p7 = 0;
    pt(27);


  }


       /**遥控数据接收 start***/
  if( Serial2.available()>1)
 { i=Serial2.read();
 if(i>0)
math();   //遥控数据处理函数调用
 }


}





//遥控数据处理函数
void math()
{
  
   /**停止飞行声音提醒 start***/

  /**停止飞行声音提醒 end***/
  
   if(i==38)
  {
    a=1;
    
  }

  if(i==33)
  { 
if(iii==1)
ss[1]=ii[0];
if(iii==2)
{ss[1]=ii[0]*10+ii[1];
 iii=0;
 }
  }
  if(i==37)
  {
//  x1=ss[1];  //x轴控制值
//  y1=ss[2];  //x轴控制值
/// iii=0;
  s=ss[1];     //Speed控制值
Serial.println(s);
if(s==22)
digitalWrite(22,HIGH);
if(s==12)
digitalWrite(22,LOW);
if(s==23)
digitalWrite(23,HIGH);
if(s==13)
digitalWrite(23,LOW);
if(s==24)
digitalWrite(24,HIGH);
if(s==14)
digitalWrite(24,LOW);
if(s==25)
digitalWrite(25,HIGH);
if(s==15)
digitalWrite(25,LOW);
if(s==26)
digitalWrite(26,HIGH);
if(s==16)
digitalWrite(26,LOW);
if(s==27)
digitalWrite(27,HIGH);
if(s==17)
digitalWrite(27,LOW);
if(s==28)
digitalWrite(28,HIGH);
if(s==18)
digitalWrite(28,LOW);
if(s==29)
digitalWrite(29,HIGH);
if(s==19)
digitalWrite(29,LOW);
 
 
   a=0;       //数据接收控制符
  }
  if(i>47)
  {
   iii=iii+1;
   if(iii==1)
   ii[0]=i-48;
    if(iii==2)
   ii[1]=i-48;
  if(iii==3)
   ii[2]=i-48; 
      i=0;  
  }
}

 /**遥控停止飞行函数 end***/
  /**遥控发送函数 start***/
void pt(int S)
{

 /**遥控端发送格式 start***/
      Serial2.print("&");//38
  Serial2.print(S);
  Serial2.print("!");//33
  Serial2.print("%");//37

  //Serial2.print("%");//37
  Serial2.print("&");//38 用于将每次传入的数值读取到，防止缓冲导致读取的不及时！
  
  /**遥控端停止飞行指令 end***/

}

  /**低压停止飞行函数 end***/