OA module fixed

This commit is contained in:
clf3 2023-09-14 00:58:38 +08:00
parent 5687eb932e
commit a6065fd9c2
1 changed files with 39 additions and 22 deletions

View File

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