def build_map():
global WORLD
move.stop()
# wait for the first real compass result
time.sleep(2)
current_angle = WORLD[W_COMPASS]
print "Starting at angle: ", current_angle
for angle in range(current_angle, current_angle + 359, 10):
if angle > 360:
target = angle - 360
else:
target = angle
print "Turning to angle: ", target
compass_turn(target)
output()
print "World Map: ", WORLD[W_COMPASS], WORLD[W_FRONT_SONAR]
WORLD[W_MAP].append(
[WORLD[W_COMPASS],
float(WORLD[W_FRONT_SONAR]) + 47])
#print WORLD[W_MAP]
WORLD_CARTESIAN = [[
int(np.cos(np.radians(i[0])) * float(i[1]) * 10) / 10,
int(np.sin(np.radians(i[0])) * float(i[1]) * 10) / 10
] for i in WORLD[W_MAP]]
#print WORLD_CARTESIAN
MQTT.mqttc.publish("/RPiMower/World/Cartesian", str(WORLD_CARTESIAN))
move.stop()
WORLD[W_USERCMD] = "start"
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 compass_turn(target):
DC = 20
while abs(target - WORLD[W_COMPASS]) > 20:
output()
print "from - to: ", WORLD[W_COMPASS], target
if WORLD[W_COMPASS] < target:
if abs(WORLD[W_COMPASS] - target)<180:
#Rechtsrum ist kuerzester Weg
move.right180(DC, DC)
else:
#Linksrum ist kuerzester Weg
move.left180(DC, DC)
else:
if abs(WORLD[W_COMPASS] - target)<180:
#Linksrum ist kuerzester Weg
move.left180(DC, DC)
else:
#Rechtsrum ist kuerzester Weg
move.right180(DC, DC)
time.sleep(0.02)
move.stop()
time.sleep(0.02)
def compass_turn(target):
DC = 20
while abs(target - WORLD[W_COMPASS]) > 20:
output()
print "from - to: ", WORLD[W_COMPASS], target
if WORLD[W_COMPASS] < target:
if abs(WORLD[W_COMPASS] - target) < 180:
#Rechtsrum ist kuerzester Weg
move.right180(DC, DC)
else:
#Linksrum ist kuerzester Weg
move.left180(DC, DC)
else:
if abs(WORLD[W_COMPASS] - target) < 180:
#Linksrum ist kuerzester Weg
move.left180(DC, DC)
else:
#Rechtsrum ist kuerzester Weg
move.right180(DC, DC)
time.sleep(0.02)
move.stop()
time.sleep(0.02)
def build_map():
global WORLD
move.stop()
# wait for the first real compass result
time.sleep(2)
current_angle = WORLD[W_COMPASS]
print "Starting at angle: ", current_angle
for angle in range(current_angle, current_angle + 359, 10):
if angle > 360:
target = angle - 360
else:
target = angle
print "Turning to angle: ", target
compass_turn(target)
output()
print "World Map: ", WORLD[W_COMPASS], WORLD[W_FRONT_SONAR]
WORLD[W_MAP].append([WORLD[W_COMPASS], float(WORLD[W_FRONT_SONAR])+47])
#print WORLD[W_MAP]
WORLD_CARTESIAN = [[int(np.cos(np.radians(i[0]))*float(i[1])*10)/10, int(np.sin(np.radians(i[0]))*float(i[1])*10)/10] for i in WORLD[W_MAP]]
#print WORLD_CARTESIAN
MQTT.mqttc.publish("/RPiMower/World/Cartesian", str(WORLD_CARTESIAN))
move.stop()
WORLD[W_USERCMD]="start"
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()