first commit
This commit is contained in:
commit
6bc29687da
|
|
@ -0,0 +1,142 @@
|
|||
#include <Servo.h>
|
||||
#include <SPI.h>
|
||||
#include "Ucglib.h"
|
||||
#define trigPin 6 //超声波模块的Trig口 6#
|
||||
#define echoPin 5 //超声波模块的echo口 5#
|
||||
#define ServoPin 3 //底座舵机端口 3#
|
||||
int Ymax = 128; //屏幕的竖向像素数
|
||||
int Xmax = 160; //屏幕的横向像素数
|
||||
int Xcent = Xmax / 2; //x中位
|
||||
int base = 118; //基线高度
|
||||
int scanline = 105; //雷达扫描线长度
|
||||
int outcircle=80;
|
||||
int midcircle=60;
|
||||
int midcircle2=40;
|
||||
int incircle=20;
|
||||
unsigned long time=0;
|
||||
Servo baseServo;
|
||||
Ucglib_ST7735_18x128x160_HWSPI ucg(/*cd=*/ 9, /*cs=*/ 10, /*reset=*/ 8);
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
ucg.begin(UCG_FONT_MODE_SOLID); //初始化屏幕
|
||||
ucg.setRotate90(); //设置成横屏 如果屏幕显示方向是反的，可以修改函数 setRotate90 或 setRotate270
|
||||
pinMode(trigPin, OUTPUT); //设置trigPin端口模式
|
||||
pinMode(echoPin, INPUT); //设置echoPin端口模式
|
||||
Serial.begin(9600); //设置串口传输率
|
||||
baseServo.attach(ServoPin); //初始化舵机
|
||||
baseServo.write(180);
|
||||
ucg.clearScreen();
|
||||
//cls();
|
||||
ucg.setFontMode(UCG_FONT_MODE_SOLID);
|
||||
ucg.setFont(ucg_font_orgv01_hr);
|
||||
}
|
||||
|
||||
void cls()
|
||||
{
|
||||
//清屏
|
||||
ucg.setColor(0, 0, 0, 0);
|
||||
for(int s=0;s<128;s+=8)
|
||||
for(int t=0;t<160;t+=16)
|
||||
{
|
||||
ucg.drawBox(t,s,16,8);
|
||||
}
|
||||
}
|
||||
|
||||
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 fix_font()
|
||||
{
|
||||
ucg.setColor(0, 180, 0);
|
||||
ucg.setPrintPos(70,46);
|
||||
ucg.print("1.00");
|
||||
ucg.setPrintPos(70,66);
|
||||
ucg.print("0.75");
|
||||
ucg.setPrintPos(70,86);
|
||||
ucg.print("0.50");
|
||||
ucg.setPrintPos(70,106);
|
||||
ucg.print("0.25");
|
||||
}
|
||||
void fix()
|
||||
{
|
||||
ucg.setColor(0, 40, 0);
|
||||
//画基线圆盘
|
||||
ucg.drawDisc(Xcent, base+1, 3, UCG_DRAW_ALL);
|
||||
ucg.drawCircle(Xcent, base+1, outcircle, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, outcircle, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle2, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle2, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawLine(0, base+1, Xmax,base+1);
|
||||
ucg.setColor(0, 120, 0);
|
||||
}
|
||||
void loop(void)
|
||||
{
|
||||
int distance;
|
||||
fix();
|
||||
fix_font(); //重绘屏幕背景元素
|
||||
Serial.print("clear:");
|
||||
Serial.println(millis()-time);
|
||||
time=millis();
|
||||
for (int x=180; x >= 0; x-=2){ //底座舵机从180~0度循环
|
||||
baseServo.write(x); //调整舵机角度
|
||||
//测距
|
||||
distance = calculateDistance();
|
||||
//根据测得距离在对应位置画点
|
||||
if (distance < 100)
|
||||
{
|
||||
ucg.setColor(255,0,0);
|
||||
ucg.drawDisc(0.8*distance*cos(radians(x))+Xcent,-0.8*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||
}
|
||||
else
|
||||
{ //超过1米以上的，用黄色画在边缘区域示意
|
||||
ucg.setColor(255,255,0);
|
||||
ucg.drawDisc((outcircle-1)*cos(radians(x))+Xcent,-(outcircle-1)*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||
}
|
||||
}
|
||||
Serial.print("left:");
|
||||
Serial.println(millis()-time);
|
||||
time=millis();
|
||||
ucg.clearScreen();
|
||||
delay(50);
|
||||
//cls();
|
||||
fix();
|
||||
fix_font(); //重绘屏幕背景元素
|
||||
Serial.print("clear:");
|
||||
Serial.println(millis()-time);
|
||||
time=millis();
|
||||
for (int x=0; x <= 180; x+=2){
|
||||
baseServo.write(x); //调整舵机角度
|
||||
//测距
|
||||
distance = calculateDistance();
|
||||
//根据测得距离在对应位置画点
|
||||
if (distance < 100)
|
||||
{
|
||||
ucg.setColor(255,0,0);
|
||||
ucg.drawDisc(0.8*distance*cos(radians(x))+Xcent,-0.8*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||
}
|
||||
else
|
||||
{ //超过1米以上的，用黄色画在边缘区域示意
|
||||
ucg.setColor(255,255,0);
|
||||
ucg.drawDisc((outcircle-1)*cos(radians(x))+Xcent,-(outcircle-1)*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||
}
|
||||
}
|
||||
Serial.print("right:");
|
||||
Serial.println(millis()-time);
|
||||
time=millis();
|
||||
ucg.clearScreen();
|
||||
delay(50);
|
||||
//cls();
|
||||
}
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
#include <SPI.h>
|
||||
#include "Ucglib.h"
|
||||
#define interruptPin 2
|
||||
|
||||
int Ymax = 128; //屏幕的竖向像素数
|
||||
int Xmax = 160; //屏幕的横向像素数
|
||||
int Xcent = Xmax / 2; //x中位
|
||||
int base = 118; //基线高度
|
||||
int scanline = 105; //雷达扫描线长度
|
||||
int outcircle=80;
|
||||
int midcircle=60;
|
||||
int midcircle2=40;
|
||||
int incircle=20;
|
||||
int x=90;
|
||||
int distance=50;
|
||||
Ucglib_ST7735_18x128x160_HWSPI ucg(/*cd=*/ 9, /*cs=*/ 10, /*reset=*/ 8);
|
||||
void fix_font()
|
||||
{
|
||||
ucg.setColor(0, 180, 0);
|
||||
ucg.setPrintPos(70,46);
|
||||
ucg.print("1.00");
|
||||
ucg.setPrintPos(70,66);
|
||||
ucg.print("0.75");
|
||||
ucg.setPrintPos(70,86);
|
||||
ucg.print("0.50");
|
||||
ucg.setPrintPos(70,106);
|
||||
ucg.print("0.25");
|
||||
}
|
||||
void fix()
|
||||
{
|
||||
ucg.setColor(0, 40, 0);
|
||||
//画基线圆盘
|
||||
ucg.drawDisc(Xcent, base+1, 3, UCG_DRAW_ALL);
|
||||
ucg.drawCircle(Xcent, base+1, outcircle, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, outcircle, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle2, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle2, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawLine(0, base+1, Xmax,base+1);
|
||||
ucg.setColor(0, 120, 0);
|
||||
}
|
||||
void setup() {
|
||||
ucg.begin(UCG_FONT_MODE_SOLID); //初始化屏幕
|
||||
ucg.setRotate90(); //设置成横屏 如果屏幕显示方向是反的，可以修改函数 setRotate90 或 setRotate270
|
||||
Serial.begin(9600); //设置串口传输率
|
||||
ucg.clearScreen();
|
||||
//cls();
|
||||
ucg.setFontMode(UCG_FONT_MODE_SOLID);
|
||||
ucg.setFont(ucg_font_orgv01_hr);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if(true){
|
||||
if(false){
|
||||
ucg.clearScreen();
|
||||
fix();
|
||||
fix_font(); //重绘屏幕背景元素
|
||||
return;
|
||||
}
|
||||
if (distance < 100){
|
||||
ucg.setColor(255,0,0);
|
||||
ucg.drawDisc(0.8*distance*cos(radians(x))+Xcent,-0.8*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||
}
|
||||
else{ //超过1米以上的，用黄色画在边缘区域示意
|
||||
ucg.setColor(255,255,0);
|
||||
ucg.drawDisc((outcircle-1)*cos(radians(x))+Xcent,-(outcircle-1)*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
#include <Servo.h>
|
||||
|
||||
#define trigPin 6 //超声波模块的Trig口 6#
|
||||
#define echoPin 5 //超声波模块的echo口 5#
|
||||
#define ServoPin 3 //底座舵机端口 3#
|
||||
|
||||
unsigned long time=0;
|
||||
Servo baseServo;
|
||||
bool increasing=true;
|
||||
bool clr=false;
|
||||
//0: increase 1: decrease 2: clr
|
||||
int status=0;
|
||||
int clrtime=40;
|
||||
int x=0;
|
||||
void setup(void)
|
||||
{
|
||||
pinMode(trigPin, OUTPUT); //设置trigPin端口模式
|
||||
pinMode(echoPin, INPUT); //设置echoPin端口模式
|
||||
Serial.begin(9600); //设置串口传输率
|
||||
baseServo.attach(ServoPin); //初始化舵机
|
||||
baseServo.write(0);
|
||||
|
||||
}
|
||||
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 loop(void)
|
||||
{
|
||||
int distance=0;
|
||||
if(status==0){
|
||||
if(x<180){
|
||||
x+=2;
|
||||
}
|
||||
else{
|
||||
status=2;
|
||||
}
|
||||
baseServo.write(x); //调整舵机角度
|
||||
//测距
|
||||
distance = calculateDistance();
|
||||
//send distance
|
||||
}
|
||||
else if(status==1){
|
||||
if(x>0){
|
||||
x-=2;
|
||||
}
|
||||
else{
|
||||
status=2;
|
||||
}
|
||||
baseServo.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);
|
||||
}
|
||||
|
|
@ -0,0 +1,75 @@
|
|||
//player1,remote controler
|
||||
#include<SPI.h>
|
||||
#include<nRF24L01.h>
|
||||
#include<RF24.h>
|
||||
#include<TimerOne.h>
|
||||
#include<SoftwareSerial.h>
|
||||
RF24 radio(7,8);//端口可能要改
|
||||
SoftwareSerial sendingSerial(5,6);
|
||||
String sending="";
|
||||
const int Rp=0,Rr=1,interval=20;//Rp,Rr 分别为两个遥杆的接口
|
||||
long propeller=0,rudder=0,dist;//propeller为螺旋桨（0-1023，512为静止），rudder为舵（0-1023，512为静止）
|
||||
long sending_signal=0;//发送的信号
|
||||
long response=0;
|
||||
const byte addresser[6]={"00001"};//创建通信通道地址，6用来写，8用来读
|
||||
long time=0;
|
||||
void setup() {
|
||||
Serial.begin(9600);
|
||||
sendingSerial.begin(38400);
|
||||
radio.begin();
|
||||
//radio.setChannel(115);//设置信道，115号通道
|
||||
radio.setDataRate(RF24_250KBPS);
|
||||
radio.setRetries(3, 5);
|
||||
radio.openWritingPipe(addresser);
|
||||
radio.openReadingPipe(1,addresser);
|
||||
radio.setPALevel(RF24_PA_MIN);
|
||||
//Timer1.initialize(interval);
|
||||
//Timer1.attachInterrupt(send);
|
||||
radio.startListening();
|
||||
}
|
||||
void send(){
|
||||
|
||||
// Serial.println(propeller);
|
||||
// Serial.println(rudder);
|
||||
// Serial.print("propeller sent ");
|
||||
// Serial.println(propeller);
|
||||
// Serial.print("rudder sent ");
|
||||
// Serial.println(rudder);
|
||||
// Serial.println(sending_signal);
|
||||
|
||||
//Serial.println(millis()-time);
|
||||
}
|
||||
void loop() {
|
||||
// put your main code here, to run repeatedly:
|
||||
time=millis();
|
||||
//Serial.println("send");
|
||||
sending_signal=0;
|
||||
radio.stopListening();
|
||||
propeller=analogRead(Rp);
|
||||
rudder=analogRead(Rr);
|
||||
if(propeller>=1000)propeller=999;
|
||||
if(rudder>=1000)rudder=999;
|
||||
sending_signal+=propeller;
|
||||
sending_signal+=rudder*1000;
|
||||
sending_signal+=1000000;
|
||||
bool b=radio.write(&sending_signal,sizeof(sending_signal));
|
||||
Serial.println(b);
|
||||
radio.startListening();
|
||||
delay(interval);
|
||||
//Serial.println("loop");
|
||||
if(radio.available()){
|
||||
Serial.print("response:");
|
||||
radio.read(&response,sizeof(response));
|
||||
Serial.println(response);
|
||||
if(response%1000000==1){
|
||||
sending=String(response);
|
||||
sendingSerial.print(sending);
|
||||
sendingSerial.print('\n');
|
||||
}
|
||||
else if(response==2000000){
|
||||
sending=String(response);
|
||||
sendingSerial.print(sending);
|
||||
sendingSerial.print('\n');
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
#include <SPI.h>
|
||||
#include<SoftwareSerial.h>
|
||||
#include "Ucglib.h"
|
||||
#define interruptPin 2
|
||||
SoftwareSerial sendingSerial(6,7);
|
||||
int Ymax = 128; //屏幕的竖向像素数
|
||||
int Xmax = 160; //屏幕的横向像素数
|
||||
int Xcent = Xmax / 2; //x中位
|
||||
int base = 118; //基线高度
|
||||
int scanline = 105; //雷达扫描线长度
|
||||
int outcircle=80;
|
||||
int midcircle=60;
|
||||
int midcircle2=40;
|
||||
int incircle=20;
|
||||
int x=90;
|
||||
int distance=50;
|
||||
bool rec=false;
|
||||
bool clr=false;
|
||||
bool ignoreClr=false;
|
||||
String received="";//用来接受字符
|
||||
Ucglib_ST7735_18x128x160_HWSPI ucg(/*cd=*/ 9, /*cs=*/ 10, /*reset=*/ 8);
|
||||
void fix_font()
|
||||
{
|
||||
ucg.setColor(0, 180, 0);
|
||||
ucg.setPrintPos(70,46);
|
||||
ucg.print("1.00");
|
||||
ucg.setPrintPos(70,66);
|
||||
ucg.print("0.75");
|
||||
ucg.setPrintPos(70,86);
|
||||
ucg.print("0.50");
|
||||
ucg.setPrintPos(70,106);
|
||||
ucg.print("0.25");
|
||||
}
|
||||
void fix()
|
||||
{
|
||||
ucg.setColor(0, 40, 0);
|
||||
//画基线圆盘
|
||||
ucg.drawDisc(Xcent, base+1, 3, UCG_DRAW_ALL);
|
||||
ucg.drawCircle(Xcent, base+1, outcircle, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, outcircle, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle2, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, midcircle2, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_LEFT);
|
||||
ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_RIGHT);
|
||||
ucg.drawLine(0, base+1, Xmax,base+1);
|
||||
ucg.setColor(0, 120, 0);
|
||||
}
|
||||
void setup() {
|
||||
ucg.begin(UCG_FONT_MODE_SOLID); //初始化屏幕
|
||||
ucg.setRotate90(); //设置成横屏 如果屏幕显示方向是反的，可以修改函数 setRotate90 或 setRotate270
|
||||
Serial.begin(9600); //设置串口传输率
|
||||
sendingSerial.begin(38400);
|
||||
sendingSerial.listen();//必须要加
|
||||
ucg.clearScreen();
|
||||
//cls();
|
||||
ucg.setFontMode(UCG_FONT_MODE_SOLID);
|
||||
ucg.setFont(ucg_font_orgv01_hr);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
while(sendingSerial.available()){
|
||||
if(sendingSerial.peek()!='\n'){//peek()查看当前字节是不是换行
|
||||
received+=(char)sendingSerial.read();
|
||||
}
|
||||
else{
|
||||
rec=true;
|
||||
sendingSerial.read();//把换行符读掉
|
||||
if(received[0]=='1'){
|
||||
ignoreClr=false;
|
||||
distance=received.substring(4).toInt();//个十百3位数
|
||||
x=received.substring(1,4).toInt();//第456位 三位数
|
||||
}
|
||||
else if(received[0]=='2'){
|
||||
if(!ignoreClr){
|
||||
clr=true;
|
||||
}
|
||||
}
|
||||
received="";
|
||||
}
|
||||
}
|
||||
if(rec){
|
||||
if(clr){
|
||||
ucg.clearScreen();
|
||||
fix();
|
||||
fix_font(); //重绘屏幕背景元素
|
||||
clr=false;
|
||||
ignoreClr=true;
|
||||
return;
|
||||
}
|
||||
else if(ignoreClr){
|
||||
return;
|
||||
}
|
||||
|
||||
if (distance < 100){
|
||||
ucg.setColor(255,0,0);
|
||||
ucg.drawDisc(0.8*distance*cos(radians(x))+Xcent,-0.8*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||
}
|
||||
else{ //超过1米以上的，用黄色画在边缘区域示意
|
||||
ucg.setColor(255,255,0);
|
||||
ucg.drawDisc((outcircle-1)*cos(radians(x))+Xcent,-(outcircle-1)*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||
}
|
||||
rec=false;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,186 @@
|
|||
#include<SPI.h>
|
||||
#include<nRF24L01.h>
|
||||
#include<RF24.h>
|
||||
#include "Arduino.h"
|
||||
#include "HardwareSerial.h"
|
||||
#include<Servo.h>
|
||||
#define trigPin 2 //超声波模块的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 RadarServo;
|
||||
RF24 radio(7,8);
|
||||
int count =0;
|
||||
int IN1 =9;
|
||||
int IN2 =10;
|
||||
int PWM=6;
|
||||
// int TrigPin=2;
|
||||
// int EcoPin=3;
|
||||
// int dAngle=5;
|
||||
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,content;//校验序列、读取内容和解码内容
|
||||
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);
|
||||
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(angle);
|
||||
|
||||
RadarServo.write(0);
|
||||
radio.begin();
|
||||
//radio.setChannel(114);
|
||||
radio.openReadingPipe(1,addresses); //接收通道，
|
||||
radio.openWritingPipe(addresses);//发送通道
|
||||
radio.setPALevel(RF24_PA_MIN); //设置功率放大器级别，将其设置为最小值
|
||||
delay(1000);
|
||||
MyServo.write(45);
|
||||
}
|
||||
long confirm(long x){
|
||||
if(x>=1000000&&x<=2000000)return x%1000000;
|
||||
else return -1;
|
||||
}
|
||||
void loop() {
|
||||
radio.startListening();
|
||||
delay(20);
|
||||
if(radio.available()){
|
||||
radio.read(&content,sizeof(content));
|
||||
Serial.println("The content is");
|
||||
Serial.println(content);//调试
|
||||
content=confirm(content);
|
||||
if(content!=-1){
|
||||
propeller=content%1000;
|
||||
rudder=(content-propeller)/1000;
|
||||
if(propeller>550||propeller<449){
|
||||
if(propeller>550){
|
||||
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<449){
|
||||
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<=550||propeller>=449){
|
||||
z=0;
|
||||
analogWrite(PWM,0);
|
||||
Serial.println("stop propelling");
|
||||
Serial.println(z);
|
||||
}
|
||||
Serial.println("inputing ruddering");
|
||||
if(rudder>550||rudder<449)
|
||||
{
|
||||
if(rudder<449){
|
||||
long fan1=0.1*rudder;
|
||||
MyServo.write(fan1);
|
||||
Serial.println("turn left");
|
||||
dj=2*fan1-90;
|
||||
Serial.println(dj);
|
||||
}
|
||||
else if(rudder>550){
|
||||
long zheng1=0.1*rudder-10.122;
|
||||
MyServo.write(zheng1);
|
||||
dj=2*zheng1-90;
|
||||
Serial.println("turn right");
|
||||
Serial.println(dj);
|
||||
}
|
||||
}
|
||||
else if(rudder<=550||rudder>=449){
|
||||
dj=0;
|
||||
MyServo.write(45);
|
||||
Serial.println("stop turing");
|
||||
Serial.println(dj);
|
||||
}
|
||||
}
|
||||
}
|
||||
radio.stopListening();
|
||||
|
||||
int distance=0;
|
||||
if(status==0){
|
||||
if(x<180){
|
||||
x+=2;
|
||||
}
|
||||
else{
|
||||
status=2;
|
||||
}
|
||||
RadarServo.write(x); //调整舵机角度
|
||||
//测距
|
||||
distance = calculateDistance();
|
||||
//send distance
|
||||
response=1000000+x*1000+distance;
|
||||
}
|
||||
else if(status==1){
|
||||
if(x>0){
|
||||
x-=2;
|
||||
}
|
||||
else{
|
||||
status=2;
|
||||
}
|
||||
RadarServo.write(x); //调整舵机角度
|
||||
//测距
|
||||
distance = calculateDistance();
|
||||
response=1000000+x*1000+distance;
|
||||
//send distance
|
||||
}
|
||||
else if(status=2){
|
||||
clrtime--;
|
||||
if(clrtime<=0){
|
||||
clrtime=4;
|
||||
if(x>=180){
|
||||
status=1;
|
||||
}
|
||||
else{
|
||||
status=0;
|
||||
}
|
||||
}
|
||||
//send clear
|
||||
response=2000000;
|
||||
}
|
||||
radio.write(&response,sizeof(response));
|
||||
//delay(20-distance/17);
|
||||
Serial.println(distance);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue