Skip to content

Commit 17e9476

Browse files
authored
Add files via upload
1 parent 838768d commit 17e9476

File tree

2 files changed

+209
-0
lines changed

2 files changed

+209
-0
lines changed

go.pde

+145
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,145 @@
1+
import processing.serial.*;
2+
3+
Serial port; //последовательный порт
4+
String received; //строка для получаемых данных
5+
int x,dist,rd; //положение сервопривода, расстояние
6+
float xx,yy;
7+
boolean power=false; //состояние радара
8+
int w = 300; //расстояние датчика (150*2)
9+
int radius = 400; //центр окружностей, для построения радара
10+
int[] cords = new int[20];//массив для расстояний
11+
PFont myFont; //стиль текста
12+
13+
void setup()
14+
{
15+
background(1); //создание окна
16+
size(800, 450);
17+
myFont = createFont("verdana", 12); //создание стиля для текста
18+
textFont(myFont);
19+
String port1 = port.list()[0]; //настраиваем последовательный порт
20+
port = new Serial(this,port1, 9600); //для связи с устройством
21+
port.bufferUntil('\n'); //вместо port1 можно вписать необходимый
22+
}
23+
24+
//процедура обработки нажатия клавиш
25+
void keyPressed()
26+
{
27+
if (key == 'f') { //запуск радара
28+
power=true;
29+
port.write('1');
30+
println("start");
31+
}
32+
if (key == 'g') { //остановка радара
33+
power=false;
34+
port.write('2');
35+
println("stop");
36+
}
37+
if (key == 'h') { //начальное положение
38+
power=false;
39+
port.write('3');
40+
clear(); //отчистка экрана
41+
x=0; dist=0; //отчистка данных
42+
for (int i=0; i<18; i++) {
43+
cords[i]=0;}
44+
println("S_position");
45+
}
46+
}
47+
48+
//процедура вызываемая, как бесконечный цикл
49+
void draw()
50+
{
51+
osnova(); //отрисовываем основные элементы (радар,тексты)
52+
poisk(); //отрисовка стрелки радара и припятствий
53+
54+
if ( port.available() > 0) { // если есть данные,
55+
// считываем их и записываем в переменную received
56+
received = port.readStringUntil('\n');
57+
received = trim(received);
58+
if (received != null) { //если что-то считалось
59+
int[] values = int(split(received, ' ')); //разбиваем строку
60+
dist = values[0]; //записываем в соответствующие переменные
61+
x = values[1];
62+
cords[x/10] = dist;
63+
}
64+
//обратная связь
65+
//отсылаем 1, когда закончили отрисовку и считывание
66+
//и при условии работы устройства
67+
if (power==true) {port.write('1');}
68+
if (power==false) {port.write('2');}
69+
}
70+
}
71+
72+
//процедура отрисовки стрелки радара и припятствий
73+
void poisk() {
74+
//стрелка
75+
strokeWeight(1);
76+
stroke(0, 180, 0);
77+
line(radius, radius, radius + cos(radians(x+(180.5)))*w, radius + sin(radians(x+(180.5)))*w);
78+
strokeWeight(2);
79+
stroke(0, 200, 0);
80+
line(radius, radius, radius + cos(radians(x+(180)))*w, radius + sin(radians(x+(180)))*w);
81+
strokeWeight(1);
82+
stroke(0, 180, 0);
83+
line(radius, radius, radius + cos(radians(x+(179.5)))*w, radius + sin(radians(x+(179.5)))*w);
84+
//припятствия
85+
for (int i=0; i<18; i++) {
86+
if (cords[i]>0) {
87+
xx=radius + (cos(radians(i*10+(180)))*(cords[i]*2));
88+
yy=radius + (sin(radians(i*10+(180)))*(cords[i]*2));
89+
strokeWeight(8);
90+
stroke(200, 0, 0);
91+
line(xx,yy,xx,yy);
92+
}
93+
}
94+
}
95+
96+
//процедура отрисовки радара и текстов
97+
void osnova() {
98+
ellipse(350, 420, 1400, 1400); //область для заданного стиля
99+
rectMode(CENTER);
100+
//полуокружности
101+
for (int i = 0; i <=6; i++){
102+
noFill();
103+
strokeWeight(1);
104+
stroke(0, 150, 0);
105+
ellipse(400, 400, (100*i), (100*i));
106+
//подписи расстояния
107+
fill(0, 100, 0);
108+
noStroke();
109+
text(Integer.toString(rd), 430, (405-rd*2), 50, 50);
110+
rd+=25;
111+
}
112+
rd = 0;
113+
//угол поворота
114+
for (int i = 0; i <= 6; i++) {
115+
strokeWeight(1);
116+
stroke(0, 90, 0);
117+
line(400, 400, radius + cos(radians(180+(30*i)))*w, radius + sin(radians(180+(30*i)))*w);
118+
fill(0, 100, 0);
119+
noStroke();
120+
//подписи углов
121+
if (0+(30*i) >= 120) {
122+
text(Integer.toString(0+(30*i)), (radius+10) + cos(radians(180+(30*i)))*(w+10), (radius+10) + sin(radians(180+(30*i)))*(w+10), 25,50);
123+
}
124+
else {
125+
text(Integer.toString(0+(30*i)), radius + cos(radians(180+(30*i)))*w, radius + sin(radians(180+(30*i)))*w, 60,40);
126+
}}
127+
//Тексты
128+
fill(0);
129+
rect(350,440,800,78);//необходим для закраски лишних элементов
130+
fill(0, 60, 0);
131+
text("designed by 521725 group P&D", 10,20);
132+
fill(0, 100, 0);
133+
text("Угол/Градус: "+Integer.toString(x), 220,35);
134+
text("Дистанция: "+Integer.toString(dist), 220,55);
135+
text("RADAR", 380, 20);
136+
text("Нажмите на клавитуре:", 600, 20);
137+
text("f (а) - запуск", 600, 40);
138+
text("g (п) - остановка", 600, 60);
139+
text("h (р) - начальная позиция", 600, 80);
140+
fill(0, 100, 0);
141+
rect(400,40,100,30,10);
142+
fill(0);
143+
if (power==false) {text("OFF", 388, 45);}
144+
else {text("ON", 388, 45);}
145+
}

