Bot Matrix Printer 11/21/13
Jan 12, 2016
Bot Matrix Printer11/21/13
SonarsTheory of operation:
The SRF-08 emits an ultrasonic pulse, then waits for an echo to return.Based on the time between the pulse generation and echo return, the sensor calculates the distance from its rear face to the detected surface,and then reports the distance in centimeters over the i2c interface.
Behavior
Row1
Row0
Row2
Row3
Y target distanceX target distance
Sensor IntegrationThe platform houses an array of sonars. Two sonars are mounted along the right face of the platform, equidistantly ahead of and behind the drive wheels. Another sonar points ahead of the platform.
SRF08SRF08SRF08SRF08
SR
F08
SR
F08
Sensor IntegrationThe array of sonars allows the robot to determine its position in 2D space relative to
two perpendicular vertical surfaces, such as the corner of a room. Having sonars on two perpendicular faces of the platform allows the robot to determine its distance from the two surfaces, while positioning two of the sonars on the same face allows it to determine its relative rotation or parallelism.
SRF08SRF08SRF08SRF08
SR
F08
SR
F08
Distance 1 Distance 2
Distance 3
Sensor Integration
SRF08SRF08SRF08SRF08
Distance 1 = Distance 2
SRF08SRF08
SRF08SRF08
SR
F08
SR
F08
Distance 1 =/= Distance 2
Opposite = Distance2-Distance1
SR
F08 Xdist
Ydist = (Distance 1 + Distance 2)/2
import mathimport smbusimport timeimport servo5import servo8import srf08_1import srf08_2import srf08_3servo5.direction(heading = 'blah')row = 0time.sleep(30)while row < 12: ytarg = 20 + 5*row xtarg = 50 - 30*math.pow(-1, row) print "xtarg is" print xtarg xrng = srf08_3.srf() print "xrng is" print xrng while xrng >= xtarg : rng = srf08_1.srf() rng2 = srf08_2.srf() ydist = (rng+rng2)/2 yerr = (ydist-ytarg) opposite = rng-rng2 servo8.direction("forward", yerr, opposite) time.sleep(.05) xrng = srf08_3.srf() print "xrng" print xrng
servo8.direction("left", yerr, opposite) time.sleep(1.8) servo8.direction("straight", yerr, opposite) time.sleep(.45) servo8.direction("right", yerr, opposite) time.sleep(1.5) row = row+1 print "row" print row ytarg = 20 + 5*row xtarg = 50 - 30*math.pow(-1, row) print "xtarg is" print xtarg xrng = srf08_3.srf() print "xrng is" print xrng while xrng <= xtarg : rng = srf08_1.srf() rng2 = srf08_2.srf() ydist = (rng+rng2)/2 yerr = (ydist-ytarg) opposite = rng-rng2 servo8.direction("backward", yerr, opposite) time.sleep(.05) xrng = srf08_3.srf() print "xrng" print xrng servo8.direction("left", yerr, opposite) time.sleep(1.8) servo8.direction("straight", yerr, opposite) time.sleep(.45) servo8.direction("right", yerr, opposite) time.sleep(1.5) row = row+1 print "row" print rowservo8.direction("x", yerr, opposite)
Navigation Programming
def color( ydist, xdist ): import numpy import servodraw
a = [[1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], [1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], [0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, ], [0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, ], [0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, ], [0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, ], [0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, ], [0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, ], [0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, ], [0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 0, ], [1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, ], [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, ]]
y = int(ydist) x = int(xdist)
print y print x b = a[y][x] servodraw.direction(b) return b
Print Programming
def direction( heading ):
import Adafruit_BBIO.PWM as PWM
servo_pin = "P8_19" duty_min = 3 duty_max = 14.5 duty_span = duty_max - duty_min
if heading == 'x':
PWM.cleanup()
return
elif heading == '1': angle = 180 elif heading == '0': angle = 0 else: angle = 90
angle_f = float(angle)
duty = ((angle_f / 180) * duty_span + duty_min)
PWM.set_duty_cycle(servo_pin, duty)
return