def wallAvoid():
print('Starting wall avoid')
speed = 100
time_end = time.time() + 4
# main loop
try:
for i in range(2):
time_end = time.time() + 6
while time.time() < time_end:
rightSensor = robohat.irRight()
leftSensor = robohat.irLeft()
if rightSensor and leftSensor:
robohat.spinLeft(speed)
time.sleep(1)
elif rightSensor:
robohat.spinLeft(speed)
time.sleep(0.2)
elif leftSensor == True:
robohat.spinRight(speed)
time.sleep(0.2)
else:
robohat.forward(speed)
robohat.forward(0)
time.sleep(3)
robohat.forward(speed)
time.sleep(0.2)
robohat.spinLeft(speed)
time.sleep(4)
except KeyboardInterrupt:
robohat.stop()
robohat.cleanup()
def wallAvoid():
print('Starting wall avoid')
speed = 100
time_end = time.time() + 4
# main loop
try:
for i in range(2):
time_end = time.time() + 6
while time.time() < time_end:
rightSensor = robohat.irRight()
leftSensor = robohat.irLeft()
if rightSensor and leftSensor:
robohat.spinLeft(speed)
time.sleep(1)
elif rightSensor:
robohat.spinLeft(speed)
time.sleep(0.2)
elif leftSensor == True:
robohat.spinRight(speed)
time.sleep(0.2)
else:
robohat.forward(speed)
robohat.forward(0)
time.sleep(3)
robohat.forward(speed)
time.sleep(0.2)
robohat.spinLeft(speed)
time.sleep(4)
except KeyboardInterrupt:
robohat.stop()
robohat.cleanup()
def detect_object(self):
newL = robohat.irLeft()
newR = robohat.irRight()
if (newL != self.lastL) or (newR != self.lastR):
robohat.turnReverse(40, 40)
time.sleep(0.7)
robohat.spinRight(70)
time.sleep(0.4)
robohat.turnForward(20, 20)
else:
pass
def __init__(self):
print("Start")
robohat.init()
self.obj_to_detect = 'None'
self.xValue = 0.0
self.yValue = 0.0
self.lastL = robohat.irLeft()
self.lastR = robohat.irRight()
self.servo_tilt = 22
self.servo_pan = 18
gpio.setup(self.servo_tilt, gpio.OUT)
gpio.setup(self.servo_pan, gpio.OUT)
self.tilt_pwm = gpio.PWM(self.servo_tilt, 200)
self.pan_pwm = gpio.PWM(self.servo_pan, 200)
self.tilt_pwm.start(16) # 16-40
self.pan_pwm.start(14) # 14-20
time.sleep(0.5)
self.mode = 1 # 1-AUTONOMOUS 2-Joystick
self.xboxCont = XboxController.XboxController(joystickNo=0,
scale=1,
invertYAxis=False)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.LTHUMBX,
self.leftThumbX)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.LTHUMBY,
self.leftThumbY)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.BACK,
self.backButton)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.BACK,
self.backButton)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.RB,
self.rb)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.LB,
self.lb)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.Y,
self.buttY)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.X,
self.buttX)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.A,
self.buttA)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.B,
self.buttB)
self.xboxCont.start()
self.running = True
import robohat, time
speed = 80
robohat.init()
# main loop
try:
while True:
rightSensor = robohat.irRight()
leftSensor = robohat.irLeft()
print "right:{0}, left:{1}".format(rightSensor, leftSensor)
if rightSensor and leftSensor:
robohat.spinLeft(speed)
time.sleep(1)
elif rightSensor:
robohat.spinLeft(speed)
time.sleep(0.2)
print('Turn Left')
elif leftSensor == True:
robohat.spinRight(speed)
time.sleep(0.2)
print('Turn Right')
else:
robohat.forward(speed)
except KeyboardInterrupt:
robohat.stop()
robohat.cleanup()
def handleMessage(msg, data):
global server, tilt, yaw
print("Incoming", msg, data)
if msg == Messages.MSG_READ_SENSORS:
irL = jsonBool(robohat.irLeft())
irR = jsonBool(robohat.irRight())
lineL = jsonBool(robohat.irLeftLine())
lineR = jsonBool(robohat.irRightLine())
sonar = robohat.getDistance()
cmpCal = jsonBool(micro_api.isCompassCalibrated())
print "Read sensors!"
msg = '{{"msg":{0},"data":{{"irL":{1},"irR":{2},"lineL":{3},"lineR":{4},"dist":{5},"cmpCal":{6}}}}}'
msg = msg.format(Messages.MSG_SENSOR_DATA, irL, irR, lineL, lineR,
sonar, cmpCal)
server.send(msg)
elif msg == Messages.MSG_FORWARD or msg == Messages.MSG_REVERSE:
revs = 2
speed = 40
clicks = move.getClicks()
if data != None:
if "r" in data:
revs = data["r"]
if "s" in data:
speed = data["s"]
if msg == Messages.MSG_FORWARD:
print "Forward!"
move.forward(revs, speed)
else:
print "Reverse!"
move.reverse(revs, speed)
newClicks = move.getClicks()
irL = jsonBool(robohat.irLeft())
irR = jsonBool(robohat.irRight())
totClicks = newClicks - clicks
reply = '{{"msg":{0},"data":{{"msg":{1},"clicks":{2},"revs":{3},"l":{4},"r":{5}}}}}'
reply = reply.format(Messages.MSG_OK_DONE, msg, totClicks,
(float(totClicks / move.CLICKS_PER_REV)), irL,
irR)
server.send(reply)
elif msg == Messages.MSG_LEFT or msg == Messages.MSG_RIGHT:
revs = 0.5
speed = 100
clicks = move.getClicks()
if data != None:
if "r" in data:
revs = data["r"]
if "s" in data:
speed = data["s"]
if msg == Messages.MSG_LEFT:
print "Left!"
move.left(revs, speed)
else:
print "Right!"
move.right(revs, speed)
newClicks = move.getClicks()
totClicks = newClicks - clicks
irL = jsonBool(robohat.irLeft())
irR = jsonBool(robohat.irRight())
reply = '{{"msg":{0},"data":{{"msg":{1},"clicks":{2},"revs":{3},"l":{4},"r":{5}}}}}'
reply = reply.format(Messages.MSG_OK_DONE, msg, totClicks,
(float(totClicks / move.CLICKS_PER_REV)), irL,
irR)
server.send(reply)
elif msg == Messages.MSG_SONAR_UP:
print "Sonar up!"
tilt.up()
elif msg == Messages.MSG_SONAR_CENTRE:
print "Sonar centre!"
tilt.centre()
elif msg == Messages.MSG_SONAR_DOWN:
print "Sonar down!"
tilt.down()
elif msg == Messages.MSG_SONAR_LOW:
print "Sonar low!"
tilt.low()
elif msg == Messages.MSG_SONAR_MID:
print "Sonar middle!"
yaw.mid()
elif msg == Messages.MSG_SONAR_LEFT:
print "Sonar left!"
yaw.left()
elif msg == Messages.MSG_SONAR_RIGHT:
print "Sonar right!"
yaw.right()
elif msg == Messages.MSG_SONAR_SCAN:
sonarScan()
elif msg == Messages.MSG_PARK_SONAR:
yaw.mid()
tilt.park()
elif msg == Messages.MSG_FLOOR_SONAR:
yaw.mid()
tilt.floor()
elif msg == Messages.MSG_GET_CLICKS:
clicks = move.getClicks()
msg = '{{"msg":{0},"data":{{"clicks":{1}}}}}'
msg = msg.format(Messages.MSG_CLICK_DATA, clicks)
server.send(msg)
elif msg == Messages.MSG_RESET_CLICKS:
move.resetClicks()
elif msg == Messages.MSG_TILT_PERCENT or msg == Messages.MSG_YAW_PERCENT:
v = 50
if data != None:
if "v" in data:
val = data["v"]
if msg == Messages.MSG_TILT_PERCENT:
print "Tilt", val
tilt.percentage(val)
else:
print "Yaw", val
yaw.percentage(val)
reply = '{{"msg":{0},"data":{{"msg":{1}}}}}'
reply = reply.format(Messages.MSG_OK_DONE, msg)
server.send(reply)
elif msg == Messages.MSG_DETAIL_SCAN:
hGran = 5
vGran = 5
width = 80
height = 60
x = 50
y = 40
if data != None:
if "hG" in data:
hGran = data["hG"]
if "vG" in data:
vGran = data["vG"]
if "w" in data:
width = data["w"]
if "h" in data:
height = data["h"]
if "x" in data:
x = data["x"]
if "y" in data:
y = data["y"]
print "Scan:", hGran, ",", vGran
detailedSonarScan(hGran, vGran, width, height, x, y)
elif msg == Messages.MSG_FIND_WALL:
print "Find wall!"
clicks = move.getClicks()
yaw.mid()
tilt.park()
move.findWall(35)
newClicks = move.getClicks()
totClicks = newClicks - clicks
irL = jsonBool(robohat.irLeft())
irR = jsonBool(robohat.irRight())
reply = '{{"msg":{0},"data":{{"msg":{1},"clicks":{2},"revs":{3},"l":{4},"r":{5}}}}}'
reply = reply.format(Messages.MSG_OK_DONE, msg, totClicks,
(float(totClicks / move.CLICKS_PER_REV)), irL,
irR)
server.send(reply)
elif msg == Messages.MSG_COMPASS_CALIBRATE:
print "[INFO] Start compass calibration"
micro_api.calibrateCompass()
elif msg == Messages.MSG_COMPASS_GET_HEADING:
heading = micro_api.getCompassHeading()
reply = '{{"msg":{0},"data":{{"heading":{1}}}}}'
print "[INFO] Compass heading: {}".format(heading)
reply = reply.format(Messages.MSG_COMPASS_HEADING, heading)
server.send(reply)
else:
print "[WARN] Not handled!", msg
import sys, time
import robohat
robohat.init()
try:
lastL = robohat.irLeft()
lastR = robohat.irRight()
lastLineL = robohat.irLeftLine()
lastLineR = robohat.irRightLine()
print 'Left, Right, LeftLine, RightLine:', lastL, lastR, lastLineL, lastLineR
print
while True:
newL = robohat.irLeft()
newR = robohat.irRight()
newLineL = robohat.irLeftLine()
newLineR = robohat.irRightLine()
if (newL != lastL) or (newR != lastR) or (newLineL != lastLineL) or (newLineR != lastLineR):
print 'Left, Right, LeftLine, RightLine:', newL, newR, newLineL, newLineR
print
lastL = newL
lastR = newR
lastLineL = newLineL
lastLineR = newLineR
time.sleep(0.1)
except KeyboardInterrupt:
print
# Decide left/Right
if left_distance >= right_distance:
return "Left"
else:
return "Right"
if __name__ == "__main__":
init()
try:
while True:
dist = int(robohat.getDistance())
print("Distance: ", dist)
left_ir = robohat.irLeft()
right_ir = robohat.irRight()
if not (left_ir or right_ir):
if dist > 50:
robohat.forward(settings["speed"])
print("No obstacles in range to worry about")
time.sleep(0.5)
elif dist > 10:
print("Getting close, speed is: ",
int((settings["speed"]) * float(dist / 50)))
robohat.forward(int(
(settings["speed"]) * float(dist / 50)))
time.sleep(0.5)
elif dist < 10:
print("I'm getting close, taking evasive action!")
if side_distances() == "Left":
print("Evading left!")
import robohat, time
speed = 80
robohat.init()
# main loop
try:
while True:
rightSensor = robohat.irRight()
leftSensor = robohat.irLeft()
print "right:{0}, left:{1}".format(rightSensor, leftSensor)
if rightSensor and leftSensor:
robohat.spinLeft(speed)
time.sleep(1)
elif rightSensor:
robohat.spinLeft(speed)
time.sleep(0.2)
print('Turn Left')
elif leftSensor == True:
robohat.spinRight(speed)
time.sleep(0.2)
print('Turn Right')
else:
robohat.forward(speed)
except KeyboardInterrupt:
robohat.stop()
robohat.cleanup()
import sys, time
import robohat
robohat.init()
try:
lastL = robohat.irLeft()
lastR = robohat.irRight()
lastLineL = robohat.irLeftLine()
lastLineR = robohat.irRightLine()
print 'Left, Right, LeftLine, RightLine:', lastL, lastR, lastLineL, lastLineR
print
while True:
newL = robohat.irLeft()
newR = robohat.irRight()
newLineL = robohat.irLeftLine()
newLineR = robohat.irRightLine()
if (newL != lastL) or (newR != lastR) or (newLineL != lastLineL) or (
newLineR != lastLineR):
print 'Left, Right, LeftLine, RightLine:', newL, newR, newLineL, newLineR
print
lastL = newL
lastR = newR
lastLineL = newLineL
lastLineR = newLineR
time.sleep(0.1)
except KeyboardInterrupt:
print
finally:
robohat.init()
# main loop
try:
while True:
keyp = readkey()
if robohat.irLeft():
robohat.reverse(speed)
time.sleep(1)
robohat.spinRight(speed)
time.sleep(0.5)
robohat.forward(speed)
time.sleep(1)
print 'I detected something on the left, course adjusted'
print 'My current speed is', speed
elif robohat.irRight():
robohat.reverse(speed)
time.sleep(1)
robohat.spinLeft(speed)
time.sleep(0.5)
robohat.forward(speed)
time.sleep(1)
print 'I detected something on the right, course adjusted'
print 'My current speed is', speed
elif keyp == '.' or keyp == '>':
speed = min(100, speed+5)
print 'Speed+', speed
elif keyp == ',' or keyp == '<':
speed = max (0, speed-5)
print 'Speed-', speed
elif keyp == ' ':