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)
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)
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()
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"
)
[(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()
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)
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()
def rot_angle_calibrate(angle, dist=None):
if dist == None:
dist = 10
motor_params.forward(dist)
motor_params.TurnOpposite(angle)
motor_params.forward(dist)
def rot_angle(angle, dist=None):
if dist == None:
dist = 10
motor_params.forward(dist)
motor_params.Right90deg()
motor_params.forward(dist)
def test_angle(angle):
motor_params.forward(10)
#rotate(angle)
motor_params.TurnOpposite(angle)
motor_params.forward(10)
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)