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()
finally:
if (distance > gripDistance):
print("object unreachable")
else: # grab and move if the object is withing grabbing distance
print("EXTERMINATE")
pz.setOutput(grip, gripMax)
time.sleep(.5)
print("EXTERMINATE")
pz.setOutput(rot, 40)
time.sleep(.5)
pz.setOutput(tilt, tiltVal - 10)
time.sleep(.5)
pz.setOutput(lift, liftMax)
time.sleep(.5)
print("EXTERMINATE!")
hcsr04.cleanup()
pz.cleanup()
time.sleep(1)
print 'Press 1 + 2 on your Wii Remote now ...'
time.sleep(1)
wii = None
i = 2
# Try to connect to the Wiimote & quit if not found after several attempts
while not wii:
try:
wii = cwiid.Wiimote()
except RuntimeError:
if i > 10:
print "Error: Could Not Connect to WiiMote"
showFace(pairData, 255, 0, 0) #
time.sleep(1)
sonar.cleanup()
quit()
break
print "Trouble connecting to connect to Wiimote"
print "attempt " + str(i)
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:
leftState=leftRev
rightState=rightRev
#sensorWin.clear()
sensorWin.addstr(1,1, "Sonar Distance: "+ str(distance)+" ")
sensorWin.addstr(1,30, " Safe: "+ str(fwdSafe)+" " )
sensorWin.addstr(1,40, " iR Distance: "+ str(ir)+" " )
sensorWin.addstr(2,1, "Left Counter: "+ str(leftTotal)+" " )
sensorWin.addstr(2,30, "Right Counter: "+ str(rightTotal)+" " )
sensorWin.refresh()
# coli detection
if( distance < fwdSafe ):
#pz.stop()
# clear value so it does not prevent any turning to avoid!
#fwdSafe=0
#statusWin.clear()
statusWin.addstr(2,1, "Stopping.... Something too close ")
statusWin.refresh()
except KeyboardInterrupt:
print
finally:
hcsr04.cleanup()
pz.cleanup()
def cleanupPZ():
pz.cleanup()
hcsr04.cleanup()
sideDist.neoPixelLight("forward")
if(LeftDist < sideStop): # still too close move forward a bit more
pz.forward(forwardSpeed)
sleep(spinTime)
sideDist.neoPixelLight("forward")
elif(RightDist <= sideStop): # heading right - turn left
print "turning left"
pz.spinRight(spinSpeed)
sleep(spinTime)
sideDist.neoPixelLight("left")
pz.forward(forwardSpeed)
sleep(forwardTime)
sideDist.neoPixelLight("forward")
if(RightDist < sideStop): # still too close move forward a bit
pz.forward(forwardSpeed)
sleep(spinTime)
sideDist.neoPixelLight("forward")
else:
print "going forward"
pz.forward(forwardSpeed)
sleep(forwardTime)
sideDist.neoPixelLight("forward")
except KeyboardInterrupt:
print "cleaning up and exiting"
finally:
dist.cleanup()
sideDist.neoPixelLight("off")
pz.cleanup()
sideDist.cleanup()
stop_recording()
