#include<SPI.h>
#include<nRF24L01.h>
#include<RF24.h>
#include "Arduino.h"
#include "HardwareSerial.h"
#include<Servo.h>

Servo MyServo;
Servo RidaServo;
RF24 radio(7,8);
int count =0; 
int IN1 =7;
int IN2 = 8;
int PWM=6;
int TrigPin=2;
int EcoPin=3;
int dAngle=5;
int angle=90;
int period=25;
float dist;
long dj,z;//dj是舵机，z是螺旋桨转速
//long propeller=0,rudder=0;//propeller为螺旋桨（0-1023，512为静止），rudder为舵（0-1023，512为静止）
//long s_propeller=0,s_rudder=0;//发送的信号
const long R_prop=5,R_rudd=6;//输出端口
long check_array[7],content,after_decode;//校验序列、读取内容和解码内容 
const byte addresses[6] = "00001";  //创建一个数组，建立接收机地址，或者说两个模块将用于通信的“管道”

long decode_and_confirm(long x){
  long sum=0,add=0,x1=x;
  while(x1!=0){
    check_array[add]=x1%10;
    x1/=10;
    add++;
  }
  for(long i=0,j=4;i<add;j--,i++){
    sum+=check_array[i]*j;
  }
  if(sum%10==check_array[4])
  return -1;//代表校验失败
  return x%10000;
}
void setup()
{
  
  Serial.begin(9600);
  MyServo.attach(4);
  RidaServo.attach(5);
  pinMode(TrigPin,OUTPUT);
  pinMode(EcoPin,INPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(PWM,OUTPUT);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  MyServo.write(angle);
  radio.begin();
 // radio.setChannel(114);
  radio.openReadingPipe(1,addresses);   //接收通道，
  radio.openWritingPipe(addresses);//发送通道
  radio.setPALevel(RF24_PA_HIGH);   //设置功率放大器级别，将其设置为最小值
  delay(1000);
}

void loop() {
  //Serial.println(sizeof(long));
  analogWrite(PWM,0);
  radio.startListening();
  //Serial.println("start listening!");

  
  if(angle<=0){
    dAngle=5;
  }
  else if(angle>=180){
    dAngle=-5;
  }
  angle=angle+dAngle;
  RidaServo.write(angle);
  digitalWrite(TrigPin,LOW);
  delayMicroseconds(8);
  digitalWrite(TrigPin,HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin,LOW);
  dist=pulseIn(EcoPin,HIGH,58800)/58.8;
  if(dist<=0.5){
    dist=100.0;
  }
  if(dist/17<period){
    delay(period-dist/17);
  }  
  radio.startListening();
 // Serial.println("start listening!");
  if(radio.available()>0){
    radio.read(&content,sizeof(content));
    Serial.println("The content is");
    Serial.println(content);//调试
    after_decode=decode_and_confirm(content);
    Serial.println(after_decode);
    if(after_decode!=-1&&content>=100000){
      if(content<200000){
        Serial.println("inputing propeller ");
        if(after_decode>600||after_decode<500){
          if(after_decode>600){ 
            long zheng=after_decode;
            digitalWrite(IN2,HIGH);
            digitalWrite(IN1,LOW);
            analogWrite(PWM,zheng);
            delay(50);
            z=zheng-512;
            Serial.println("forward propelling");
            Serial.println(z);
          }
          else if(after_decode<500){
            long fan=1100-after_decode;
            digitalWrite(IN1,HIGH);
            digitalWrite(IN2,LOW);
            analogWrite(PWM,fan);
            delay(50);
            z=fan-512;
            Serial.println("back propelling");
            Serial.println(z);
          }
        }
        else if(after_decode<=600||after_decode>=500){
          z=0;
          Serial.println("stop propelling");
          Serial.println(z);
        }
      } 
      else{
        Serial.println("inputing ruddering");
        if(after_decode>600||after_decode<500){
          if(after_decode<500){
            long fan1=0.18*after_decode;
            MyServo.write(fan1/2);
            delay(150);
            dj=fan1-90;
            Serial.println("turn left");
            Serial.println(dj);
          }
          else if(after_decode>600){
            long zheng1=0.22*after_decode-42;
            MyServo.write(zheng1/2);
            delay(150);
            dj=zheng1-90;
            Serial.println("turn right");
            Serial.println(dj);
          }
        } 
        else if(after_decode<=600||after_decode>=500){
          dj=0;
          Serial.println("stop turing");
          Serial.println(dj);
        }
      }
    }
  }
 // radio.stopListening();
  //radio.write(&dist,sizeof(dist));
 // radio.write(&dj,sizeof(dj));
  //radio.write(&z,sizeof(z));
  //radio.startListening();
  //delay(20);
 /* if (radio.available()) {  //判断是否有要接收的数据
    radio.read(response,sizeof(response));
    Serial.print(text);
    Serial.println(response);
  }*/
  //delay(20);  //延迟等待0.3秒 
  //text++;
}