Для самообучения сделал сканирующий ультразвуковой локатор (УЗ модуль на серве), который отсылает данные по Bluetooth (как в последовательный порт), и эти данные отрисовывает программа на Processing. В результате получается 100-градусный сектор в котором вспыхивают и медленно гаснут очертания предметов, эхо от которых улавливает радар.
Я пытался ловить несколько эх на один импульс, чтобы рисовать препятствия которые одновременно попали в луч, но находящиеся на разном расстоянии. Но локатор, скорее, рисует переотражения пришедшие со сторон 😉
Схему уже разобрал так что фоток нету 😇
Код для загрузки в Arduino:
#include <LiquidCrystal.h>
#include <stdarg.h>
#include <Servo.h>
// LCD connector;
// 1 2 3 4 5 6 7 8 9
// GND U+ A0 E D4 D5 D6 D7 UBL+
// GND 5V D7 D8 D9 D10 D11 D12
// initialize the library with the numbers of the interface pins
//LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
LiquidCrystal lcd(7, 8, 9, 10, 11, 12);
const int minAngle = -50;
const int maxAngle = +50;
const int angleStep = 4;
const int MAX_IN_BEAM = 5;
void lcdPrint(char *fmt, ... ){
char tmp[16]; // resulting string limited to 16 chars
va_list args;
va_start (args, fmt);
vsnprintf(tmp, 128, fmt, args);
va_end (args);
lcd.print(tmp);
}
void serPrint(char *fmt, ... ){
char tmp[16]; // resulting string limited to 16 chars
va_list args;
va_start (args, fmt);
vsnprintf(tmp, 128, fmt, args);
va_end (args);
Serial.print(tmp);
}
// variables to take x number of readings and then average them
// to remove the jitter/noise from the DYP-ME007 sonar readings
const int numReadings = 2;
// setup pins and variables for DYP-ME007 sonar device
int echoPin = 2; // DYP-ME007 echo pin (digital 2)
int initPin = 3; // DYP-ME007 trigger pin (digital 3)
unsigned long lastDistance = 0; // variable for storing the distance (cm)
Servo myservo; // create servo object to control a servo
const int servoPin = 6;
int servo = 0;
int servoDir = 1;
void setup() {
pinMode(4, OUTPUT); // LCD V;
pinMode(initPin, OUTPUT); // set init pin 3 as output
pinMode(echoPin, INPUT); // set echo pin 2 as input
myservo.attach(servoPin); // attaches the servo on pin 6 to the servo object
digitalWrite(4, HIGH); // power the LCD;
// set up the LCD's number of columns and rows:
lcd.begin(16, 2);
tone(5,500);
// Print a message to the LCD.
lcd.print("SoundLocator 1.0");
delay(1000);
lcd.clear();
noTone(5);
Serial.begin(115200);
// pinMode(servoPin, OUTPUT);
myservo.write(0); // tell servo to go to position in variable 'pos'
}
void loop() {
unsigned long pulseTime;
float distance[MAX_IN_BEAM];
unsigned long maxWaitTime = 50000UL;
int found=0;
for (int i=0; i<MAX_IN_BEAM; i++) {
if (i==0) {
digitalWrite(initPin, HIGH); // send 10 microsecond pulse
delayMicroseconds(10); // wait 10 microseconds before turning off
digitalWrite(initPin, LOW); // stop sending the pulse
} else {
digitalWrite(initPin, HIGH); // cheat locator module to force it listen without sending pulse
digitalWrite(initPin, LOW); // stop sending the pulse
}
pulseTime = pulseIn(echoPin, HIGH, maxWaitTime); // Look for a return pulse, it should be high as the pulse goes low-high-low
if (pulseTime > 0) {
distance[found] = float(pulseTime)/58.0 + (found>0 ? distance[found-1] : 0);
found++;
}
maxWaitTime-= pulseTime;
if (maxWaitTime <= 0)
break;
}
serPrint("L%d", servo);
for (int i=0; i<found; i++) {
serPrint(",%d", int(round(distance[i])));
}
serPrint("\r\n");
myservo.write(90-servo); // tell servo to go to position in variable 'pos'
servo+= servoDir;
if (servo >= maxAngle) {
servoDir = -angleStep;
} else
if (servo <= minAngle) {
servoDir = +angleStep;
}
lcd.setCursor(0, 0);
lcdPrint("L:%-1d", found);
lcd.setCursor(6, 1);
lcdPrint("S=%-5d", servo);
lcd.setCursor(12, 1);
lcdPrint("D=%-2d", servoDir);
lcd.setCursor(12, 0);
lcdPrint("1=%-5d", found ? 0 : distance[0]);
delay(100);
}
Код в Processing:
import processing.serial.*; // serial library
PFont font;
boolean init_com = false;
Serial g_serial = null;
static int minAngle = -50;
static int maxAngle = +50;
static int angleStep = 4;
static int MAX_IN_BEAM = 5;
static int START_INTENSITY = 200;
static class Point {
int distance;
int intensity;
Point() {
distance = 0;
intensity = 0;
}
void fade() {
if (intensity > 0) {
intensity--;
if (intensity == 0)
distance = 0;
}
}
void setDistance(int aDistance) {
distance = aDistance;
intensity = START_INTENSITY;
}
}
static class Scan {
Point points[];
Scan (int num) {
points = new Point[num];
for (int i=0; i<num; i++)
points[i] = new Point();
}
}
static int NUM_SCANS = (maxAngle-minAngle)/angleStep+1;
static Scan scans[] = new Scan[NUM_SCANS];
static {
for (int i=0; i<NUM_SCANS; i++)
scans[i] = new Scan(MAX_IN_BEAM);
}
int lastAngle = 0;
void setup() {
size(1024, 768, P3D);
font = createFont("Arial",18);
textFont(font);
g_serial = new Serial(this, "COM19", 115200);
init_com = true;
}
void fadeAll() {
for (int a=minAngle; a<=maxAngle; a+=angleStep) {
int scanNum = (a-minAngle)/angleStep;
Scan scan = scans[scanNum];
for (int l=0; l<scan.points.length; l++) {
scan.points[l].fade();
}
}
}
int R = 500;
int rArr = 50;
int locAng = 15;
void draw() {
if (init_com) {
// if (g_serial.available() >100) g_serial.clear();
while (g_serial.available() >= 1)
processSerialData();
}
background(25,10,10);
noLights();
// perspective();
noStroke();
colorMode(HSB, 100);
pushMatrix();
translate(width/2, height-rArr, 0);
noFill();
ellipseMode(CENTER);
float ab1 = radians(minAngle-90);
float ab2 = radians(maxAngle-90);
for (int d = 20; d<=R; d+=20) {
noFill();
stroke(float(d)/R*100, 50, 45);
arc(0,0,d*2,d*2,ab1,ab2);
fill(float(d)/R*100, 100, 100);
textAlign(RIGHT, CENTER);
text(d, round(d*cos(ab1))-5, round(d*sin(ab1)));
textAlign(LEFT, CENTER);
text(d, round(d*cos(ab2))+5, round(d*sin(ab1)));
}
noFill();
lights();
int prevX = 0;
int prevY = 0;
boolean first = true;
strokeWeight(4);
for (int a = minAngle; a <= maxAngle; a+= angleStep) {
int scanNum = (a-minAngle)/angleStep;
Scan scan = scans[scanNum];
for (int l=0; l<scan.points.length; l++) {
Point point = scan.points[l];
if (point.intensity == 0)
continue;
int d = min(R, point.distance);
float a1 = radians(a-90);
float a2 = radians(a-90+angleStep);
int x1 = round(d*cos(a1));
int y1 = round(d*sin(a1));
int x2 = round(d*cos(a2));
int y2 = round(d*sin(a2));
d = round(float(d)/R*100.0);
stroke(d, 100, point.intensity/(START_INTENSITY/100));
line(x1, y1, x2, y2);
// stroke(d, 50, point.intensity/(START_INTENSITY/100));
// arc(0,0,d*2,d*2, a1, a2);
// println("a="+a+", d"+l+"="+d);
}
}
strokeWeight(1);
popMatrix();
stroke(0, 50, 50);
translate(width/2, height-rArr, 0);
rotateX(-PI/6);
rotateY(-radians(lastAngle));
fill(0,50,50);
pushMatrix();
pushMatrix();
box(50,15,5);
popMatrix();
translate(0, 0, 0);
rotateZ(PI/2);
rotateX(-PI/2);
rotateY(PI/2);
int dx = round(sin(radians(locAng/2))*rArr/2);
triangle(-dx, rArr, +dx, rArr, 0, 7);
popMatrix();
fadeAll();
}
void processSerialData() {
boolean found = false;
while (g_serial.available() > 0) {
if (g_serial.read() == 'L') {
found = true;
break;
}
}
if (!found)
return;
String sIn = g_serial.readString();
if (sIn == null)
return;
String s[] = sIn.split(",");
if (s.length == 0)
return;
int angle = int(s[0]);
int scanNum = (angle-minAngle)/angleStep;
Scan scan = scans[scanNum];
print("angle="+angle);
for (int i=1; i<s.length && (i-1)<scan.points.length; i++) {
String s1 = s[i].trim();
if (s1.length() == 0)
continue;
int distance = int(s1);
scan.points[i-1].setDistance(distance);
print(", d"+i+"="+distance);
}
lastAngle = angle;
if (angle < minAngle || angle > maxAngle) {
println (" ---");
} else {
println ("");
}
}
