def handle_command(self, line):
if line != "":
sl = 0
sr = 0
if line == 'rdlt':
answer = 'rdlt ' + str(sl + 100) + '\n'
self.send(answer)
print answer
elif line == 'rdrt':
answer = 'rdrt ' + str(sr + 100) + '\n'
self.send(answer)
print answer
elif line == 'rdus':
us = int(hcsr04.getDistance())
if us > 100:
us = 100
answer = 'rdus ' + str(us) + '\n'
self.send(answer)
print answer
else:
c, p = line.split()
print line.split()
if c == 'lt':
self.send('\n')
print 'motor left ', p
sl = int(p) - 100
pz.setMotor(0, sl)
elif c == 'rt':
self.send('\n')
print 'motor right ', p
sr = int(p) - 100
pz.setMotor(1, sr)
else:
self.send('unknown command\n')
print 'Unknown command:', line
def texasRanger():
a = 0 # distance sum dummy
i = 0 # measurement counter
imax = 10 # total measurement number
while i < imax:
a += hcsr04.getDistance()
i += 1
return a / imax
def run():
distance = int(hcsr04.getDistance())
while (distance > 10):
distance = int(hcsr04.getDistance())
pz.stop()
while True:
distance = int(hcsr04.getDistance())
print "Distance:", distance
distance2Pixel(distance)
if distance < 10:
pz.stop()
time.sleep(1)
while (distance < 18):
distance = int(hcsr04.getDistance())
pz.spinLeft(speed)
elif distance < 25:
pz.forward(speed - 20)
else:
pz.forward(speed)
time.sleep(0.1)
liftVal = 0
gripVal = 90
pz.setOutput(rot, rotVal)
pz.setOutput(tilt, tiltVal)
pz.setOutput(lift, liftVal)
pz.setOutput(grip, gripVal)
gripDistance = 3 # the distance to sensor below which the grabbing is commenced
pz.setOutput(grip, gripMin)
time.sleep(.5)
import hcsr04
hcsr04.init()
# main loop
try:
distance = int(hcsr04.getDistance())
print "object " + str(distance) + " far"
while (distance > gripDistance) and (tiltVal > tiltMin):
distance = int(hcsr04.getDistance()) # distance in cm
print "object " + str(distance - gripDistance) + " too far"
tiltVal = max(tiltMin, tiltVal -
5) # moves closer, but within the operational limit
time.sleep(.5) # gives you time to evacuate, can have zero
pz.setOutput(rot, rotVal)
pz.setOutput(tilt, tiltVal)
pz.setOutput(lift, liftVal)
except KeyboardInterrupt:
print()
i += 1
showFace(pairData, 255, 0, 255)#
time.sleep(1)
print 'Wiimote connected'
wii.led = 1
showFace(pairData, 0, 255, 0)
wii.rpt_mode = cwiid.RPT_BTN
time.sleep(1)
while True:
buttons = wii.state['buttons']
if (buttons & cwiid.BTN_UP):
# Forwards
# stop all motors if too close to an object to prevent face plant
if sonar.getDistance() < 10:
pz.stop()
showFace(oooohData, 0, 255, 0)
else:
time.sleep(button_delay)
pz.forward(50)
showFace(smileData, 255, 0, 0)
elif (buttons & cwiid.BTN_DOWN):
time.sleep(button_delay)
pz.reverse(50)
showFace(grimaceData, 255, 0, 255)
elif (buttons & cwiid.BTN_LEFT):
time.sleep(button_delay)
pz.spinLeft(50)
showFace(oooohData, 0, 255, 0)
elif (buttons & cwiid.BTN_RIGHT):
#! /usr/bin/env python
#
# Basic test of HC-SR04 ultrasonic sensor on Picon Zero
import hcsr04, time
import piconzero as pz
pz.setInputConfig(2, 0)
pz.setInputConfig(3, 0)
pz.init()
hcsr04.init()
try:
while True:
distance = int(hcsr04.getDistance())
print("Distance : "), distance
if (((pz.readInput(2) != 1) or (pz.readInput(3) != 1))
and distance > 13):
pz.reverse(60)
else:
pz.stop()
time.sleep(0.2)
pz.spinRight(50)
time.sleep(0.2)
pz.stop()
break
except KeyboardInterrupt:
print
finally:
hcsr04.cleanup()
pz.cleanup()
def automaze():
pz.stop()
mylcd.lcd_display_string("Auto Maze ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
time.sleep(2)
MSPEED = 20
MTURN = 90
GO = 1
PREP = 1
STEP = 0 # start step count
while PREP == 1: #get ready to go
for event in get_key():
mylcd.lcd_display_string("Press G to GO ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
if event.code == "KEY_G":
PREP = 0
elif event.code == "KEY_E":
GO = 0
PREP = 0
while GO == 1: #new simple quit - works perfectly
if button.is_pressed:
pz.stop()
GO = 0
else:
RIGHTIR = pz.readInput(0) #assign right IR to a variable
LEFTIR = pz.readInput(1) #assign left IR to a variable
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
pz.forward(MSPEED)
#steps to follow to complete the maze
if RANGE < 25 and STEP == 0: #first right turn
pz.stop()
pz.spinRight(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 1
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 1: #second right turn
pz.stop()
pz.spinRight(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 2
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 2: #third right
pz.stop()
pz.spinRight(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 3
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 3: #first left
pz.stop()
pz.spinLeft(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 4
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 4: #second left
pz.stop()
pz.spinLeft(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 5
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 5: #third left
pz.stop()
pz.spinLeft(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 6
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
#emergency wall avoidance protocol
if LEFTIR == 0:
pz.spinRight(100)
time.sleep(0.01)
elif RIGHTIR == 0:
pz.spinLeft(100)
time.sleep(0.01)
mylcd = I2C_LCD_driver.lcd() #assign LCD to variable for ease of use
pz.init() #initiate hardware
pz.setInputConfig(0, 0) #right IR sensor is input 0 and digital
pz.setInputConfig(1, 0) #left IR sensor is input 1 and digital
pz.setInputConfig(2, 0) #right line sensor is input 2 and digital
pz.setInputConfig(3, 0) #left line is input 3 and digital
RIGHTIR = pz.readInput(0) #assign right IR to a variable
LEFTIR = pz.readInput(1) #assign left IR to a variable
RIGHTLINE = pz.readInput(2) #assign right line sensor to a variable
LEFTLINE = pz.readInput(3) #assign left line sensor to a variable
hcsr04.init() #initiate hardware
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
button = Button(22)
#end of hardware setup
#______________________________________________________________________________
#____________________________________________________________________________________
#functions for individual tasks
def remotecontrol(): #works perfect
pz.stop()
mylcd.lcd_display_string("Remote Control ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
time.sleep(2)
def getDistance():
return hcsr04.getDistance()
pz.stop()
pts = []
row=2
scanWin.addstr(1,1,"Distance Map" )
scanWin.addstr(2,1, "Sonar : ir")
for span in range( 39, 75, 3 ):
irLine=""
sonLine=""
for stilt in range( 150, 30, -3 ):
pz.setOutput (pan, span)
pz.setOutput (tilt, stilt)
time.sleep( 0.1)
ir = pz.readInput(irSen)
distance = int(hcsr04.getDistance())
#print "At pan,tilt: ",span,",",stilt,": ir Distance:", ir, " sonic distance: ", distance
#volts=min(1,ir*0.0048828125); # // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
volts=min(1,ir*0.002929688); # // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
actdist=65*pow(volts, -1.10); # // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
#print "ir act distance: ",actdist
# TODO: Save values and pos so build map
# rescale reading to ascii value
irLine = irLine + asciiSensor( 400, 400-min(400,ir) )
spinTime = 0.009
forwardTime = 0.0005
# stop variables
sideStop = 21
frontStop = 100
# start recording
ts = str(time())
cam.vflip = True
cam.hflip = True
cam.start_recording("/home/pi/qmpiwars/videos/straight-" + ts + ".h264")
try:
while True:
distance = int(dist.getDistance())
LeftDist = sideDist.DistSensor(27, 22) #Left
RightDist = sideDist.DistSensor(17, 18) #Right
print "----------------------------------"
print "Front Distance:", distance
print "Left Distance:", LeftDist
print "Right Distance:", RightDist
print "----------------------------------"
pz.forward(forwardSpeed)
sleep(forwardTime)
sideDist.neoPixelLight("forward")
if(distance <= frontStop): # too close - STOP!
print "Stop!"
#pz.stop()
sideDist.neoPixelLight("off")
elif(LeftDist <= sideStop): # heading left - turn right
#! /usr/bin/env python
#
# Basic test of HC-SR04 ultrasonic sensor on Picon Zero
import hcsr04, time
hcsr04.init()
try:
while True:
distance = int(hcsr04.getDistance())
print "Distance:", distance
time.sleep(1)
except KeyboardInterrupt:
print
finally:
hcsr04.cleanup()
# distance variables
leftMax = 12
frontStop = 25
rightMax = 12
# last known dist
lastFront = 0
lastLeft = 0
lastRight = 0
# start camera
# picamera start recording now...
try:
lastFront = int(dist.getDistance())
while lastFront > frontStop: # go forward to the first wall
print "head to first wall"
pz.forward(forwardSpeed)
sleep(forwardTime)
lastFront = int(dist.getDistance())
print str(lastFront)
pz.stop()
leftTurn = True
# measure rhs dist
currentRight = sideDist.DistSensor(17, 18)
# assume this to be largest and smallest reading
largeRight = currentRight
smallRight = currentRight
