def return_home(): global Stop move.stop() compass_turn(NORTH_FENCE) i = float(WORLD[W_FRONT_SONAR]) while (i < 50.0): output() i = float(WORLD[W_FRONT_SONAR]) print "Getting in 50cm distance to north fence. Current distance: ", i time.sleep(.05) if (abs(WORLD[W_COMPASS] - NORTH_FENCE) > 15): compass_turn(NORTH_FENCE) move.backward() move.stop() while (i > 50.0): output() i = float(WORLD[W_FRONT_SONAR]) print "Getting in 50cm distance to north fence. Current distance: ", i time.sleep(.05) if (abs(WORLD[W_COMPASS] - NORTH_FENCE) > 15): compass_turn(NORTH_FENCE) move.forward() move.stop() compass_turn(WEST_FENCE) while (i > 15.0): output() print "Getting in 15cm distance to west fence. Current distance: ", i i = float(WORLD[W_FRONT_SONAR]) time.sleep(.05) if (abs(WORLD[W_COMPASS] - WEST_FENCE - 90) > 10): compass_turn(WEST_FENCE - 90) move.forward() move.stop() compass_turn(WEST_FENCE + 90) while i > 15.0: output() print "Getting in 50cm distance to north fence. Current distance: ", i i = float(WORLD[W_FRONT_SONAR]) time.sleep(.05) if (abs(WORLD[W_COMPASS] - WEST_FENCE + 90) > 10): compass_turn(WEST_FENCE + 90) move.forward() move.stop() compass_turn_rel(180) Stop = True
def mow(): """ The main loop in which we mow the lawn. """ global Stop blocking = False running = False #Sendeverzoegerung fuer MQTT k = 0 #Zaehler fuer die Maehdauer m = 0 while True: output() time.sleep(0.05) k = k + 1 m = m + 1 if k == 50: #update WORLD information on MQTT every 50 loops if DEBUG: print WORLD[W_FRONT_SONAR], WORLD[W_GROUND_COLOUR], blocking, m MQTT.mqttc.publish("/RPiMower/World/Polar", str(WORLD[W_MAP]), qos=0, retain=True) MQTT.mqttc.publish("/RPiMower/World/Compass", str(WORLD[W_COMPASS]), qos=0, retain=True) MQTT.mqttc.publish("/RPiMower/World/FrontUS", str(WORLD[W_FRONT_SONAR]), qos=0, retain=True) #MQTT.mqttc.publish("/RPiMower/World/BackUS", WORLD[WORLD_BACK_SONAR], qos=0, retain=True) MQTT.mqttc.publish("/RPiMower/World/GroundColour", WORLD[W_GROUND_COLOUR], qos=0, retain=True) k = 1 if WORLD[W_USERCMD] <> "": if WORLD[W_USERCMD] == "start": print "Command 'Start' received. Starting mower" move.forward() VSS.on([5]) ESC.setThrottle(8.5) WORLD[W_RUNNING] = True elif WORLD[W_USERCMD] == "stop": print "Command 'Stop' received. Stopping mower" move.stop() VSS.off([5]) ESC.setThrottle(7.5) WORLD[W_RUNNING] = False elif WORLD[W_USERCMD] == "turn": print "Usercommand 'turn' received. Turning mower 20*" move.turn(20) elif WORLD[W_USERCMD] == "return": print "Usercommand 'return' received. Return mower to home base" return_home() WORLD[W_USERCMD] = "" blocking = False; detect_blocking(WORLD[W_FRONT_SONAR]) obstacle = (float(WORLD[W_FRONT_SONAR]) < MIN_DISTANCE) off_field = False; #(WORLD[W_GROUND_COLOUR] != 'green') tilt = ((abs(WORLD[W_PITCH]) > 35) or (abs(WORLD[W_ROLL]) > 35)) if tilt: print "Stopping mower, RPiMower pitched at ", WORLD[W_PITCH] ESC.setThrottle(7.5) move.stop() if blocking or obstacle or off_field: if DEBUG: print WORLD[W_FRONT_SONAR], WORLD[W_GROUND_COLOUR], blocking print "Front obstacle detected or left the green, turning...", blocking, obstacle, off_field move.stop() VSS.off([5]) move.turn(random.uniform(-1.0,1.0)) move.forward() if m > 1000000: print "Mowing max time reached, exiting!" move.stop() return move.stop()
def mow(): """ The main loop in which we mow the lawn. """ global Stop blocking = False running = False #Sendeverzoegerung fuer MQTT k = 0 #Zaehler fuer die Maehdauer m = 0 while True: output() time.sleep(0.05) k = k + 1 m = m + 1 if k == 50: #update WORLD information on MQTT every 50 loops if DEBUG: print WORLD[W_FRONT_SONAR], WORLD[W_GROUND_COLOUR], blocking, m MQTT.mqttc.publish("/RPiMower/World/Polar", str(WORLD[W_MAP]), qos=0, retain=True) MQTT.mqttc.publish("/RPiMower/World/Compass", str(WORLD[W_COMPASS]), qos=0, retain=True) MQTT.mqttc.publish("/RPiMower/World/FrontUS", str(WORLD[W_FRONT_SONAR]), qos=0, retain=True) #MQTT.mqttc.publish("/RPiMower/World/BackUS", WORLD[WORLD_BACK_SONAR], qos=0, retain=True) MQTT.mqttc.publish("/RPiMower/World/GroundColour", WORLD[W_GROUND_COLOUR], qos=0, retain=True) k = 1 if WORLD[W_USERCMD] <> "": if WORLD[W_USERCMD] == "start": print "Command 'Start' received. Starting mower" move.forward() VSS.on([5]) ESC.setThrottle(8.5) WORLD[W_RUNNING] = True elif WORLD[W_USERCMD] == "stop": print "Command 'Stop' received. Stopping mower" move.stop() VSS.off([5]) ESC.setThrottle(7.5) WORLD[W_RUNNING] = False elif WORLD[W_USERCMD] == "turn": print "Usercommand 'turn' received. Turning mower 20*" move.turn(20) elif WORLD[W_USERCMD] == "return": print "Usercommand 'return' received. Return mower to home base" return_home() WORLD[W_USERCMD] = "" blocking = False detect_blocking(WORLD[W_FRONT_SONAR]) obstacle = (float(WORLD[W_FRONT_SONAR]) < MIN_DISTANCE) off_field = False #(WORLD[W_GROUND_COLOUR] != 'green') tilt = ((abs(WORLD[W_PITCH]) > 35) or (abs(WORLD[W_ROLL]) > 35)) if tilt: print "Stopping mower, RPiMower pitched at ", WORLD[W_PITCH] ESC.setThrottle(7.5) move.stop() if blocking or obstacle or off_field: if DEBUG: print WORLD[W_FRONT_SONAR], WORLD[W_GROUND_COLOUR], blocking print "Front obstacle detected or left the green, turning...", blocking, obstacle, off_field move.stop() VSS.off([5]) move.turn(random.uniform(-1.0, 1.0)) move.forward() if m > 1000000: print "Mowing max time reached, exiting!" move.stop() return move.stop()