333 lines
9.0 KiB
C++
333 lines
9.0 KiB
C++
#include<SPI.h>
|
||
#include "Arduino.h"
|
||
#include "HardwareSerial.h"
|
||
#include<SoftwareSerial.h>
|
||
#include<Servo.h>
|
||
#define trigPin 2 //超声波模块的Trig口 6#
|
||
#define echoPin 5 //超声波模块的echo口 5#
|
||
#define ServoPin 3 //底座舵机端口 3#
|
||
#define propellerFix 75
|
||
unsigned long time=0;
|
||
bool increasing=true;
|
||
bool clr=false;
|
||
//0: increase 1: decrease 2: clr
|
||
int status=0;
|
||
int clrtime=40;
|
||
int x=0;
|
||
int distRec[90];
|
||
int leftAlert=0;
|
||
int frontAlert=0;
|
||
int rightAlert=0;
|
||
int rudderModify=0;
|
||
const int autoTurn1=200;
|
||
const int autoTurn2=450;
|
||
const int alert=1000;
|
||
const int interval=100;
|
||
String sending="",received="";
|
||
Servo MyServo;
|
||
Servo RadarServo;
|
||
SoftwareSerial bluetoothSerial(7,8);
|
||
int count =0;
|
||
int IN1 =9;
|
||
int IN2 =11;
|
||
int PWM=6;
|
||
int angle=90;
|
||
int period=25;
|
||
// 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;//发送的信号
|
||
long propeller,rudder;//校验序列、读取内容和解码内容
|
||
long response=0;
|
||
const byte addresses[6] = "00001"; //创建一个数组，建立接收机地址，或者说两个模块将用于通信的“管道”
|
||
int calculateDistance()
|
||
{
|
||
long duration;
|
||
digitalWrite(trigPin, LOW);
|
||
delayMicroseconds(2);
|
||
digitalWrite(trigPin, HIGH);
|
||
delayMicroseconds(10);
|
||
digitalWrite(trigPin, LOW);
|
||
duration = pulseIn(echoPin, HIGH);
|
||
return duration*0.034/2;
|
||
}
|
||
|
||
void setup()
|
||
{
|
||
Serial.begin(9600);
|
||
bluetoothSerial.begin(57600);
|
||
bluetoothSerial.listen();
|
||
MyServo.attach(4);
|
||
RadarServo.attach(ServoPin);
|
||
pinMode(trigPin,OUTPUT);
|
||
pinMode(echoPin,INPUT);
|
||
pinMode(IN1,OUTPUT);
|
||
pinMode(IN2,OUTPUT);
|
||
pinMode(PWM,OUTPUT);
|
||
pinMode(A1,INPUT);
|
||
pinMode(A2,INPUT);
|
||
MyServo.write(45);
|
||
RadarServo.write(0);
|
||
delay(1000);
|
||
}
|
||
long confirm(long x){
|
||
if(x>=1000000&&x<=2000000)return x%1000000;
|
||
else return -1;
|
||
}
|
||
void loop() {
|
||
int distance=0;
|
||
// Serial.print("s: ");
|
||
// Serial.println(status);
|
||
if(status==0){
|
||
if(x<180){
|
||
x+=2;
|
||
}
|
||
else{
|
||
status=2;
|
||
}
|
||
RadarServo.write(x); //调整舵机角度
|
||
//测距
|
||
distance = calculateDistance();
|
||
if(distance>=100)
|
||
distance=100;
|
||
if(x!=180&&x%2==0)
|
||
distRec[x/2]=distance;
|
||
//send distance
|
||
//response=1000000+x*1000+distance;
|
||
response=0;
|
||
response+=distance;
|
||
//Serial.println(response);
|
||
response+=(long)x*1000;
|
||
//Serial.println(response);
|
||
response+=1000000;
|
||
//Serial.println(response);
|
||
}
|
||
else if(status==1){
|
||
if(x>0){
|
||
x-=2;
|
||
}
|
||
else{
|
||
status=2;
|
||
}
|
||
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;
|
||
response=0;
|
||
response+=distance;
|
||
//Serial.println(response);
|
||
response+=(long)x*1000;
|
||
//Serial.println(response);
|
||
response+=1000000;
|
||
//Serial.println(response);
|
||
//send distance
|
||
}
|
||
else if(status=2){
|
||
clrtime--;
|
||
if(clrtime<=0){
|
||
clrtime=4;
|
||
if(x>=180){
|
||
status=1;
|
||
}
|
||
else{
|
||
status=0;
|
||
}
|
||
}
|
||
//send clear
|
||
response=2000000;
|
||
}
|
||
sending=String(response);
|
||
|
||
// Serial.print("x: ");
|
||
// Serial.println(x);
|
||
// Serial.print("d: ");
|
||
// Serial.println(distance);
|
||
// Serial.println(response);
|
||
// Serial.println(sending);
|
||
bluetoothSerial.println(sending);
|
||
delay(interval);
|
||
//delay(20-distance/17);
|
||
//Serial.println(distance);
|
||
while(bluetoothSerial.available()>0){
|
||
//Serial.println("available!");
|
||
if(bluetoothSerial.peek()!='\n'){//peek()查看当前字节是不是换行
|
||
received+=(char)bluetoothSerial.read();
|
||
//Serial.println(received);
|
||
//Serial.println("no \n");
|
||
}
|
||
else{
|
||
bluetoothSerial.read();//把换行符读掉
|
||
Serial.println(received);
|
||
//Serial.println(received.length());
|
||
//content=toInt(received);
|
||
if(received[0]=='1'&&received.length()-1==7){
|
||
propeller=received.substring(4).toInt();
|
||
rudder=received.substring(1,4).toInt();
|
||
if(propeller>600||propeller<400){
|
||
if(propeller>600){
|
||
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<400){
|
||
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<=600||propeller>=400){
|
||
z=0;
|
||
analogWrite(PWM,0);
|
||
// Serial.println("stop propelling");
|
||
// Serial.println(z);
|
||
}
|
||
//Serial.println("inputing ruddering");
|
||
if(rudder>600||rudder<400)
|
||
{
|
||
if(rudder<400){
|
||
long fan1=0.1*rudder;
|
||
MyServo.write(fan1);
|
||
//Serial.println("turn left");
|
||
dj=2*fan1-90;
|
||
//Serial.println(dj);
|
||
}
|
||
else if(rudder>600){
|
||
long zheng1=0.1*rudder-10.122;
|
||
MyServo.write(zheng1);
|
||
dj=2*zheng1-90;
|
||
//Serial.println("turn right");
|
||
//Serial.println(dj);
|
||
}
|
||
}
|
||
else if(rudder<=600||rudder>=400){
|
||
dj=0;
|
||
MyServo.write(45);
|
||
//Serial.println("stop turing");
|
||
//Serial.println(dj);
|
||
}
|
||
}
|
||
else if(received[0]=='2'&&received.length()-1==7){
|
||
propeller=received.substring(4).toInt();
|
||
rudder=received.substring(1,4).toInt();
|
||
for(int i=0;i<30;i++){
|
||
rightAlert+=distRec[i];
|
||
frontAlert+=distRec[i+30];
|
||
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,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,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("inputing ruddering");
|
||
if(leftAlert<alert&&frontAlert<alert&&rightAlert<alert){
|
||
analogWrite(PWM,0);
|
||
|
||
}
|
||
else if(leftAlert<alert&&frontAlert<alert&&rightAlert>=alert){
|
||
rudder-=autoTurn2;
|
||
if(rudder<0){
|
||
rudder=0;
|
||
}
|
||
}
|
||
else if(leftAlert<alert&&frontAlert>=alert&&rightAlert<alert){
|
||
|
||
}
|
||
else if(leftAlert<alert&&frontAlert>=alert&&rightAlert>=alert){
|
||
rudder-=autoTurn2;
|
||
if(rudder<0){
|
||
rudder=0;
|
||
}
|
||
}
|
||
else if(leftAlert>=alert&&frontAlert<alert&&rightAlert<alert){
|
||
rudder+=autoTurn2;
|
||
if(rudder>999){
|
||
rudder=999;
|
||
}
|
||
}
|
||
else if(leftAlert>=alert&&frontAlert<alert&&rightAlert>=alert){
|
||
rudder-=autoTurn2;
|
||
if(rudder<0){
|
||
rudder=0;
|
||
}
|
||
}
|
||
else if(leftAlert>=alert&&frontAlert>=alert&&rightAlert<alert){
|
||
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){
|
||
long fan1=0.1*rudder;
|
||
MyServo.write(fan1);
|
||
//Serial.println("turn left");
|
||
dj=2*fan1-90;
|
||
//Serial.println(dj);
|
||
}
|
||
else if(rudder>600){
|
||
long zheng1=0.1*rudder-10.122;
|
||
MyServo.write(zheng1);
|
||
dj=2*zheng1-90;
|
||
//Serial.println("turn right");
|
||
//Serial.println(dj);
|
||
}
|
||
}
|
||
else if(rudder<=600||rudder>=400){
|
||
dj=0;
|
||
MyServo.write(45);
|
||
//Serial.println("stop turing");
|
||
//Serial.println(dj);
|
||
}
|
||
}
|
||
received="";
|
||
}
|
||
}
|
||
}
|
||
|