Ultrasound Triangulation
Jump to navigation
Jump to search
- https://www.sciencedirect.com/science/article/pii/S1474667015305036
- https://nxttime.wordpress.com/2012/11/25/triangulation-with-two-ultrasonic-sensors/
Experiment with BITPlan hardware
see SG90-Servo
Source code
# Test plotting distances at angle
# WF 2019-06-13
from pi.servo import Servo
from pi.ultrasound import UltraSound
import RPi.GPIO as GPIO
import pantograph
import math
import time
# a point at the angle and distance given
class DistPoint:
def __init__(self,angle,dist):
angle=angle-75
self.x=math.sin(math.radians(angle))*dist;
self.y=math.cos(math.radians(angle))*dist;
#
# plot the measured distances of the ultrasonic sensor
#
class DistancePlot(pantograph.PantographHandler):
def setup(self):
print("Starting angle/distance measurement ...")
self.radius = min(self.width, self.height) / 2
self.maxdist=150
self.us1=UltraSound(15,14)
self.us2=UltraSound(21,20)
self.us1.start()
self.us2.start()
self.servo1 = Servo(23)
self.servo2 = Servo(24)
self.servo1.start()
self.servo2.start()
self.dpDict1={}
self.dpDict2={}
self.firstUpdate=True
def ruler(self):
for dist in range(10,self.maxdist+10,10):
# draw the circle for the "rim" of the wheel
self.draw_circle(self.width/2, 0, self.scaleY(dist), "#f0f0f0")
def scaleX(self,x):
return x*self.width/self.maxdist;
def scaleY(self,y):
return y*self.height/self.maxdist;
def valid(self,dist):
if dist>self.maxdist:
return False;
if dist<0:
return False;
return True;
#
# plot the distance of the given dist/angle
#
def plotDistance(self,xofs,dangle,dpDict,dist,angle,step,color):
angle=angle+dangle
prevangle=angle-step
# only plot valid distances
if self.valid(dist):
# create the point
dp=DistPoint(angle,self.scaleY(dist))
if prevangle in dpDict:
dp2=dpDict.get(prevangle)
xofs=self.width/2+self.scaleX(xofs)
self.draw_line(self.width-(xofs+dp.x),dp.y,self.width-(xofs+dp2.x),dp2.y,color)
dpDict[angle]=dp
def update(self):
if self.firstUpdate:
self.ruler()
self.firstUpdate=False
step=1
start=50
stop=110+step
meanCount=3
dx=0
dangle=7.5
for angle in range(start,stop,step):
pwm1=self.servo1.setAngle(angle)
pwm2=self.servo2.setAngle(angle)
dist1 = self.us1.meanDistance(meanCount)
time.sleep(0.02) # avoid indirect echos
dist2 = self.us2.meanDistance(meanCount)
self.plotDistance(dx,dangle,self.dpDict1,dist1,angle,step,"#ff8000")
self.plotDistance(-dx,-dangle,self.dpDict2,dist2,angle,step,"#0000FF")
print angle,"{0:.2f}".format(pwm1),self.us1.distAsString(dist1),self.us2.distAsString(dist2)
# speed of servo is 0.1 secs per 60 degrees
time.sleep(0.1/6) #
if __name__ == '__main__':
try:
app = pantograph.SimplePantographApplication(DistancePlot)
app.run("0.0.0.0", 8000)
except KeyboardInterrupt:
print("Aborted ultrasound distance measurement by User ... cleaning up GPIO")
GPIO.cleanup()