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 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()
