Compare commits
10 Commits
805f8de61c
...
20faf372b5
Author | SHA1 | Date |
---|---|---|
clf3 | 20faf372b5 | |
clf3 | 209f13c5b4 | |
clf3 | b5f240f1b2 | |
clf3 | d901d7a080 | |
clf3 | 46182acec1 | |
clf3 | 8262e30455 | |
clf3 | bc33d84551 | |
clf3 | af269dd20a | |
clf3 | 4a666d121d | |
clf3 | 42b038308d |
269
radar/radar.ino
269
radar/radar.ino
|
@ -1,16 +1,6 @@
|
||||||
//淘宝『创元素店』https://shop423015102.taobao.com/
|
|
||||||
//更新日期 2021/03/06
|
|
||||||
//MiniRadar 超声波雷达 程序
|
|
||||||
//本程序对应商品 https://item.taobao.com/item.htm?spm=a1z10.1-c.w4004-23815833841.8.4f231fe7qvLFZi&id=649834806872
|
|
||||||
|
|
||||||
//Github版链接: https://github.com/johnsonwust/MiniRadar
|
|
||||||
|
|
||||||
#include <Servo.h>
|
#include <Servo.h>
|
||||||
#include <SPI.h>
|
#include <SPI.h>
|
||||||
#include "Ucglib.h"
|
#include "Ucglib.h"
|
||||||
//显示屏的lib 如果没有该lib请按Ctrl+Shift+I 从 库管理器中搜索 ucglib,并安装
|
|
||||||
|
|
||||||
|
|
||||||
#define trigPin 6 //超声波模块的Trig口 6#
|
#define trigPin 6 //超声波模块的Trig口 6#
|
||||||
#define echoPin 5 //超声波模块的echo口 5#
|
#define echoPin 5 //超声波模块的echo口 5#
|
||||||
#define ServoPin 3 //底座舵机端口 3#
|
#define ServoPin 3 //底座舵机端口 3#
|
||||||
|
@ -19,109 +9,65 @@ int Xmax = 160; //屏幕的横向像素数
|
||||||
int Xcent = Xmax / 2; //x中位
|
int Xcent = Xmax / 2; //x中位
|
||||||
int base = 118; //基线高度
|
int base = 118; //基线高度
|
||||||
int scanline = 105; //雷达扫描线长度
|
int scanline = 105; //雷达扫描线长度
|
||||||
int outcircle=81;
|
int outcircle=80;
|
||||||
int midcircle=54;
|
int midcircle=60;
|
||||||
int incircle=27;
|
int midcircle2=40;
|
||||||
|
int incircle=20;
|
||||||
|
unsigned long time=0;
|
||||||
Servo baseServo;
|
Servo baseServo;
|
||||||
Ucglib_ST7735_18x128x160_HWSPI ucg(/*cd=*/ 9, /*cs=*/ 10, /*reset=*/ 8);
|
Ucglib_ST7735_18x128x160_HWSPI ucg(/*cd=*/ 9, /*cs=*/ 10, /*reset=*/ 8);
|
||||||
|
|
||||||
void setup(void)
|
void setup(void)
|
||||||
{
|
{
|
||||||
|
|
||||||
ucg.begin(UCG_FONT_MODE_SOLID); //初始化屏幕
|
ucg.begin(UCG_FONT_MODE_SOLID); //初始化屏幕
|
||||||
ucg.setRotate90(); //设置成横屏 如果屏幕显示方向是反的,可以修改函数 setRotate90 或 setRotate270
|
ucg.setRotate90(); //设置成横屏 如果屏幕显示方向是反的,可以修改函数 setRotate90 或 setRotate270
|
||||||
|
|
||||||
pinMode(trigPin, OUTPUT); //设置trigPin端口模式
|
pinMode(trigPin, OUTPUT); //设置trigPin端口模式
|
||||||
pinMode(echoPin, INPUT); //设置echoPin端口模式
|
pinMode(echoPin, INPUT); //设置echoPin端口模式
|
||||||
Serial.begin(115200); //设置串口传输率
|
Serial.begin(9600); //设置串口传输率
|
||||||
baseServo.attach(ServoPin); //初始化舵机
|
baseServo.attach(ServoPin); //初始化舵机
|
||||||
|
baseServo.write(180);
|
||||||
//欢迎屏幕
|
ucg.clearScreen();
|
||||||
ucg.setFontMode(UCG_FONT_MODE_TRANSPARENT);
|
//cls();
|
||||||
ucg.setColor(0, 0, 100, 0);
|
|
||||||
ucg.setColor(1, 0, 100, 0);
|
|
||||||
ucg.setColor(2, 20, 20,20);
|
|
||||||
ucg.setColor(3, 20, 20, 20);
|
|
||||||
ucg.drawGradientBox(0, 0, 160, 128);
|
|
||||||
ucg.setPrintDir(0);
|
|
||||||
ucg.setColor(0, 5, 0);
|
|
||||||
ucg.setPrintPos(27,42);
|
|
||||||
ucg.setFont(ucg_font_logisoso18_tf);
|
|
||||||
ucg.print("Mini Radar");
|
|
||||||
ucg.setColor(0, 255, 0);
|
|
||||||
ucg.setPrintPos(25,40);
|
|
||||||
ucg.print("Mini Radar");
|
|
||||||
ucg.setFont(ucg_font_helvB08_tf);
|
|
||||||
ucg.setColor(20, 255, 20);
|
|
||||||
ucg.setPrintPos(40,100);
|
|
||||||
ucg.print("Testing...");
|
|
||||||
baseServo.write(90);
|
|
||||||
|
|
||||||
//测试底座的运行情况,注意检测底座位置和转动姿态,是否有卡住(或者导线缠绕)的情况。
|
|
||||||
for(int x=0;x<180;x+=5)
|
|
||||||
{ baseServo.write(x);
|
|
||||||
delay(50);
|
|
||||||
}
|
|
||||||
ucg.print("OK!");
|
|
||||||
delay(500);
|
|
||||||
|
|
||||||
|
|
||||||
//清屏
|
|
||||||
//ucg.clearScreen();
|
|
||||||
cls();
|
|
||||||
ucg.setFontMode(UCG_FONT_MODE_SOLID);
|
ucg.setFontMode(UCG_FONT_MODE_SOLID);
|
||||||
ucg.setFont(ucg_font_orgv01_hr);
|
ucg.setFont(ucg_font_orgv01_hr);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void cls()
|
void cls()
|
||||||
{
|
{
|
||||||
//清屏
|
//清屏
|
||||||
ucg.setColor(0, 0, 0, 0);
|
ucg.setColor(0, 0, 0, 0);
|
||||||
|
|
||||||
for(int s=0;s<128;s+=8)
|
for(int s=0;s<128;s+=8)
|
||||||
for(int t=0;t<160;t+=16)
|
for(int t=0;t<160;t+=16)
|
||||||
{
|
{
|
||||||
ucg.drawBox(t,s,16,8);
|
ucg.drawBox(t,s,16,8);
|
||||||
// delay(1);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int calculateDistance()
|
int calculateDistance()
|
||||||
{
|
{
|
||||||
long duration;
|
long duration;
|
||||||
//trigPin断电 并 等待2微妙
|
|
||||||
digitalWrite(trigPin, LOW);
|
digitalWrite(trigPin, LOW);
|
||||||
delayMicroseconds(2);
|
delayMicroseconds(2);
|
||||||
//trigPin加电 延时 10微妙 再断电
|
|
||||||
digitalWrite(trigPin, HIGH);
|
digitalWrite(trigPin, HIGH);
|
||||||
delayMicroseconds(10);
|
delayMicroseconds(10);
|
||||||
digitalWrite(trigPin, LOW);
|
digitalWrite(trigPin, LOW);
|
||||||
//读取echoPin返回声波的传播时间(微妙)
|
|
||||||
duration = pulseIn(echoPin, HIGH);
|
duration = pulseIn(echoPin, HIGH);
|
||||||
//将回声时间转换成距离数值
|
|
||||||
return duration*0.034/2;
|
return duration*0.034/2;
|
||||||
}
|
}
|
||||||
|
|
||||||
void fix_font()
|
void fix_font()
|
||||||
{
|
{
|
||||||
ucg.setColor(0, 180, 0);
|
ucg.setColor(0, 180, 0);
|
||||||
ucg.setPrintPos(70,14);
|
ucg.setPrintPos(70,46);
|
||||||
ucg.print("1.00");
|
ucg.print("1.00");
|
||||||
ucg.setPrintPos(70,52);
|
ucg.setPrintPos(70,66);
|
||||||
ucg.print("0.67");
|
ucg.print("0.75");
|
||||||
ucg.setPrintPos(70,90);
|
ucg.setPrintPos(70,86);
|
||||||
ucg.print("0.33");
|
ucg.print("0.50");
|
||||||
|
ucg.setPrintPos(70,106);
|
||||||
|
ucg.print("0.25");
|
||||||
}
|
}
|
||||||
|
|
||||||
void fix()
|
void fix()
|
||||||
{
|
{
|
||||||
|
|
||||||
ucg.setColor(0, 40, 0);
|
ucg.setColor(0, 40, 0);
|
||||||
//画基线圆盘
|
//画基线圆盘
|
||||||
ucg.drawDisc(Xcent, base+1, 3, UCG_DRAW_ALL);
|
ucg.drawDisc(Xcent, base+1, 3, UCG_DRAW_ALL);
|
||||||
|
@ -129,86 +75,25 @@ void fix()
|
||||||
ucg.drawCircle(Xcent, base+1, outcircle, UCG_DRAW_UPPER_RIGHT);
|
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_LEFT);
|
||||||
ucg.drawCircle(Xcent, base+1, midcircle, UCG_DRAW_UPPER_RIGHT);
|
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_LEFT);
|
||||||
ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_RIGHT);
|
ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_RIGHT);
|
||||||
ucg.drawLine(0, base+1, Xmax,base+1);
|
ucg.drawLine(0, base+1, Xmax,base+1);
|
||||||
|
|
||||||
ucg.setColor(0, 120, 0);
|
ucg.setColor(0, 120, 0);
|
||||||
//画刻度表
|
|
||||||
// for(int i= 40;i < 140; i+=2)
|
|
||||||
// {
|
|
||||||
|
|
||||||
// if (i % 10 == 0)
|
|
||||||
// ucg.drawLine(105*cos(radians(i))+Xcent,base - 105*sin(radians(i)) , 113*cos(radians(i))+Xcent,base - 113*sin(radians(i)));
|
|
||||||
// else
|
|
||||||
|
|
||||||
// ucg.drawLine(110*cos(radians(i))+Xcent,base - 110*sin(radians(i)) , 113*cos(radians(i))+Xcent,base - 113*sin(radians(i)));
|
|
||||||
// }
|
|
||||||
|
|
||||||
//画一些装饰性图案
|
|
||||||
ucg.setColor(0,200,0);
|
|
||||||
ucg.drawLine(0,0,0,18);
|
|
||||||
for(int i= 0;i < 5; i++)
|
|
||||||
{
|
|
||||||
ucg.setColor(0,random(200)+50,0);
|
|
||||||
ucg.drawBox(2,i*4,random(14)+2,3);
|
|
||||||
}
|
|
||||||
|
|
||||||
ucg.setColor(0,180,0);
|
|
||||||
ucg.drawFrame(146,0,14,14);
|
|
||||||
ucg.setColor(0,60,0);
|
|
||||||
ucg.drawHLine(148,0,10);
|
|
||||||
ucg.drawVLine(146,2,10);
|
|
||||||
ucg.drawHLine(148,13,10);
|
|
||||||
ucg.drawVLine(159,2,10);
|
|
||||||
|
|
||||||
ucg.setColor(0,220,0);
|
|
||||||
ucg.drawBox(148,2,4,4);
|
|
||||||
ucg.drawBox(148,8,4,4);
|
|
||||||
ucg.drawBox(154,8,4,4);
|
|
||||||
ucg.setColor(0,100,0);
|
|
||||||
ucg.drawBox(154,2,4,4);
|
|
||||||
|
|
||||||
ucg.setColor(0,90,0);
|
|
||||||
ucg.drawTetragon(62,123,58,127,98,127,102,123);
|
|
||||||
ucg.setColor(0,160,0);
|
|
||||||
ucg.drawTetragon(67,123,63,127,93,127,97,123);
|
|
||||||
ucg.setColor(0,210,0);
|
|
||||||
ucg.drawTetragon(72,123,68,127,88,127,92,123);
|
|
||||||
|
|
||||||
// ucg.setColor(0,155, 0);
|
|
||||||
// ucg.setPrintPos(0,126);
|
|
||||||
// ucg.print("DEG: ");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void loop(void)
|
void loop(void)
|
||||||
{
|
{
|
||||||
|
|
||||||
int distance;
|
int distance;
|
||||||
|
|
||||||
fix();
|
fix();
|
||||||
fix_font(); //重绘屏幕背景元素
|
fix_font(); //重绘屏幕背景元素
|
||||||
|
Serial.print("clear:");
|
||||||
for (int x=180; x > 4; x-=2){ //底座舵机从180~0度循环
|
Serial.println(millis()-time);
|
||||||
|
time=millis();
|
||||||
|
for (int x=180; x >= 0; x-=2){ //底座舵机从180~0度循环
|
||||||
baseServo.write(x); //调整舵机角度
|
baseServo.write(x); //调整舵机角度
|
||||||
|
|
||||||
//绘制雷达扫描线
|
|
||||||
// int f = x - 4;
|
|
||||||
// ucg.setColor(0, 255, 0);
|
|
||||||
// ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
|
|
||||||
// f+=2;
|
|
||||||
// ucg.setColor(0, 128, 0);
|
|
||||||
// ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
|
|
||||||
// f+=2;
|
|
||||||
// ucg.setColor(0, 0, 0);
|
|
||||||
// ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
|
|
||||||
// ucg.setColor(0,200, 0);
|
|
||||||
//测距
|
//测距
|
||||||
distance = calculateDistance();
|
distance = calculateDistance();
|
||||||
|
|
||||||
//根据测得距离在对应位置画点
|
//根据测得距离在对应位置画点
|
||||||
if (distance < 100)
|
if (distance < 100)
|
||||||
{
|
{
|
||||||
|
@ -220,86 +105,38 @@ void loop(void)
|
||||||
ucg.setColor(255,255,0);
|
ucg.setColor(255,255,0);
|
||||||
ucg.drawDisc((outcircle-1)*cos(radians(x))+Xcent,-(outcircle-1)*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
ucg.drawDisc((outcircle-1)*cos(radians(x))+Xcent,-(outcircle-1)*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
//调试代码,输出角度和测距值
|
|
||||||
Serial.print(x);
|
|
||||||
Serial.print(" , ");
|
|
||||||
Serial.println(distance);
|
|
||||||
|
|
||||||
|
|
||||||
//if (x > 70 and x < 110) fix_font(); //扫描线和数字重合时,重绘数字
|
|
||||||
|
|
||||||
// ucg.setColor(0,155, 0);
|
|
||||||
// ucg.setPrintPos(0,126);
|
|
||||||
// ucg.print("DEG: ");
|
|
||||||
//ucg.setPrintPos(24,126);
|
|
||||||
//ucg.print(x);
|
|
||||||
// ucg.print(" ");
|
|
||||||
//ucg.setPrintPos(125,126);
|
|
||||||
// ucg.print(" ");
|
|
||||||
//ucg.print(distance);
|
|
||||||
// ucg.print("cm ");
|
|
||||||
|
|
||||||
}
|
}
|
||||||
ucg.clearScreen(); //清屏 如果arduino供电不足,可能会引起白屏(显示信号中断)可以用 cls();函数代替 ucg.clearScreen();
|
Serial.print("left:");
|
||||||
delay(50);
|
Serial.println(millis()-time);
|
||||||
//cls(); //如有频繁白屏情况,可以使用该函数 。或者增加外部供电
|
time=millis();
|
||||||
|
ucg.clearScreen();
|
||||||
fix();
|
delay(50);
|
||||||
fix_font(); //重绘屏幕背景元素
|
//cls();
|
||||||
|
fix();
|
||||||
for (int x=1; x < 176; x+=2){
|
fix_font(); //重绘屏幕背景元素
|
||||||
baseServo.write(x); //调整舵机角度
|
Serial.print("clear:");
|
||||||
|
Serial.println(millis()-time);
|
||||||
//绘制雷达扫描线
|
time=millis();
|
||||||
// int f = x + 4;
|
for (int x=0; x <= 180; x+=2){
|
||||||
// ucg.setColor(0, 255, 0);
|
baseServo.write(x); //调整舵机角度
|
||||||
// ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
|
//测距
|
||||||
// f-=2;
|
distance = calculateDistance();
|
||||||
// ucg.setColor(0, 128, 0);
|
//根据测得距离在对应位置画点
|
||||||
// ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
|
if (distance < 100)
|
||||||
// f-=2;
|
{
|
||||||
// ucg.setColor(0, 0, 0);
|
ucg.setColor(255,0,0);
|
||||||
// ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
|
ucg.drawDisc(0.8*distance*cos(radians(x))+Xcent,-0.8*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||||
// ucg.setColor(0, 200, 0);
|
}
|
||||||
//测距
|
else
|
||||||
distance = calculateDistance();
|
{ //超过1米以上的,用黄色画在边缘区域示意
|
||||||
|
ucg.setColor(255,255,0);
|
||||||
//根据测得距离在对应位置画点
|
ucg.drawDisc((outcircle-1)*cos(radians(x))+Xcent,-(outcircle-1)*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
||||||
if (distance < 100)
|
}
|
||||||
{
|
}
|
||||||
ucg.setColor(255,0,0);
|
Serial.print("right:");
|
||||||
ucg.drawDisc(0.8*distance*cos(radians(x))+Xcent,-0.8*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
|
Serial.println(millis()-time);
|
||||||
}
|
time=millis();
|
||||||
else
|
ucg.clearScreen();
|
||||||
{ //超过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(x);
|
|
||||||
Serial.print(" , ");
|
|
||||||
Serial.println(distance);
|
|
||||||
|
|
||||||
//if (x > 70 and x < 110) fix_font(); //扫描线和数字重合时,重绘数字
|
|
||||||
|
|
||||||
// ucg.setColor(0,155, 0);
|
|
||||||
// ucg.setPrintPos(0,126);
|
|
||||||
// ucg.print("DEG: ");
|
|
||||||
// ucg.setPrintPos(24,126);
|
|
||||||
// ucg.print(x);
|
|
||||||
// ucg.print(" ");
|
|
||||||
//ucg.setPrintPos(125,126);
|
|
||||||
// ucg.print(" ");
|
|
||||||
//ucg.print(distance);
|
|
||||||
// ucg.print("cm ");
|
|
||||||
|
|
||||||
}
|
|
||||||
ucg.clearScreen(); //
|
|
||||||
delay(50);
|
delay(50);
|
||||||
//cls();
|
//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);
|
||||||
|
}
|
|
@ -3,23 +3,46 @@
|
||||||
#include<nRF24L01.h>
|
#include<nRF24L01.h>
|
||||||
#include<RF24.h>
|
#include<RF24.h>
|
||||||
#include<TimerOne.h>
|
#include<TimerOne.h>
|
||||||
|
#include<SoftwareSerial.h>
|
||||||
RF24 radio(7,8);//端口可能要改
|
RF24 radio(7,8);//端口可能要改
|
||||||
|
SoftwareSerial sendingSerial(5,6);
|
||||||
|
String sending="";
|
||||||
const int Rp=0,Rr=1,interval=20;//Rp,Rr 分别为两个遥杆的接口
|
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 propeller=0,rudder=0,dist;//propeller为螺旋桨(0-1023,512为静止),rudder为舵(0-1023,512为静止)
|
||||||
long sending_signal=0;//发送的信号
|
long sending_signal=0;//发送的信号
|
||||||
|
long response=0;
|
||||||
const byte addresser[6]={"00001"};//创建通信通道地址,6用来写,8用来读
|
const byte addresser[6]={"00001"};//创建通信通道地址,6用来写,8用来读
|
||||||
|
long time=0;
|
||||||
void setup() {
|
void setup() {
|
||||||
// put your setup code here, to run once:
|
|
||||||
Serial.begin(9600);
|
Serial.begin(9600);
|
||||||
|
sendingSerial.begin(38400);
|
||||||
radio.begin();
|
radio.begin();
|
||||||
//radio.setChannel(114);//设置信道,114号通道
|
//radio.setChannel(115);//设置信道,115号通道
|
||||||
|
radio.setDataRate(RF24_250KBPS);
|
||||||
|
radio.setRetries(3, 5);
|
||||||
radio.openWritingPipe(addresser);
|
radio.openWritingPipe(addresser);
|
||||||
radio.openReadingPipe(1,addresser);
|
radio.openReadingPipe(1,addresser);
|
||||||
radio.setPALevel(RF24_PA_HIGH);
|
radio.setPALevel(RF24_PA_MIN);
|
||||||
Timer1.initialize(interval);
|
//Timer1.initialize(interval);
|
||||||
Timer1.attachInterrupt(send);
|
//Timer1.attachInterrupt(send);
|
||||||
|
radio.startListening();
|
||||||
}
|
}
|
||||||
void send(){
|
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;
|
sending_signal=0;
|
||||||
radio.stopListening();
|
radio.stopListening();
|
||||||
propeller=analogRead(Rp);
|
propeller=analogRead(Rp);
|
||||||
|
@ -29,16 +52,24 @@ void send(){
|
||||||
sending_signal+=propeller;
|
sending_signal+=propeller;
|
||||||
sending_signal+=rudder*1000;
|
sending_signal+=rudder*1000;
|
||||||
sending_signal+=1000000;
|
sending_signal+=1000000;
|
||||||
radio.write(&sending_signal,sizeof(sending_signal));
|
bool b=radio.write(&sending_signal,sizeof(sending_signal));
|
||||||
Serial.println(propeller);
|
Serial.println(b);
|
||||||
Serial.println(rudder);
|
radio.startListening();
|
||||||
Serial.print("propeller sent ");
|
delay(interval);
|
||||||
Serial.println(propeller);
|
//Serial.println("loop");
|
||||||
Serial.print("rudder sent ");
|
if(radio.available()){
|
||||||
Serial.println(rudder);
|
Serial.print("response:");
|
||||||
Serial.println(sending_signal);
|
radio.read(&response,sizeof(response));
|
||||||
}
|
Serial.println(response);
|
||||||
void loop() {
|
if(response%1000000==1){
|
||||||
// put your main code here, to run repeatedly:
|
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;
|
||||||
|
}
|
||||||
|
}
|
215
ship/ship.ino
215
ship/ship.ino
|
@ -4,170 +4,183 @@
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
#include "HardwareSerial.h"
|
#include "HardwareSerial.h"
|
||||||
#include<Servo.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 MyServo;
|
||||||
Servo RidaServo;
|
Servo RadarServo;
|
||||||
RF24 radio(7,8);
|
RF24 radio(7,8);
|
||||||
int count =0;
|
int count =0;
|
||||||
int IN1 =7;
|
int IN1 =9;
|
||||||
int IN2 = 8;
|
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;//发送的信号
|
||||||
const long R_prop=5,R_rudd=6;//输出端口
|
long propeller,rudder,content;//校验序列、读取内容和解码内容
|
||||||
long check_array[7],content,after_decode;//校验序列、读取内容和解码内容
|
long response=0;
|
||||||
const byte addresses[6] = "00001"; //创建一个数组,建立接收机地址,或者说两个模块将用于通信的“管道”
|
const byte addresses[6] = "00001"; //创建一个数组,建立接收机地址,或者说两个模块将用于通信的“管道”
|
||||||
|
int calculateDistance()
|
||||||
long decode_and_confirm(long x){
|
{
|
||||||
long sum=0,add=0,x1=x;
|
long duration;
|
||||||
while(x1!=0){
|
digitalWrite(trigPin, LOW);
|
||||||
check_array[add]=x1%10;
|
delayMicroseconds(2);
|
||||||
x1/=10;
|
digitalWrite(trigPin, HIGH);
|
||||||
add++;
|
delayMicroseconds(10);
|
||||||
}
|
digitalWrite(trigPin, LOW);
|
||||||
for(long i=0,j=4;i<add;j--,i++){
|
duration = pulseIn(echoPin, HIGH);
|
||||||
sum+=check_array[i]*j;
|
return duration*0.034/2;
|
||||||
}
|
|
||||||
if(sum%10==check_array[4])
|
|
||||||
return -1;//代表校验失败
|
|
||||||
return x%10000;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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_MIN); //设置功率放大器级别,将其设置为最小值
|
||||||
//radio.setChannel(0);
|
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
MyServo.write(45);
|
||||||
|
}
|
||||||
|
long confirm(long x){
|
||||||
|
if(x>=1000000&&x<=2000000)return x%1000000;
|
||||||
|
else return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop() {
|
void loop() {
|
||||||
//Serial.println(sizeof(long));
|
|
||||||
analogWrite(PWM,0);
|
|
||||||
radio.startListening();
|
radio.startListening();
|
||||||
//Serial.println("start listening!");
|
delay(20);
|
||||||
|
if(radio.available()){
|
||||||
|
|
||||||
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();
|
|
||||||
// Serial.println("start listening!");
|
|
||||||
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);//调试
|
||||||
after_decode=decode_and_confirm(content);
|
content=confirm(content);
|
||||||
Serial.println(after_decode);
|
if(content!=-1){
|
||||||
if(after_decode!=-1&&content>=100000){
|
propeller=content%1000;
|
||||||
if(content<200000){
|
rudder=(content-propeller)/1000;
|
||||||
Serial.println("inputing propeller ");
|
if(propeller>550||propeller<449){
|
||||||
if(after_decode>600||after_decode<500){
|
if(propeller>550){
|
||||||
if(after_decode>600){
|
long zheng=0.568*propeller-312.4;
|
||||||
long zheng=after_decode;
|
|
||||||
digitalWrite(IN2,HIGH);
|
digitalWrite(IN2,HIGH);
|
||||||
digitalWrite(IN1,LOW);
|
digitalWrite(IN1,LOW);
|
||||||
analogWrite(PWM,zheng);
|
analogWrite(PWM,zheng);
|
||||||
delay(50);
|
z=zheng;
|
||||||
z=zheng-512;
|
|
||||||
Serial.println("forward propelling");
|
Serial.println("forward propelling");
|
||||||
Serial.println(z);
|
Serial.println(z);
|
||||||
}
|
}
|
||||||
else if(after_decode<500){
|
else if(propeller<449){
|
||||||
long fan=1100-after_decode;
|
long fan=255-0.568*propeller;
|
||||||
digitalWrite(IN1,HIGH);
|
digitalWrite(IN1,HIGH);
|
||||||
digitalWrite(IN2,LOW);
|
digitalWrite(IN2,LOW);
|
||||||
analogWrite(PWM,fan);
|
analogWrite(PWM,fan);
|
||||||
delay(50);
|
z=fan;
|
||||||
z=fan-512;
|
|
||||||
Serial.println("back propelling");
|
Serial.println("back propelling");
|
||||||
Serial.println(z);
|
Serial.println(z);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if(after_decode<=600||after_decode>=500){
|
else if(propeller<=550||propeller>=449){
|
||||||
z=0;
|
z=0;
|
||||||
|
analogWrite(PWM,0);
|
||||||
Serial.println("stop propelling");
|
Serial.println("stop propelling");
|
||||||
Serial.println(z);
|
Serial.println(z);
|
||||||
}
|
}
|
||||||
}
|
|
||||||
else{
|
|
||||||
Serial.println("inputing ruddering");
|
Serial.println("inputing ruddering");
|
||||||
if(after_decode>600||after_decode<500){
|
if(rudder>550||rudder<449)
|
||||||
if(after_decode<500){
|
{
|
||||||
long fan1=0.18*after_decode;
|
if(rudder<449){
|
||||||
MyServo.write(fan1/2);
|
long fan1=0.1*rudder;
|
||||||
delay(150);
|
MyServo.write(fan1);
|
||||||
dj=fan1-90;
|
|
||||||
Serial.println("turn left");
|
Serial.println("turn left");
|
||||||
|
dj=2*fan1-90;
|
||||||
Serial.println(dj);
|
Serial.println(dj);
|
||||||
}
|
}
|
||||||
else if(after_decode>600){
|
else if(rudder>550){
|
||||||
long zheng1=0.22*after_decode-42;
|
long zheng1=0.1*rudder-10.122;
|
||||||
MyServo.write(zheng1/2);
|
MyServo.write(zheng1);
|
||||||
delay(150);
|
dj=2*zheng1-90;
|
||||||
dj=zheng1-90;
|
|
||||||
Serial.println("turn right");
|
Serial.println("turn right");
|
||||||
Serial.println(dj);
|
Serial.println(dj);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if(after_decode<=600||after_decode>=500){
|
else if(rudder<=550||rudder>=449){
|
||||||
dj=0;
|
dj=0;
|
||||||
|
MyServo.write(45);
|
||||||
Serial.println("stop turing");
|
Serial.println("stop turing");
|
||||||
Serial.println(dj);
|
Serial.println(dj);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
radio.stopListening();
|
||||||
|
|
||||||
|
int distance=0;
|
||||||
|
if(status==0){
|
||||||
|
if(x<180){
|
||||||
|
x+=2;
|
||||||
}
|
}
|
||||||
// radio.stopListening();
|
else{
|
||||||
//radio.write(&dist,sizeof(dist));
|
status=2;
|
||||||
// radio.write(&dj,sizeof(dj));
|
}
|
||||||
//radio.write(&z,sizeof(z));
|
RadarServo.write(x); //调整舵机角度
|
||||||
//radio.startListening();
|
//测距
|
||||||
//delay(20);
|
distance = calculateDistance();
|
||||||
/* if (radio.available()) { //判断是否有要接收的数据
|
//send distance
|
||||||
radio.read(response,sizeof(response));
|
response=1000000+x*1000+distance;
|
||||||
Serial.print(text);
|
}
|
||||||
Serial.println(response);
|
else if(status==1){
|
||||||
}*/
|
if(x>0){
|
||||||
//delay(20); //延迟等待0.3秒
|
x-=2;
|
||||||
//text++;
|
}
|
||||||
|
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