Пример #1
0
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"
Пример #2
0
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
Пример #3
0
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
Пример #4
0
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)
Пример #5
0
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)
Пример #6
0
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"
Пример #7
0
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()
Пример #8
0
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()