OA module fixed
This commit is contained in:
parent
5687eb932e
commit
a6065fd9c2
|
@ -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,7 +285,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>999){
|
if(rudder>999){
|
||||||
rudder=999;
|
rudder=999;
|
||||||
}
|
}
|
||||||
|
@ -283,7 +293,14 @@ void loop() {
|
||||||
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){
|
||||||
|
|
Loading…
Reference in New Issue