ship new protocol

This commit is contained in:
clf3 2023-09-04 21:42:44 +08:00
parent 805f8de61c
commit 42b038308d
1 changed files with 92 additions and 104 deletions

View File

@ -9,8 +9,8 @@ Servo MyServo;
Servo RidaServo; Servo RidaServo;
RF24 radio(7,8); RF24 radio(7,8);
int count =0; int count =0;
int IN1 =7; int IN1 =9;
int IN2 = 8; int IN2 =10;
int PWM=6; int PWM=6;
int TrigPin=2; int TrigPin=2;
int EcoPin=3; int EcoPin=3;
@ -21,61 +21,47 @@ float dist;
long dj,z;//dj是舵机z是螺旋桨转速 long dj,z;//dj是舵机z是螺旋桨转速
//long propeller=0,rudder=0;//propeller为螺旋桨0-1023512为静止rudder为舵0-1023512为静止 //long propeller=0,rudder=0;//propeller为螺旋桨0-1023512为静止rudder为舵0-1023512为静止
//long s_propeller=0,s_rudder=0;//发送的信号 //long s_propeller=0,s_rudder=0;//发送的信号
const long R_prop=5,R_rudd=6;//输出端口 long propeller,rudder,content;//校验序列、读取内容和解码内容
long check_array[7],content,after_decode;//校验序列、读取内容和解码内容
const byte addresses[6] = "00001"; //创建一个数组,建立接收机地址,或者说两个模块将用于通信的“管道” 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() void setup()
{ {
Serial.begin(9600);
Serial.begin(9600); MyServo.attach(4);
MyServo.attach(4); RidaServo.attach(5);
RidaServo.attach(5); pinMode(TrigPin,OUTPUT);
pinMode(TrigPin,OUTPUT); pinMode(EcoPin,INPUT);
pinMode(EcoPin,INPUT); pinMode(IN1,OUTPUT);
pinMode(IN1,OUTPUT); pinMode(IN2,OUTPUT);
pinMode(IN2,OUTPUT); pinMode(PWM,OUTPUT);
pinMode(PWM,OUTPUT); pinMode(A1,INPUT);
pinMode(A1,INPUT); pinMode(A2,INPUT);
pinMode(A2,INPUT); MyServo.write(angle);
MyServo.write(angle);
radio.begin(); radio.begin();
// radio.setChannel(114); //radio.setChannel(114);
radio.openReadingPipe(1,addresses); //接收通道, radio.openReadingPipe(1,addresses); //接收通道,
radio.openWritingPipe(addresses);//发送通道 radio.openWritingPipe(addresses);//发送通道
radio.setPALevel(RF24_PA_HIGH); //设置功率放大器级别,将其设置为最小值 radio.setPALevel(RF24_PA_HIGH); //设置功率放大器级别,将其设置为最小值
//radio.setChannel(0); delay(1000);
delay(1000); MyServo.write(45);
}
long confirm(long x){
if(x>=1000000&&x<=2000000)return x%1000000;
else return -1;
} }
void loop() { void loop() {
//Serial.println(sizeof(long));
analogWrite(PWM,0);
radio.startListening();
//Serial.println("start listening!");
if(angle<=0){ radio.startListening();
// Serial.println("start listening!");
/*if(angle<=0){
dAngle=5; dAngle=5;
} }
else if(angle>=180){ else if(angle>=180){
dAngle=-5; dAngle=-5;
} }
angle=angle+dAngle; /*angle=angle+dAngle;
RidaServo.write(angle); RidaServo.write(angle);
digitalWrite(TrigPin,LOW); digitalWrite(TrigPin,LOW);
delayMicroseconds(8); delayMicroseconds(8);
@ -88,74 +74,76 @@ void loop() {
} }
if(dist/17<period){ if(dist/17<period){
delay(period-dist/17); delay(period-dist/17);
} } */
radio.startListening(); radio.startListening();
// Serial.println("start listening!"); // Serial.println("start listening!");
if(radio.available()>0){ if(radio.available()>0){
radio.read(&content,sizeof(content)); radio.read(&content,sizeof(content));
Serial.println("The content is"); Serial.println("The content is");
Serial.println(content);//调试 Serial.println(content);//调试
after_decode=decode_and_confirm(content); content=confirm(content);
Serial.println(after_decode); if(content!=-1){
if(after_decode!=-1&&content>=100000){ propeller=content%1000;
if(content<200000){ rudder=(content-propeller)/1000;
Serial.println("inputing propeller "); if(propeller>550||propeller<449)
if(after_decode>600||after_decode<500){ {
if(after_decode>600){ if(propeller>550)
long zheng=after_decode; {
digitalWrite(IN2,HIGH); long zheng=0.568*propeller-312.4;
digitalWrite(IN1,LOW); digitalWrite(IN2,HIGH);
analogWrite(PWM,zheng); digitalWrite(IN1,LOW);
delay(50); analogWrite(PWM,zheng);
z=zheng-512; z=zheng;
Serial.println("forward propelling"); Serial.println("forward propelling");
Serial.println(z); Serial.println(z);
} }
else if(after_decode<500){ else if(propeller<449)
long fan=1100-after_decode; {
digitalWrite(IN1,HIGH); long fan=255-0.568*propeller;
digitalWrite(IN2,LOW); digitalWrite(IN1,HIGH);
analogWrite(PWM,fan); digitalWrite(IN2,LOW);
delay(50); analogWrite(PWM,fan);
z=fan-512; z=fan;
Serial.println("back propelling"); Serial.println("back propelling");
Serial.println(z); 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);
}
} }
} }
} else if(propeller<=550||propeller>=449)
{
z=0;
analogWrite(PWM,0);
Serial.println("stop propelling");
Serial.println(z);
}
Serial.println("inputing ruddering");
if(rudder>550||rudder<449)
{
if(rudder<449)
{
long fan1=0.1*rudder;
MyServo.write(fan1);
Serial.println("turn left");
dj=2*fan1-90;
Serial.println(dj);
}
else if(rudder>550)
{
long zheng1=0.1*rudder-10.122;
MyServo.write(zheng1);
dj=2*zheng1-90;
Serial.println("turn right");
Serial.println(dj);
}
}
else if(rudder<=550||rudder>=449)
{
dj=0;
MyServo.write(45);
Serial.println("stop turing");
Serial.println(dj);
}
}
}
// radio.stopListening(); // radio.stopListening();
//radio.write(&dist,sizeof(dist)); //radio.write(&dist,sizeof(dist));
// radio.write(&dj,sizeof(dj)); // radio.write(&dj,sizeof(dj));