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 == ' ':