示例#1
0
def square(side=None):
    if side == None:
        side = 40
    for i in range(4):
        motor_params.forward(side)
        motor_params.Left90deg()
        time.sleep(0.5)
示例#2
0
def uncertainNavigate(state, dest):
    """
    Arguments: 
        state - of type State
        dest - absolute orientation
    """
    goal_theta = math.atan2(dest.y - state.y, dest.x - state.x)
    dist = math.sqrt((dest.y - state.y) ** 2 + (dest.x - state.x) ** 2)
    delta_theta_rad = goal_theta - state.theta
    if delta_theta_rad > math.pi:
        delta_theta_rad -= 2*math.pi
    elif delta_theta_rad < -math.pi:
        delta_theta_rad += 2*math.pi  
    motor_params.rotate(delta_theta_rad / math.pi * 180.0)
    state = state.rotate(delta_theta_rad)
    motor_params.forward(dist)
    return state.move_forward(dist)
def navigateToWaypoint(state, dest):
    """
    NOTE: USE THIS ONLY FOR MCL
    Arguments: 
        state - of type State
        dest - of type walls.Point
    """
    goal_theta = math.atan2(dest.y - state.y, dest.x - state.x)
    delta_theta_rad = goal_theta - state.theta
    if delta_theta_rad > math.pi:
        delta_theta_rad -= 2 * math.pi
    elif delta_theta_rad < -math.pi:
        delta_theta_rad += 2 * math.pi
    motor_params.rotate(delta_theta_rad / math.pi * 180.0)
    state = state.rotate(delta_theta_rad)
    if (delta_theta_rad / math.pi *
            180) > 45 or delta_theta_rad / math.pi * 180 < -45:
        return state
    dist = min(20.0, math.sqrt((state.y - dest.y)**2 + (state.x - dest.x)**2))
    motor_params.forward(dist)
    return state.move_forward(dist)
示例#4
0
def calibrate():
    kp_values = range(100, 1000, 50)

    for i, kp in enumerate(kp_values):
        print "kp =", kp
        interface.startLogging("plot/new_pid_logs/kp_%d.log" % int(kp))
        left_motor_params = interface.MotorAngleControllerParameters()
        left_motor_params.maxRotationAcceleration = 8.0
        left_motor_params.maxRotationSpeed = 12.0
        left_motor_params.feedForwardGain = 255 / 20.0
        left_motor_params.minPWM = 18.0
        left_motor_params.pidParameters.minOutput = -255
        left_motor_params.pidParameters.maxOutput = 255
        left_motor_params.pidParameters.k_p = kp
        left_motor_params.pidParameters.k_i = 0
        left_motor_params.pidParameters.K_d = 0
        interface.setMotorAngleControllerParameters(motor_params.MOTOR_LEFT,
                                                    left_motor_params)

        right_motor_params = interface.MotorAngleControllerParameters()
        right_motor_params.maxRotationAcceleration = 8.0
        right_motor_params.maxRotationSpeed = 12.0
        right_motor_params.feedForwardGain = 255 / 20.0
        right_motor_params.minPWM = 18.0
        right_motor_params.pidParameters.minOutput = -255
        right_motor_params.pidParameters.maxOutput = 255
        right_motor_params.pidParameters.k_p = kp
        right_motor_params.pidParameters.k_i = 0
        right_motor_params.pidParameters.K_d = 0
        interface.setMotorAngleControllerParameters(motor_params.MOTOR_RIGHT,
                                                    right_motor_params)

        # Choose your action here.
        motor_params.forward(50)
        motor_params.forward(-50)
        # End of choose action.

        interface.stopLogging()
    interface.terminate()
示例#5
0
def test_performance():
    for sig_point in SIGNATURE_POINTS:

        ls_normal = rot_sensor.takeSignature(sig_point.rstart, sig_point.rend,
                                             sig_point)
        ls_normal.save(sig_point, NORMAL_DIR)

        raw_input("Place a bottle somewhere and press enter")

        ls_bottle = rot_sensor.takeSignature(sig_point.rstart, sig_point.rend,
                                             sig_point)
        ls_bottle.save(sig_point, BOTTLE_DIR)

        bottle_loc = get_bottle_belief(ls_normal, ls_bottle, sig_point)

        print bottle_loc

        motor_params.rotate(bottle_loc.angle)
        motor_params.forward(bottle_loc.distance)

        raw_input(
            "Place the robot in a the next signature point for a new test and press enter"
        )
示例#6
0
        [(200 + x * DISTANCE_TO_PIXEL, 200 + y * DISTANCE_TO_PIXEL, t)
         for x, y, t in particles])


def update_particles_straight(d):
    for i in range(len(particles)):
        x, y, theta = particles[i]
        e = random.gauss(0, 0.07)
        f = random.gauss(0, 0.25 * math.pi / 180.0)
        particles[i] = (x + (d + e) * math.cos(theta),
                        y + (d + e) * math.sin(theta), theta + f)


def update_particles_rotation(alpha):
    for i in range(len(particles)):
        g = random.gauss(0, 2.0) * math.pi / 180.0
        x, y, theta = particles[i]
        particles[i] = (x, y, theta + alpha + g)


for __ in range(4):
    draw_particles()
    for _ in range(4):
        motor_params.forward(10)
        update_particles_straight(10)
        draw_particles()

    motor_params.Left90deg()
    update_particles_rotation(math.pi / 2)
    draw_particles()