radar.ino

+64
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
//Подключение библиотек
2+
#include <NewPing.h>
3+
#include <Servo.h>
4+
// Настройка пинов
5+
#define LED 13 //светодиод
6+
#define TRIGGER_PIN 12 //ультразвуковой датчик
7+
#define ECHO_PIN 11
8+
#define SERV_PIN 9 //сервопривод
9+
#define MAX_DISTANCE 150 //дистанция пинга
10+
Servo srv; //создание объект сервопривода
11+
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); //создаем объект УЗ датчик
12+
char com; //вспомогательные переменные
13+
int x,x1;
14+
boolean flag1=true;
15+
16+
void setup() {
17+
Serial.begin(9600); //настраиваем последовательный порт
18+
srv.attach(SERV_PIN); //подключаем сервопривод и устонавливаем его
19+
srv.write(180); //в наччальное положение
20+
x=180;
21+
pinMode(LED, OUTPUT); //подключаем светодиод
22+
pinMode(2, OUTPUT);
23+
digitalWrite(2,LOW);
24+
}
25+
26+
void loop() {
27+
if (Serial.available()) { //если есть данные счтивыаем их и записываем
28+
com = Serial.read();}
29+
if (com=='1') { //поступила 1 начинаем работу радара
30+
while (com=='1') { //цикл, если не 1 радар остановится
31+
if(x<=170 && flag1==false) { //условие для прохода от 0 до 180 градусов
32+
x=x+10;
33+
srv.write(x);
34+
digitalWrite(LED,HIGH); //мигаем светодиодом
35+
delay(50);
36+
digitalWrite(LED,LOW);
37+
Serial.print(sonar.ping_cm()); //отправляем значения расстояния
38+
Serial.print(' ');
39+
Serial.println(180-x); //и угла поворота сервопривода
40+
com = Serial.read();
41+
delay(100);
42+
if (x==180) {flag1=true;} //установливаем флаг о прохождении от 0 до 180
43+
}
44+
if(x>0 && flag1==true) { //условие для прохода от 180 до 0 градусов
45+
x=x-10; //логика такая же как и в условии выше
46+
srv.write(x);
47+
digitalWrite(LED,HIGH);
48+
delay(50);
49+
digitalWrite(LED,LOW);
50+
Serial.print(sonar.ping_cm());
51+
Serial.print(' ');
52+
Serial.println(180-x);
53+
com = Serial.read();
54+
delay(100);
55+
if (x==0) {flag1=false;} //установливаем флаг о прохождении от 180 до 0
56+
}
57+
}
58+
}
59+
if (com=='3') { //условие для установки сервопривода в начальное положение
60+
srv.write(180);
61+
delay (100);
62+
x=180;
63+
flag1=true;}
64+
}

0 commit comments

Comments
 (0)