radar added to ship
This commit is contained in:
parent
8262e30455
commit
46182acec1
|
@ -1,6 +1,7 @@
|
||||||
#include <SPI.h>
|
#include <SPI.h>
|
||||||
#include "Ucglib.h"
|
#include "Ucglib.h"
|
||||||
#define interruptPin 2
|
#define interruptPin 2
|
||||||
|
|
||||||
int Ymax = 128; //屏幕的竖向像素数
|
int Ymax = 128; //屏幕的竖向像素数
|
||||||
int Xmax = 160; //屏幕的横向像素数
|
int Xmax = 160; //屏幕的横向像素数
|
||||||
int Xcent = Xmax / 2; //x中位
|
int Xcent = Xmax / 2; //x中位
|
||||||
|
|
|
@ -10,7 +10,8 @@ bool increasing=true;
|
||||||
bool clr=false;
|
bool clr=false;
|
||||||
//0: increase 1: decrease 2: clr
|
//0: increase 1: decrease 2: clr
|
||||||
int status=0;
|
int status=0;
|
||||||
int clrtimes=40;
|
int clrtime=40;
|
||||||
|
int x=0;
|
||||||
void setup(void)
|
void setup(void)
|
||||||
{
|
{
|
||||||
pinMode(trigPin, OUTPUT); //设置trigPin端口模式
|
pinMode(trigPin, OUTPUT); //设置trigPin端口模式
|
||||||
|
@ -42,6 +43,10 @@ void loop(void)
|
||||||
else{
|
else{
|
||||||
status=2;
|
status=2;
|
||||||
}
|
}
|
||||||
|
baseServo.write(x); //调整舵机角度
|
||||||
|
//测距
|
||||||
|
distance = calculateDistance();
|
||||||
|
//send distance
|
||||||
}
|
}
|
||||||
else if(status==1){
|
else if(status==1){
|
||||||
if(x>0){
|
if(x>0){
|
||||||
|
@ -50,9 +55,12 @@ void loop(void)
|
||||||
else{
|
else{
|
||||||
status=2;
|
status=2;
|
||||||
}
|
}
|
||||||
|
baseServo.write(x); //调整舵机角度
|
||||||
|
//测距
|
||||||
|
distance = calculateDistance();
|
||||||
|
//send distance
|
||||||
}
|
}
|
||||||
else if(status=2){
|
else if(status=2){
|
||||||
|
|
||||||
clrtime--;
|
clrtime--;
|
||||||
if(clrtime<=0){
|
if(clrtime<=0){
|
||||||
clrtime=4;
|
clrtime=4;
|
||||||
|
@ -63,14 +71,6 @@ void loop(void)
|
||||||
status=0;
|
status=0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
if(status!=2){
|
|
||||||
baseServo.write(x); //调整舵机角度
|
|
||||||
//测距
|
|
||||||
distance = calculateDistance();
|
|
||||||
//send distance
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
//send clear
|
//send clear
|
||||||
}
|
}
|
||||||
delay(20-distance/17);
|
delay(20-distance/17);
|
||||||
|
|
|
@ -9,7 +9,6 @@ long propeller=0,rudder=0,dist;//propeller为螺旋桨(0-1023,512为静止
|
||||||
long sending_signal=0;//发送的信号
|
long sending_signal=0;//发送的信号
|
||||||
const byte addresser[6]={"00001"};//创建通信通道地址,6用来写,8用来读
|
const byte addresser[6]={"00001"};//创建通信通道地址,6用来写,8用来读
|
||||||
void setup() {
|
void setup() {
|
||||||
// put your setup code here, to run once:
|
|
||||||
Serial.begin(9600);
|
Serial.begin(9600);
|
||||||
radio.begin();
|
radio.begin();
|
||||||
//radio.setChannel(114);//设置信道,114号通道
|
//radio.setChannel(114);//设置信道,114号通道
|
||||||
|
@ -40,5 +39,5 @@ void send(){
|
||||||
}
|
}
|
||||||
void loop() {
|
void loop() {
|
||||||
// put your main code here, to run repeatedly:
|
// put your main code here, to run repeatedly:
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
240
ship/ship.ino
240
ship/ship.ino
|
@ -4,158 +4,176 @@
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
#include "HardwareSerial.h"
|
#include "HardwareSerial.h"
|
||||||
#include<Servo.h>
|
#include<Servo.h>
|
||||||
|
#define trigPin 6 //超声波模块的Trig口 6#
|
||||||
|
#define echoPin 5 //超声波模块的echo口 5#
|
||||||
|
#define ServoPin 3 //底座舵机端口 3#
|
||||||
|
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;
|
||||||
|
|
||||||
Servo MyServo;
|
Servo MyServo;
|
||||||
Servo RidaServo;
|
Servo RadarServo;
|
||||||
RF24 radio(7,8);
|
RF24 radio(7,8);
|
||||||
int count =0;
|
int count =0;
|
||||||
int IN1 =9;
|
int IN1 =9;
|
||||||
int IN2 =10;
|
int IN2 =10;
|
||||||
int PWM=6;
|
int PWM=6;
|
||||||
int TrigPin=2;
|
// int TrigPin=2;
|
||||||
int EcoPin=3;
|
// int EcoPin=3;
|
||||||
int dAngle=5;
|
// int dAngle=5;
|
||||||
int angle=90;
|
int angle=90;
|
||||||
int period=25;
|
int period=25;
|
||||||
float dist;
|
// float dist;
|
||||||
long dj,z;//dj是舵机,z是螺旋桨转速
|
long dj,z;//dj是舵机,z是螺旋桨转速
|
||||||
//long propeller=0,rudder=0;//propeller为螺旋桨(0-1023,512为静止),rudder为舵(0-1023,512为静止)
|
//long propeller=0,rudder=0;//propeller为螺旋桨(0-1023,512为静止),rudder为舵(0-1023,512为静止)
|
||||||
//long s_propeller=0,s_rudder=0;//发送的信号
|
//long s_propeller=0,s_rudder=0;//发送的信号
|
||||||
long propeller,rudder,content;//校验序列、读取内容和解码内容
|
long propeller,rudder,content;//校验序列、读取内容和解码内容
|
||||||
const byte addresses[6] = "00001"; //创建一个数组,建立接收机地址,或者说两个模块将用于通信的“管道”
|
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()
|
void setup()
|
||||||
{
|
{
|
||||||
Serial.begin(9600);
|
Serial.begin(9600);
|
||||||
MyServo.attach(4);
|
MyServo.attach(4);
|
||||||
RidaServo.attach(5);
|
RadarServo.attach(ServoPin);
|
||||||
pinMode(TrigPin,OUTPUT);
|
pinMode(trigPin,OUTPUT);
|
||||||
pinMode(EcoPin,INPUT);
|
pinMode(echoPin,INPUT);
|
||||||
pinMode(IN1,OUTPUT);
|
pinMode(IN1,OUTPUT);
|
||||||
pinMode(IN2,OUTPUT);
|
pinMode(IN2,OUTPUT);
|
||||||
pinMode(PWM,OUTPUT);
|
pinMode(PWM,OUTPUT);
|
||||||
pinMode(A1,INPUT);
|
pinMode(A1,INPUT);
|
||||||
pinMode(A2,INPUT);
|
pinMode(A2,INPUT);
|
||||||
MyServo.write(angle);
|
MyServo.write(angle);
|
||||||
|
RadarServo.write(0);
|
||||||
radio.begin();
|
radio.begin();
|
||||||
//radio.setChannel(114);
|
//radio.setChannel(114);
|
||||||
radio.openReadingPipe(1,addresses); //接收通道,
|
radio.openReadingPipe(1,addresses); //接收通道,
|
||||||
radio.openWritingPipe(addresses);//发送通道
|
radio.openWritingPipe(addresses);//发送通道
|
||||||
radio.setPALevel(RF24_PA_HIGH); //设置功率放大器级别,将其设置为最小值
|
radio.setPALevel(RF24_PA_HIGH); //设置功率放大器级别,将其设置为最小值
|
||||||
delay(1000);
|
delay(1000);
|
||||||
MyServo.write(45);
|
MyServo.write(45);
|
||||||
}
|
}
|
||||||
long confirm(long x){
|
long confirm(long x){
|
||||||
if(x>=1000000&&x<=2000000)return x%1000000;
|
if(x>=1000000&&x<=2000000)return x%1000000;
|
||||||
else return -1;
|
else return -1;
|
||||||
}
|
}
|
||||||
void loop() {
|
void loop() {
|
||||||
|
radio.startListening();
|
||||||
|
|
||||||
radio.startListening();
|
|
||||||
// Serial.println("start listening!");
|
|
||||||
/*if(angle<=0){
|
|
||||||
dAngle=5;
|
|
||||||
}
|
|
||||||
else if(angle>=180){
|
|
||||||
dAngle=-5;
|
|
||||||
}
|
|
||||||
/*angle=angle+dAngle;
|
|
||||||
RidaServo.write(angle);
|
|
||||||
digitalWrite(TrigPin,LOW);
|
|
||||||
delayMicroseconds(8);
|
|
||||||
digitalWrite(TrigPin,HIGH);
|
|
||||||
delayMicroseconds(10);
|
|
||||||
digitalWrite(TrigPin,LOW);
|
|
||||||
dist=pulseIn(EcoPin,HIGH,58800)/58.8;
|
|
||||||
if(dist<=0.5){
|
|
||||||
dist=100.0;
|
|
||||||
}
|
|
||||||
if(dist/17<period){
|
|
||||||
delay(period-dist/17);
|
|
||||||
} */
|
|
||||||
radio.startListening();
|
radio.startListening();
|
||||||
// Serial.println("start listening!");
|
|
||||||
if(radio.available()>0){
|
if(radio.available()>0){
|
||||||
radio.read(&content,sizeof(content));
|
radio.read(&content,sizeof(content));
|
||||||
Serial.println("The content is");
|
Serial.println("The content is");
|
||||||
Serial.println(content);//调试
|
Serial.println(content);//调试
|
||||||
content=confirm(content);
|
content=confirm(content);
|
||||||
if(content!=-1){
|
if(content!=-1){
|
||||||
propeller=content%1000;
|
propeller=content%1000;
|
||||||
rudder=(content-propeller)/1000;
|
rudder=(content-propeller)/1000;
|
||||||
if(propeller>550||propeller<449)
|
if(propeller>550||propeller<449){
|
||||||
{
|
if(propeller>550){
|
||||||
if(propeller>550)
|
long zheng=0.568*propeller-312.4;
|
||||||
{
|
digitalWrite(IN2,HIGH);
|
||||||
long zheng=0.568*propeller-312.4;
|
digitalWrite(IN1,LOW);
|
||||||
digitalWrite(IN2,HIGH);
|
analogWrite(PWM,zheng);
|
||||||
digitalWrite(IN1,LOW);
|
z=zheng;
|
||||||
analogWrite(PWM,zheng);
|
Serial.println("forward propelling");
|
||||||
z=zheng;
|
Serial.println(z);
|
||||||
Serial.println("forward propelling");
|
}
|
||||||
Serial.println(z);
|
else if(propeller<449){
|
||||||
}
|
long fan=255-0.568*propeller;
|
||||||
else if(propeller<449)
|
digitalWrite(IN1,HIGH);
|
||||||
{
|
digitalWrite(IN2,LOW);
|
||||||
long fan=255-0.568*propeller;
|
analogWrite(PWM,fan);
|
||||||
digitalWrite(IN1,HIGH);
|
z=fan;
|
||||||
digitalWrite(IN2,LOW);
|
Serial.println("back propelling");
|
||||||
analogWrite(PWM,fan);
|
Serial.println(z);
|
||||||
z=fan;
|
}
|
||||||
Serial.println("back propelling");
|
|
||||||
Serial.println(z);
|
|
||||||
}
|
}
|
||||||
}
|
else if(propeller<=550||propeller>=449){
|
||||||
else if(propeller<=550||propeller>=449)
|
z=0;
|
||||||
{
|
analogWrite(PWM,0);
|
||||||
z=0;
|
Serial.println("stop propelling");
|
||||||
analogWrite(PWM,0);
|
Serial.println(z);
|
||||||
Serial.println("stop propelling");
|
|
||||||
Serial.println(z);
|
|
||||||
}
|
}
|
||||||
Serial.println("inputing ruddering");
|
Serial.println("inputing ruddering");
|
||||||
if(rudder>550||rudder<449)
|
if(rudder>550||rudder<449)
|
||||||
{
|
{
|
||||||
if(rudder<449)
|
if(rudder<449){
|
||||||
{
|
long fan1=0.1*rudder;
|
||||||
long fan1=0.1*rudder;
|
MyServo.write(fan1);
|
||||||
MyServo.write(fan1);
|
Serial.println("turn left");
|
||||||
Serial.println("turn left");
|
dj=2*fan1-90;
|
||||||
dj=2*fan1-90;
|
Serial.println(dj);
|
||||||
Serial.println(dj);
|
|
||||||
}
|
}
|
||||||
else if(rudder>550)
|
else if(rudder>550){
|
||||||
{
|
long zheng1=0.1*rudder-10.122;
|
||||||
long zheng1=0.1*rudder-10.122;
|
MyServo.write(zheng1);
|
||||||
MyServo.write(zheng1);
|
dj=2*zheng1-90;
|
||||||
dj=2*zheng1-90;
|
Serial.println("turn right");
|
||||||
Serial.println("turn right");
|
Serial.println(dj);
|
||||||
Serial.println(dj);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if(rudder<=550||rudder>=449)
|
else if(rudder<=550||rudder>=449){
|
||||||
{
|
dj=0;
|
||||||
dj=0;
|
MyServo.write(45);
|
||||||
MyServo.write(45);
|
Serial.println("stop turing");
|
||||||
Serial.println("stop turing");
|
Serial.println(dj);
|
||||||
Serial.println(dj);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// radio.stopListening();
|
radio.stopListening();
|
||||||
//radio.write(&dist,sizeof(dist));
|
|
||||||
// radio.write(&dj,sizeof(dj));
|
int distance=0;
|
||||||
//radio.write(&z,sizeof(z));
|
if(status==0){
|
||||||
//radio.startListening();
|
if(x<180){
|
||||||
//delay(20);
|
x+=2;
|
||||||
/* if (radio.available()) { //判断是否有要接收的数据
|
}
|
||||||
radio.read(response,sizeof(response));
|
else{
|
||||||
Serial.print(text);
|
status=2;
|
||||||
Serial.println(response);
|
}
|
||||||
}*/
|
RadarServo.write(x); //调整舵机角度
|
||||||
//delay(20); //延迟等待0.3秒
|
//测距
|
||||||
//text++;
|
distance = calculateDistance();
|
||||||
|
//send distance
|
||||||
|
}
|
||||||
|
else if(status==1){
|
||||||
|
if(x>0){
|
||||||
|
x-=2;
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
status=2;
|
||||||
|
}
|
||||||
|
RadarServo.write(x); //调整舵机角度
|
||||||
|
//测距
|
||||||
|
distance = calculateDistance();
|
||||||
|
//send distance
|
||||||
|
}
|
||||||
|
else if(status=2){
|
||||||
|
clrtime--;
|
||||||
|
if(clrtime<=0){
|
||||||
|
clrtime=4;
|
||||||
|
if(x>=180){
|
||||||
|
status=1;
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
status=0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//send clear
|
||||||
|
}
|
||||||
|
delay(20-distance/17);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue