Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
A processing and arduino sketch for 3D mapping with an ultrasonic sensor
/*
Arduino program to control a ultrasonic 3-D mapping sensor.
The programs controls an HC-SR04 ultrasonic sensor connected to the Arduino board.
A collaborating Processing sketch interacts with this program with serial communication to control the angle
Authors:Prithvijit Chakrabarty (prithvichakra@gmail.com)
Kartik S. Lovekar (kslovekar@gmail.com)
*/
#include <Servo.h>
#include <NewPing.h>
//Set pin numbers
#define PING_PIN 8
#define ECHO_PIN 9
//Maximum distance(cm) the HC-SR04 can detect accurately
#define MAX_DISTANCE 450
//Time delay to wait for the receiver to register the echo
#define DELAY_LOOP 3
Servo servo1;
Servo servo2;
NewPing sonar(PING_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pin and maximum distance.
int val;
void setup() {
pinMode(1,OUTPUT);
//Attach servos
servo1.attach(14); //analog pin 0
servo2.attach(15); //analog pin 1
Serial.begin(9600);
//Serial.println("Ready");
val = 0;
//Set servos to 0 degreees initially
servo1.write(val);
servo2.write(val);
}
void loop() {
//Protocol to communicate with the Processing sketch
if(Serial.available()){
int c = Serial.read();
//delay(50);
switch(c){
case '0'...'9': val = val*10 + c - '0';
break;
case 'x': servo1.write(val);
scan(1);
c = '\0';
break;
case 'y': servo2.write(val);
scan(2);
c = '\0';
break;
}
//servo1.write(c);
//delay(50);
}
}
//Run one scan with a sonar ping
void scan(int servo_num){
int avg = 0, i, NUM = 3;
//for(i = 0; i < NUM; i++){
unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
/*int dist = sonar.ping();
avg += dist;
//delay(30);
}
unsigned int uS = avg/NUM;*/
//Format to communicate with the Processing sketch <servo_num>-<angle>:<distance>c
//servo_num represents the horizontal or vertical servo
Serial.print(servo_num);
Serial.print("-");
Serial.print(val);
Serial.print(":");
//Convert to cm
Serial.print(uS / US_ROUNDTRIP_CM);
Serial.print("c");
val = 0;
}
/*Processing sketch to control two servos
and an HC-SR04 ultrasonic sensor for 3D surface scanning.
Authors:Prithvijit Chakrabarty (prithvichakra@gmail.com)
Kartik S. Lovekar (kslovekar@gmail.com)
*/
import processing.serial.*;
import toxi.geom.*;
import toxi.processing.*;
import peasy.*;
Serial port;
ToxiclibsSupport gfx;
PeasyCam cam;
int servo_num = 1, val = 0, alpha = 0, beta = 0, dist = 0;
int INC = 3;
float SERVO_RADIUS = 3.5;
float distances[][];
ArrayList<Vec3D> cloud;
boolean scanOver;
void setup(){
size(700, 500, P3D);
frameRate(100);
println(Serial.list());
port = new Serial(this, Serial.list()[0], 9600);
//Setup 3D plotting tool
gfx = new ToxiclibsSupport(this);
cam = new PeasyCam(this, 0, 0, 0, 100);
cloud = new ArrayList<Vec3D>();
scanOver = false;
distances = new float[181][71]; //Holds the minimum distance to a point on a surface
for(int i = 0; i < 181; i++)
for(int j = 0; j < 71; j++)
distances[i][j] = -1;
}
void draw(){
background(255);
if(!scanOver){
//Control loops to control the servos on the Arduino
for(int i = 1; i < 70; i+=INC){ //Loop for beta
for(int j = 0; j < 180; j+=INC){ //Loop for alpha
port.write(j+"x"); //Rotate servo 1
delay(50);
}
for(int j = 180; j >= 0; j-=INC){ //Return loop for alpha
port.write(j+"x"); //Rotate back servo 1
delay(50);
}
port.write(i+"y"); //Rotate servo 2
delay(50);
}
}
scanOver = true;
plot();
drawAxes();
}
//Parse echo scan values returned from the Arduino
void serialEvent(Serial port){
int next = port.read();
//Format for return: <servo_num>-<angle>:<distance>c
if(next == (int)('-')){
servo_num = val;
val = 0;
}
else if(next == (int)(':')){
if(servo_num == 1)
alpha = val;
else if(servo_num == 2)
beta = val;
val = 0;
}
else if(next == (int)('c')){
//Print current scan status
println("Servo:"+servo_num+" alpha="+alpha+" beta="+beta+" Dist = "+val);
dist = val;
val = 0;
//Find coordinates here - apply transforms to calculate (x,y,z)
float x = SERVO_RADIUS*sin(radians(beta)) + dist*cos(radians(alpha))*cos(radians(beta));
float y = dist*sin(radians(alpha));
float z = SERVO_RADIUS*cos(radians(beta)) + dist*cos(radians(alpha))*sin(radians(beta));
//Now, rotate out the faulty initial setting
int INIT_XY = 20, INIT_XZ = 60;
x = x*cos(radians(20))-y*sin(radians(20));
y = x*sin(radians(20))+y*cos(radians(20));
//rotation(INIT_XZ, x, z);
x = x*cos(radians(60))-z*sin(-radians(60));
z = x*sin(-radians(60))+z*cos(radians(60));
if(distances[alpha][beta] != -1){
distances[alpha][beta] = distances[alpha][beta]<dist?distances[alpha][beta]:dist;
cloud.add(new Vec3D(x,y,z));
}
else
distances[alpha][beta] = dist;
}
else if(next >= '0' && next <= '9'){
val = val * 10 + next - (int)('0');
}
}
//Add points from the cloud to the 3D space
void plot(){
for(int i = 0; i < cloud.size(); i++)
gfx.point(cloud.get(i));
}
void drawAxes() {
//X axis - Red
stroke(255, 0, 0);
line(-500, 0, 0, 500, 0, 0);
//Y axis - Green
stroke(0, 255, 0);
line(0, -500, 0, 0, 500, 0);
//Z axis - Blue
stroke(0, 0, 255);
line(0, 0, -500, 0, 0, 500);
}
@PCJohn

This comment has been minimized.

Copy link
Owner Author

commented Aug 25, 2015

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.