Figure 1 : System
This project was designed to show that you can't solve every problem in software, that you need to understand a system's limitations and that adding hardware can actually simplify software development. Aim: to implement a system that scans 180 degrees, measure the distance to the nearest obstacle and display these as a polar plot. One system will be implemented in software (as much as possible), another using both software and dedicated hardware. Both systems use a common sensor platform as shown in figure 2. Position control is implemented using a standard analogue model servo (Link), distance measurement via an off the shelf, HC-SR04 ultrasonic module (Link). The 3D printed brackets where obtained from: (Link) (Link), the bottom base bracket was quickly done in openScad (Link). The base was laser cut, layout drawn in LibreCad (Link). All 3D model and dxf files are available here (Local).
Figure 2 : Sensor platform
Figure 3 : 3D printed parts
Figure 4 : Plastic base
The software biased system requires minimal external hardware, as shown in figures 5 and 6. The resistors shown are purely for protection i.e. the Raspberry Pi is not 5V tolerant. Resistors R2 and R4 are not really needed, these are 41 ohms (were to hand), just there to limit the current if i do something silly. Resistors R1 and R3 (4K7) form a potential, shifting the ultrasonic module's output pulse from 5V down to 2.5V i.e. Pi safe.
Figure 5 : Software system interface
Figure 6 : Software interface circuit diagram
The servo and ultrasonic sensor are controlled using a simple python prgram. The ultrasonic sensor control was based on this example: (Link) (Local). The servo control was based on this example: (Link) (Local). The full code listing is shown below:
import RPi.GPIO as GPIO import time # Servo position control func def update(angle): duty = float(angle) / 10 + 2.5 pwm.ChangeDutyCycle(duty) #IO pin numbers TRIG = 17 ECHO = 27 SERVO = 18 #Setup IO ports GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) GPIO.setup(TRIG, GPIO.OUT) GPIO.setup(ECHO, GPIO.IN) GPIO.setup(SERVO, GPIO.OUT) pwm = GPIO.PWM(SERVO, 100) pwm.start(5) GPIO.output(TRIG, False) #Initialise variables direction = True angle = 0 pos = 0 pos_prev = 17 pulse_start = 0 pulse_end = 0 b = [ 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ] c = [ 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ] #Main loop while True : #Send trigger (start) pulse to US sensor GPIO.output(TRIG, True) time.sleep(0.00001) GPIO.output(TRIG, False) #Wait until RX signal 0 or max delay timeout = 0 while (GPIO.input(ECHO)==0) and (timeout < 50): timeout = timeout + 1 pulse_start = time.time() #Wait for RX signal while GPIO.input(ECHO)==1: pulse_end = time.time() #Calculate time of flight pulse_duration = pulse_end - pulse_start #convert time to distance distance = pulse_duration * 17150 distance = round(distance, 2) #Trap if no pulse recieved if distance < 0: distance = 0 distance = distance * 10 #Set max distance for display if distance > 600: distance = 600 print "Distance: ", distance,"mm", "Angle: ", pos #Move sensor head update(angle) time.sleep(0.5) #update log file file = open('plot_1.dat', 'w') outputString = "\n" #this array contains a single value i.e. scan line on plot c[pos] = 600 c[pos_prev] = 0 #update distance string with new value for i in range(0,18): if pos == i: b[i] = distance outputString = outputString + str(i*10) + "\t" + str(b[i]) + "\t" + str(c[i]) + "\n" #write data to file file.write(outputString) file.close #update scan direction and position pos_prev = pos if direction: angle = angle + 10 pos = pos + 1 else: angle = angle - 10 pos = pos - 1 if angle > 180: direction = False angle = 170 pos = 17 if angle < 0: direction = True angle = 10 pos = 1
This program produces a log file, three columns, [Angle : Range to Object : Scan Line], as shown below:
0 106.5 0 10 600 0 20 600 0 30 600 0 40 83.7 0 50 73.8 0 60 66.2 0 70 64.8 0 80 88.6 0 90 600 0 100 600 600 110 83.2 0 120 83.4 0 130 69.6 0 140 84.9 0 150 107.2 0 160 106.6 0 170 69.1 0
A separate shell script program then reads this file and plots this data to the screen using GnuPlot. This automatically rereads this file and updates the display every 0.5 seconds. A screen shot of the display is shown in figure 7. RED regions indicate open space, WHITE regions indicate the distance to the closest object, the current sensor position is indicated by the GREEN scan line. A video of the displayed plot is available here: (Video). Figure 8 shows the objects in front of the scanning sensor (circled in red) for this plot, to its right is the stacked tables, to its left open space and the other sensor platform.
#!/bin/bash gnuplot plot.gnu
unset border set polar set angles degrees set style line 10 lt 1 lc 0 lw 0.3 set grid polar 60 set grid ls 10 set xrange [-700:700] set yrange [-700:700] set xtics axis set ytics axis set xtics scale 0 set xtics ("" 100, "" 200, "" 300, "" 400, "" 500, "" 600) set ytics 0, -100, -600 set size square unset key set_label(x, text) = sprintf("set label '%s' at (750*cos(%f)), (750*sin(%f)) center", text, x, x) #this places a label on the outside #here all labels are created eval set_label(0, "0") eval set_label(60, "60") eval set_label(120, "120") eval set_label(180, "180") eval set_label(240, "240") eval set_label(300, "300") #set style line 11 lt 1 lc 3 lw 2 pt 2 ps 2 #set the line style for the plot plot "plot.dat" using 1:2 with filledcurves, "plot.dat" using 1:3 with line pause 0.5 reread<
Figure 7 : Screen plot
Figure 8 : Table
This program works, but the timing of the servo pulse does vary a little, resulting in servo jitter i.e. delays in receiving the ultrasonic signal, operating system calls, or just simply updating the mouse pointer position etc, means that the servo pulse is not the correct length or is not produced at the correct time, as shown in figure 9. A video of the software controlled servo is available here: (Video).
Figure 9 : Pulse jitter
To remove this jitter the servo can be controlled by dedicated hardware i.e. using a CPLD, as shown in figures 10 - 13. The Raspberry Pi tells the CPLD what position to turn the servo to, low level control is now done in hardware, therefore, any small changes in the software timings no longer affects the servo control pulse. This hardware was based on the following example: (Link) (Local). To further simplify the software, control of the ultrasonic sensor can also be passed to the CPLD. The Raspberry Pi now requests a distance measurement from the CPLD which returns a 7 bit value. Again all low level control / timing is performed in hardware on the CPLD, a simple state machine, coded in VHDL.
Figure 10 : Hardware system interface
Figure 11 : CPLD
Figure 12 : Clock (4MHz) and Buffer
Figure 13 : I2C interface
Figure 14 : I2C interface IC
Figure 15 : I2C interface internals
To allow the Raspberry Pi to communicate to the CPLD it needs a data link. The simplest way to do this is to used three PCF8574 I2C 8bit IO expanders (Local), as shown in figures 13 - 15. Two are set up as outputs (servo position and US sensor control) and one as an input (US sensor data), the Raspbery Pi can then read or write to these I2C mapped addresses. Connected to these parallel ports is the CPLD as shown in figure 16 - 17. This operates from a 4MHz clock, with an internal clock divider generating the timing signals needed for each hardware subsystem. Individual VHDL files and the complete CPLD ISE project is available here:
Figure 16 : Top Level Hardware Controller, VHDL (left), Symbol (right)
Figure 17 : Hardware Controller
Top level control is implemented in another python program, shown below:
import smbus import time POS_I2C_ADDR = 0x38 DIS_I2C_ADDR = 0x39 CTL_I2C_ADDR = 0x3A bus = smbus.SMBus(1) pos = 0 pos_prev = 31 direction = True # Distance (37) b = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] c = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] bus.write_byte(DIS_I2C_ADDR, 0xFF) while True: # Trigger TX bus.write_byte(CTL_I2C_ADDR, 0x01) bus.write_byte(CTL_I2C_ADDR, 0x00) # Time to send start = 2 x 50us # Start pulse 200us # Max RX wait 10ms # Software delay 0.1s = 100ms time.sleep(0.1) # Time to read 50us data = bus.read_byte(DIS_I2C_ADDR) # print "Distance: " + str(data) # Speed of sound 340m/s, clock 64KHz = 15us # 1 sec = 340,000mm # 1 tick = 2.5mm distance = float(data) * 2.5 # Max distance = 255 x 2.5 = 638mm if distance > 600: distance = 600 # print "Distance: " + str(data) file = open('plot.dat', 'w') outputString = "\n" c[pos] = 600 c[pos_prev] = 0 for i in range(0,31): if pos == i: b[i] = distance outputString = outputString + str(i*6) + "\t" + str(b[i]) + "\t" + str(c[i]) + "\n" file.write(outputString) file.close pos_prev = pos if direction: pos = pos + 1 else: pos = pos - 1 if pos == 31: direction = False pos = 29 if pos < 0: direction = True pos = 1 # Print Angle print "Angle: " + str( pos ) + " Distance: " + str(data) bus.write_byte(POS_I2C_ADDR, int(float(pos) * 3.5)) time.sleep(0.2)
As servo control is implemented in dedicated hardware, timing resolution can be more accurately controlled, allowing more readings to be taken during a scan. A video of the hardware controlled servo is available here: (Video).
Figure 18 : System
The GNUplot display was a bit of a bodge, a quick and dirty solution. Finally got around to getting a nicer GUI working using Tkinter. Do confess GUIs are not my thing, to simplify the job went for a simple gif background (shown in figure 19) and canvas + lines.
Figure 19 : GIF background
Test code is below, current reading is displayed in green, readings recorded during the current sweep are a light blue, readings recorded on the previous sweep a dark blue. A video of the software display random test data value is available here: (Video).
import Tkinter as tk import math import random canvasWidth = 860 canvasHeight = 860 canvasCenterX = (canvasWidth/2) canvasCenterY = (canvasHeight/2) centerRadius = 10 angleOffset = 90 lineWidth = 3 lineLength = 300 lineNumber = 0 lineObjects =  colourScale = [(0,255,0), (0, 208, 226), (0,190,207), (0,170,188), (0,150,162), (0,130,142), (0,110,121), (0,90,100), (0,75,75), (0,50,50)] clockwise = True anticlockwise = False # Init measurement array (36) measurement =  for i in range(0, 185, 5): ran_number = random.random() distance = int(ran_number*300) measurementData = (i, distance) measurement.append( measurementData ) def updateDisplay(): global lineNumber, lineObjects, clockwise, anticlockwise, measurement #print len(measurement), lineNumber angle = measurement[lineNumber] length = measurement[lineNumber] dx = length * math.sin(math.radians(angleOffset - angle)) dy = length * math.cos(math.radians(angleOffset - angle)) colour = '#%02x%02x%02x' % colourScale lineObjects.append(canvas.create_line(canvasCenterX, canvasCenterY, dx+canvasCenterX, dy+canvasCenterY, width=lineWidth, fill=colour)) if lineNumber != 0: if clockwise: canvas.delete(lineObjects[lineNumber-1]) angle = measurement[lineNumber-1] length = measurement[lineNumber-1] dx = length * math.sin(math.radians(angleOffset - angle)) dy = length * math.cos(math.radians(angleOffset - angle)) colour = '#%02x%02x%02x' % colourScale lineObjects[lineNumber-1] = canvas.create_line(canvasCenterX, canvasCenterY, dx+canvasCenterX, dy+canvasCenterY, width=lineWidth, fill=colour) else: if lineNumber < (len(measurement)-1): canvas.delete(lineObjects[lineNumber+1]) angle = measurement[lineNumber+1] length = measurement[lineNumber+1] dx = length * math.sin(math.radians(angleOffset - angle)) dy = length * math.cos(math.radians(angleOffset - angle)) colour = '#%02x%02x%02x' % colourScale lineObjects[lineNumber+1] = canvas.create_line(canvasCenterX, canvasCenterY, dx+canvasCenterX, dy+canvasCenterY, width=lineWidth, fill=colour) if clockwise: lineNumber +=1 else: lineNumber -=1 if ((lineNumber == len(measurement)) and clockwise) or ((lineNumber == -1) and anticlockwise): for i in range(0, len(lineObjects)): canvas.delete(lineObjects[i]) lineObjects =  for i in range(0, len(measurement)): angle = measurement[i] length = measurement[i] dx = length * math.sin(math.radians(angleOffset - angle)) dy = length * math.cos(math.radians(angleOffset - angle)) colour = '#%02x%02x%02x' % colourScale lineObjects.append(canvas.create_line(canvasCenterX, canvasCenterY, dx+canvasCenterX, dy+canvasCenterY, width=lineWidth, fill=colour)) if clockwise: clockwise = False anticlockwise = True lineNumber -=1 else: clockwise = True anticlockwise = False lineNumber = 0 measurement = for i in range(0, 185, 5): ran_number = random.random() distance = int(ran_number*300) measurementData = (i, distance) measurement.append( measurementData ) root.after(200, updateDisplay) root = tk.Tk() frame = tk.Frame(root) frame.pack() canvas = tk.Canvas(frame, width=canvasWidth, height=canvasHeight, bg='black') canvas.pack(expand=tk.YES, fill=tk.BOTH) photo=tk.PhotoImage(file='background2.gif') canvas.create_image(35, 50, image=photo, anchor=tk.NW) colour = '#%02x%02x%02x' % colourScale canvas.create_oval(canvasCenterX-centerRadius, canvasCenterY-centerRadius, canvasCenterX+centerRadius, canvasCenterY+centerRadius, width=0, fill=colour) root.after(0, updateDisplay) root.mainloop()
This work is licensed under a Creative Commons Attribution-NonCommercial-NoDerivatives 4.0 International License.
Contact details: email - firstname.lastname@example.org, telephone - 01904 32(5473)Back