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&&rightAlert>=alert){ - rudder-=autoTurn1; + rudder-=autoTurn2; if(rudder<0){ rudder=0; } @@ -275,15 +285,22 @@ void loop() { } } else if(leftAlert>=alert&&frontAlert>=alert&&rightAlert999){ 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){