#include<Servo.h>//加载伺服器包
#include <AFMotor.h> //加载
//CH1升降，CH2副翼，CH3油门，CH4方向

int CH[7],AH[7],i,j;  
int R[5];                //1,2,3,4分别对应左前，右前，右后，左后
AF_DCMotor m1(3,MOTOR12_64KHZ);
AF_DCMotor m2(4,MOTOR12_64KHZ);
AF_DCMotor m3(1,MOTOR12_64KHZ);
AF_DCMotor m4(2,MOTOR12_64KHZ);
Servo A[9];              //定义脉冲输出接口

void setup() {
   //定义输入接口
   for(i=14;i<=16;i++){pinMode(i,INPUT);}
   Serial.begin(9600);
}

void loop() {
  //for(i=2;i<=4;i++){CH[i]= pulseIn(i,HIGH);}//获取脉冲接收机脉冲信号
  CH[2]= pulseIn(14,HIGH);
  CH[3]= pulseIn(15,HIGH);
  CH[4]= pulseIn(16,HIGH);
  mapCH();               //计算通道输出范围
  wheel();               //计算*速度
  wheelout();            //输出*方向和速度 
  
  Serial.print(R[3]);
  Serial.print(",");
  Serial.print(R[2]);
  Serial.print(",");
  Serial.print(R[3]);
  Serial.print(",");
  Serial.println(R[4]);
  delay(20);
}

void mapCH(){            //计算通道输出范围
  for(i=2;i<=4;i++){
    if(CH[i]>=1425 and CH[i]<=1575){
      AH[i]=0;
    }
    if(CH[i]>1575){
      //AH[i] = map(CH[i],1575,1910,10,85);
      AH[i] = map(CH[i],1575,1920,130,255);
    }
    if(CH[i]<1425){ 
      //AH[i] = map(CH[i],1100,1425,-85,-10);
      AH[i] = map(CH[i],1090,1425,-255,-130);
    }
  }
  return AH[2],AH[3],AH[4];
}
void wheel(){            //计算*速度 
  R[1]= AH[2]+AH[3]-AH[4];
  R[2]=-AH[2]+AH[3]-AH[4];
  R[3]= AH[2]+AH[3]+AH[4];
  R[4]=-AH[2]+AH[3]+AH[4];
  return R[1],R[2],R[3],R[4];
}
void wheelout(){         //输出*方向和速度 
  if(R[1]==0){
      m1.run(RELEASE);
    }else if(R[1]>0){
      m1.setSpeed(R[1]);
      m1.run(FORWARD);
    }else if(R[1]<0){
      m1.setSpeed(-R[1]);
      m1.run(BACKWARD);
    } 
  if(R[2]==0){
      m2.run(RELEASE);
    }else if(R[2]>0){
      m2.setSpeed(R[2]);
      m2.run(FORWARD);
    }else if(R[2]<0){
      m2.setSpeed(-R[2]);
      m2.run(BACKWARD);
    } 
  if(R[3]==0){
      m3.run(RELEASE);
    }else if(R[3]>0){
      m3.setSpeed(R[3]);
      m3.run(FORWARD);
    }else if(R[3]<0){
      m3.setSpeed(-R[3]);
      m3.run(BACKWARD);
    } 
  if(R[4]==0){
      m4.run(RELEASE);
    }else if(R[4]>0){
      m4.setSpeed(R[4]);
      m4.run(FORWARD);
    }else if(R[4]<0){
      m4.setSpeed(-R[4]);
      m4.run(BACKWARD);
    } 
}