From 42b038308dc3d495cf161b151545c7954b1f8b6e Mon Sep 17 00:00:00 2001
From: clf3 <smh66ccff@outlook.com>
Date: Mon, 4 Sep 2023 21:42:44 +0800
Subject: [PATCH] ship new protocol

---
 ship/ship.ino | 196 ++++++++++++++++++++++++--------------------------
 1 file changed, 92 insertions(+), 104 deletions(-)

diff --git a/ship/ship.ino b/ship/ship.ino
index e31ae90..1a74d01 100644
--- a/ship/ship.ino
+++ b/ship/ship.ino
@@ -9,8 +9,8 @@ Servo MyServo;
 Servo RidaServo;
 RF24 radio(7,8);
 int count =0; 
-int IN1 =7;
-int IN2 = 8;
+int IN1 =9;
+int IN2 =10;
 int PWM=6;
 int TrigPin=2;
 int EcoPin=3;
@@ -21,61 +21,47 @@ 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;//校验序列、读取内容和解码内容 
+long propeller,rudder,content;//校验序列、读取内容和解码内容 
 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);
+    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.setChannel(114);
   radio.openReadingPipe(1,addresses);   //接收通道，
   radio.openWritingPipe(addresses);//发送通道
   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() {
-  //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;
   }
   else if(angle>=180){
     dAngle=-5;
   }
-  angle=angle+dAngle;
+  /*angle=angle+dAngle;
   RidaServo.write(angle);
   digitalWrite(TrigPin,LOW);
   delayMicroseconds(8);
@@ -88,74 +74,76 @@ void loop() {
   }
   if(dist/17<period){
     delay(period-dist/17);
-  }  
+  }  */
   radio.startListening();
- // Serial.println("start listening!");
+//  Serial.println("start listening!");
   if(radio.available()>0){
-    radio.read(&content,sizeof(content));
+   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);
-        }
+    content=confirm(content);
+    if(content!=-1){
+     propeller=content%1000;
+     rudder=(content-propeller)/1000;
+     if(propeller>550||propeller<449)
+     {
+      if(propeller>550)
+      { 
+       long zheng=0.568*propeller-312.4;
+       digitalWrite(IN2,HIGH);
+       digitalWrite(IN1,LOW);
+       analogWrite(PWM,zheng);
+       z=zheng;
+       Serial.println("forward propelling");
+       Serial.println(z);
+       }
+     else if(propeller<449)
+      {
+      long fan=255-0.568*propeller;
+      digitalWrite(IN1,HIGH);
+      digitalWrite(IN2,LOW);
+      analogWrite(PWM,fan);
+      z=fan;
+      Serial.println("back propelling");
+      Serial.println(z);
       }
     }
-  }
+     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.write(&dist,sizeof(dist));
  // radio.write(&dj,sizeof(dj));