173 lines
4.5 KiB
C++
173 lines
4.5 KiB
C++
#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++;
|
||
}
|
||
|