示例#7
0
def main():

    walls.wallmap.draw()

    state = motion_predict.State(
            particles=[motion_predict.Particle(
                    x=BOTTLES[3][1][0].x, y=BOTTLES[3][1][0].y, theta=0)] * NUMBER_OF_PARTICLES,
            weights=[1.0 / NUMBER_OF_PARTICLES
                     for _ in range(NUMBER_OF_PARTICLES)])
    mcl.draw_particles(state)
    raw_input("Click enter to begin")
    
    # visitpoints is a list of points that we need to visit
    for key, visitpoints in BOTTLES:
        # print "Going into key = ", key
        # print "Entering state is ", state
        # mcl_points is a list of lists
        area_mcl_points = MCL_POINTS[key]

        # Bottles
        if key != "FINAL":
            # waypoint refers to the next destination
            for waypoint, mcl_points in zip(visitpoints, area_mcl_points):
                distance = 0.0
                # Navigate properly
                for i in range(1,3):
                #while True:
                    x_is_close = abs(state.x - waypoint.x) <= WAYPOINT_MIN_OFFSET
                    y_is_close = abs(state.y - waypoint.y) <= WAYPOINT_MIN_OFFSET
    
                    if x_is_close and y_is_close:
                        # We have reached our destination
                        break
                    else:
                        # TO DO: Smart navigation with not many rotations
                        # print "state = ", state
                        state = uncertainNavigate(state, waypoint)
                        # print "Navigating to ", waypoint
                        # Run MCL
                        if key!= "A":
                            if i != 2:
                                state = sigPointMCLStep(state, mcl_points)
                        # print "CURRENT STATE: x=%f, y =%f, theta=%f" % (state.x, state.y, state.theta)
    
                # Make sure your orientation is the same as the orientation a signature must be taken at 
                state = uncertainRotate(state, waypoint)

                # Compare signatures
                bottle_loc = get_bottle(waypoint)

                # We did not find a bottle, go to the next waypoint
                # TO DO: What to do if we went to all waypoints and we still did not hit a bottle
                if bottle_loc is None:
                    # print "BOTTLE NOT DETECTED"
                    continue
                # We have a possible bottle location
                else:
                    # print "bottle_loc = ", bottle_loc
                    # 1. Try navigating to the bottle.
                    motor_params.rotate(bottle_loc.angle)
                    state = state.rotate(math.radians(bottle_loc.angle))
                    # print "State right before moving towards bottle:"
                    # print state
                    nearest_wall_dist = walls.getWallDist(
                            motion_predict.Particle(x=state.x, y=state.y, theta=state.theta),
                            incidence_angle=False)
                    if nearest_wall_dist == ultrasound.GARBAGE:
                        overshoot = 5.0
                    else:
                        # bottle.distance + overshoot == nearest_wall_dist - 10.0
                        overshoot = nearest_wall_dist - 10.0 - bottle_loc.distance

                    distance, hit_bottle = motor_params.slow_down_forward(
                            bottle_loc.distance,
                            place_rec.bump_termination_callback,
                            overshoot=overshoot)
                    # Don't perform MCL here, we are fairly sure that it will
                    # screw up. (Due to the bottle).
                    state = state.move_forward(distance)

                    if hit_bottle:
                        # print "State right after touching bottle:"
                        # print state
                        # 2. If we hit the bottle, we will want to reverse.
                        motor_params.forward(-distance * 0.8)
                        state = state.move_forward(-distance * 0.8)
                        # print "State right after reversing from bottle:"
                        # print state

                        # 3. Break if we hit the bottle. Then we go on to
                        #    handle the next bottle area.
                        state = sigPointMCLStep(state, mcl_points)
                        # print "State right after performing MCL"
                        # print state
                        break
                    else:
                        # 4. if we did not hit a bottle, we continue the loop.
                        #    Going to the next waypoint in the area.
                        motor_params.forward(-distance * 0.8)
                        state = state.move_forward(-distance * 0.8)
                        continue
        # Final endpoint
        else:
            waypoint = visitpoints[0]
            mcl_points = area_mcl_points[0]

            # Navigate properly
            for i in range(1,3):
            #while True:
                x_is_close = abs(state.x - waypoint.x) <= FINAL_WAYPOINT_MIN_OFFSET
                y_is_close = abs(state.y - waypoint.y) <= FINAL_WAYPOINT_MIN_OFFSET
    
                if x_is_close and y_is_close:
                    # We have reached our destination
                    break
                else:
                    # TO DO: Make sure you are tunning MCL facing to the proper walls
                    state = uncertainNavigate(state, waypoint)
                    state = sigPointMCLStep(state, mcl_points)
                    print "CURRENT STATE: x=%f, y =%f, theta=%f" % (state.x, state.y, state.theta)
示例#8
0
import argparse
import contextlib
import time
import sys
import math

import brickpi
import motor_params


#cmd_parser.add_argument('k_p', type=float, help='P gain')
interface = motor_params.interface#args = cmd_parser.parse_args()
motors = motor_params.motors

motor_params.forward(40*40/39.5)
motor_params.forward(-40*40/39.5)
motor_params.Left90deg()
time.sleep(0.5)
motor_params.Right90deg()


示例#9
0
def rot_angle_calibrate(angle, dist=None):
    if dist == None:
        dist = 10
    motor_params.forward(dist)
    motor_params.TurnOpposite(angle)
    motor_params.forward(dist)
示例#10
0
def rot_angle(angle, dist=None):
    if dist == None:
        dist = 10
    motor_params.forward(dist)
    motor_params.Right90deg()
    motor_params.forward(dist)
示例#11
0
def test_angle(angle):
    motor_params.forward(10)
    #rotate(angle)
    motor_params.TurnOpposite(angle)
    motor_params.forward(10)
示例#12
0
import motor_params as mp

angle = (360.0 / 332.5) * 180.0
distance = (40.0 / 38.84) * 40

mp.forward(distance)
mp.rotate(angle)
mp.forward(distance)
mp.rotate(angle)