From a6065fd9c24d757bb257975b82ada8e1a3b14af7 Mon Sep 17 00:00:00 2001
From: clf3 <smh66ccff@outlook.com>
Date: Thu, 14 Sep 2023 00:58:38 +0800
Subject: [PATCH] OA module fixed

---
 ship_with_OA/ship_with_OA.ino | 61 ++++++++++++++++++++++-------------
 1 file changed, 39 insertions(+), 22 deletions(-)

diff --git a/ship_with_OA/ship_with_OA.ino b/ship_with_OA/ship_with_OA.ino
index 2770a48..780095a 100644
--- a/ship_with_OA/ship_with_OA.ino
+++ b/ship_with_OA/ship_with_OA.ino
@@ -20,7 +20,7 @@ int frontAlert=0;
 int rightAlert=0;
 int rudderModify=0;
 const int autoTurn1=200;
-const int autoTurn2=400;
+const int autoTurn2=450;
 const int alert=1000;
 const int interval=100;
 String sending="",received="";
@@ -29,7 +29,7 @@ Servo RadarServo;
 SoftwareSerial bluetoothSerial(7,8);
 int count =0; 
 int IN1 =9;
-int IN2 =10;
+int IN2 =11;
 int PWM=6;
 int angle=90;
 int period=25;
@@ -88,6 +88,8 @@ void loop() {
     RadarServo.write(x);             //调整舵机角度
     //测距
     distance = calculateDistance();
+    if(distance>=100)
+      distance=100;
     if(x!=180&&x%2==0)
       distRec[x/2]=distance;
     //send distance
@@ -110,6 +112,8 @@ void loop() {
     RadarServo.write(x);             //调整舵机角度
     //测距
     distance = calculateDistance();
+    if(distance>=100)
+      distance=100;
     if(x!=180&&x%2==0)
       distRec[x/2]=distance;
     //response=1000000+x*1000+distance;
@@ -143,11 +147,11 @@ void loop() {
   // Serial.print("d: ");
   // Serial.println(distance);
   // Serial.println(response);  
-  Serial.println(sending);
+  // Serial.println(sending);
   bluetoothSerial.println(sending);
   delay(interval);
   //delay(20-distance/17);
-  Serial.println(distance);
+  //Serial.println(distance);
   while(bluetoothSerial.available()>0){
     //Serial.println("available!");
     if(bluetoothSerial.peek()!='\n'){//peek()查看当前字节是不是换行
@@ -170,8 +174,8 @@ void loop() {
             digitalWrite(IN1,LOW);
             analogWrite(PWM,zheng);
             z=zheng;
-            Serial.println("forward propelling");
-            Serial.println(z);
+            // Serial.println("forward propelling");
+            // Serial.println(z);
           }
           else if(propeller<400){
             long fan=255-0.568*propeller;
@@ -179,15 +183,15 @@ void loop() {
             digitalWrite(IN2,LOW);
             analogWrite(PWM,fan);
             z=fan;
-            Serial.println("back propelling");
-            Serial.println(z);
+            // Serial.println("back propelling");
+            // Serial.println(z);
           }
         }
         else if(propeller<=600||propeller>=400){
           z=0;
           analogWrite(PWM,0);
-          Serial.println("stop propelling");
-          Serial.println(z);
+          // Serial.println("stop propelling");
+          // Serial.println(z);
         } 
         //Serial.println("inputing ruddering");
         if(rudder>600||rudder<400)
@@ -218,29 +222,35 @@ void loop() {
         propeller=received.substring(4).toInt();
         rudder=received.substring(1,4).toInt();
         for(int i=0;i<30;i++){
-          leftAlert+=distRec[i];
+          rightAlert+=distRec[i];
           frontAlert+=distRec[i+30];
-          rightAlert+=distRec[i+60];
+          leftAlert+=distRec[i+60];
         }
         if(propeller>600||propeller<400){
           if(propeller>600){ 
+            long zheng=(0.568*propeller-312.4)*0.3;
             digitalWrite(IN2,HIGH);
             digitalWrite(IN1,LOW);
-            analogWrite(PWM,propellerFix);
-            Serial.println("forward propelling");
+            analogWrite(PWM,zheng);
+            z=zheng;
+            // Serial.println("forward propelling");
+            // Serial.println(z);
           }
           else if(propeller<400){
+            long fan=(255-0.568*propeller)*0.3;
             digitalWrite(IN1,HIGH);
             digitalWrite(IN2,LOW);
-            analogWrite(PWM,propellerFix);
-            Serial.println("back propelling");
+            analogWrite(PWM,fan);
+            z=fan;
+            // Serial.println("back propelling");
+            // Serial.println(z);
           }
         }
         else if(propeller<=600||propeller>=400){
           z=0;
           analogWrite(PWM,0);
-          Serial.println("stop propelling");
-          Serial.println(z);
+          // Serial.println("stop propelling");
+          // Serial.println(z);
         } 
         //Serial.println("inputing ruddering");
         if(leftAlert<alert&&frontAlert<alert&&rightAlert<alert){
@@ -257,7 +267,7 @@ void loop() {
           
         }
         else if(leftAlert<alert&&frontAlert>=alert&&rightAlert>=alert){
-          rudder-=autoTurn1;
+          rudder-=autoTurn2;
           if(rudder<0){
             rudder=0;
           }
@@ -275,15 +285,22 @@ void loop() {
           }
         }
         else if(leftAlert>=alert&&frontAlert>=alert&&rightAlert<alert){
-          rudder+=autoTurn1;
+          rudder+=autoTurn2;
           if(rudder>999){
             rudder=999;
           }
         }
         else if(leftAlert>=alert&&frontAlert>=alert&&rightAlert>=alert){
           
-        }        
-        
+        } 
+        Serial.print(leftAlert);
+        Serial.print(" ");
+        Serial.print(frontAlert);
+        Serial.print(" ");
+        Serial.println(rightAlert);        
+        leftAlert=0;
+        frontAlert=0;
+        rightAlert=0;
         if(rudder>600||rudder<400)
         {
           if(rudder<400){