def charge_and_swerve_F_minus(R):
center_marker = 32
if R.zone == 0: # Zone A
close_marker = 33
far_marker = 34
else: # Zone B
close_marker = 34
far_marker = 33
move.forward_dist(R, 0.9)
move.slowly_stop_all_motors(R)
move.scoop_left(R, 130)
can_see_marker = False
time.sleep(3)
markers = R.see()
print "I see", len(markers)
for marker in markers:
print marker.info.code, marker.dist
if marker.info.code == far_marker: # The marker in the opposite zone
# move towards marker
move.forward(R, marker.dist)
can_see_marker = True
if not can_see_marker:
print("Cannot see")
def square():
#18.6046511628
current = (0, 0, 0)
for i in range(0, 4):
for j in range(4):
forward(10)
previous_pos = current
particles = generate_particles_from_movement(current, 10)
avgX = sum([x for (x, y, theta) in particles]) / 100
avgY = sum([y for (x, y, theta) in particles]) / 100
avgTheta = sum([theta for (x, y, theta) in particles]) / 100
current = (avgX, avgY, avgTheta)
line = (previous_pos[0], previous_pos[1], avgX, avgY)
#plot the points
print("drawLine:" + str(line))
print("drawParticles:" + str(particles))
print("###########################WENT FORWARD#################")
time.sleep(0.2)
left(90)
print("##############TURNED LEFT######################")
particles = generate_particles_from_turn(current, 90)
#plot the new points
print("drawParticles:" + str(particles))
time.sleep(0.1)
def directions(instructions):
if instructions[0] == "Forward":
move.forward()
elif instructions[0] == "Left":
move.left()
elif instructions[0] == "Right":
move.right()
elif instructions[0] == "Standing":
move.stand()
elif instructions[0] == "Auto":
move.autonom()
def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
T = 15000.0 # max simulation
goal_dis = 0.2
stop_speed = 0.1
state = modelito.State(x=0.15, y=0.15, yaw=6, v=0.0)
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
while T >= time:
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
ai = PIDControl(speed_profile[target_ind], state.v)
state = modelito.update(state, ai, di)
if abs(state.v) <= stop_speed:
target_ind += 1
time = time + modelito.dt
dx = state.x - goal[0]
dy = state.y - goal[1]
if math.sqrt(dx**2 + dy**2) <= goal_dis:
print("Logrado")
break
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
move.forward(100 * abs(state.v),
100 * abs(state.v * np.cos(state.yaw)))
t.append(time)
return t, x, y, yaw, v
def navigateToWaypoint(X, Y, current): #X is desired X,Y is desired Y
#assuming we have access to our x,y,theta values (position and direction of robot)
#take dY = Y-y;dX = X-x
#we need to turn (phi - theta) degrees with phi = atan2(dY,dX).
#then move forward a distance of sqrt(pow(dY,2)+pow(dX,2))
dY = Y - current[1]
dX = X - current[0]
print(dY)
print(dX)
phi = math.atan2(dY, dX)
dist = math.sqrt(math.pow(dY, 2) + math.pow(dX, 2))
if dX > 0:
angle = phi - current[2] #align with point if dX +ve
else:
angle = phi - (current[2] - math.pi) #offset by pi if dX -ve
print(angle)
print(dist)
right(angle)
forward(dist) #idk if this is how it works in python
current[0] = current[0] + dX
current[1] = current[1] + dY
current[3] = current[3] + phi
interface = brickpi.Interface()
interface.initialize()
speed = -6.0
motors = [0,3]
interface.motorEnable(motors[0])
interface.motorEnable(motors[1])
motorParams = interface.MotorAngleControllerParameters()
motorParams.maxRotationAcceleration = 6.0
motorParams.maxRotationSpeed = 12.0
motorParams.feedForwardGain = 255/20.0
motorParams.minPWM = 18.0
motorParams.pidParameters.minOutput = -255
motorParams.pidParameters.maxOutput = 255
motorParams.pidParameters.k_p = 540
motorParams.pidParameters.k_i = 2000
motorParams.pidParameters.K_d = 34
interface.setMotorAngleControllerParameters(motors[0], motorParams)
interface.setMotorAngleControllerParameters(motors[1], motorParams)
forward(30)
interface.setMotorRotationSpeedReferences(motors,[speed,speed])
while not interface.motorAngleReferencesReached(motors) :
time.sleep(0.1)
def move_asynch(chosen_path, state):
global DUMPED, NEXT_NODE
instruction = None
# Calling stop on this thread throws ThreadKiller at the current point of
# execution, wrapping all of the logic in a try catch allows for cleanup and
# prevents the stack trace.
try:
while True:
# Get the next instruction
instruction = chosen_path.pop(0)
success = True
# Dispatch to the relavent functions depending on the command
if isinstance(instruction, Move):
#print("moving")
success = forward(instruction.dist, tolerance = instruction.tolerance)
elif isinstance(instruction, Dump):
# Communication between the two bricks was turned off during
# profiling
if not PROFILING:
# Instruct the second brick to dump
CLIENT.publish("dump", json.dumps(instruction.slots))
# Spin until it replies
while True:
with dumped_lock:
if DUMPED:
DUMPED = False
break
elif isinstance(instruction, Rotate):
#print("rotating")
# The route planner always generates instructions telling the
# robot to turn right, this adapts to turn left when that is
# more efficient
if instruction.angle <= 180:
direction = Directions.ROT_RIGHT
angle = instruction.angle
else:
direction = Directions.ROT_LEFT
angle = instruction.angle - 180
success = rotate(angle, tolerance = instruction.tolerance, direction = direction)
elif isinstance(instruction, ToDesk):
#print("approaching desk")
angle = instruction.angle
if instruction.is_left:
direction = Directions.ROT_LEFT
else:
direction = Directions.ROT_RIGHT
# This function unconditionally turns 90 degrees and so can't
# fail
approach(angle=angle, direction=direction)
elif isinstance(instruction, FromDesk):
#print("leaving desk")
angle = instruction.angle
if instruction.is_left:
direction = Directions.ROT_LEFT
else:
direction = Directions.ROT_RIGHT
# On the way back however it searches for a line so can
success = approach(angle=angle, tolerance=instruction.tolerance, direction=direction, reverse=True)
elif isinstance(instruction, Report):
#print("reporting")
CLIENT.publish("location_info", payload=instruction.where)
# If an instruction failed panic
if not success:
#print("panicking")
STATE_QUEUE.put(T_PANICKING)
break
# If we ran out of instructions without panicking make a transition
# depending on what state we are currently in
if len(chosen_path) == 0:
if state == State.DELIVERING:
#print("Returning")
STATE_QUEUE.put(T_RETURNING)
#print(STATE_QUEUE)
break
elif state == State.RETURNING:
#print("Loading")
STATE_QUEUE.put(T_LOADING)
break
# Last reported location for return
with next_node_lock:
if isinstance(instruction, Report):
NEXT_NODE = instruction.where
#print(NEXT_NODE)
# TODO right now the code spins here forever after executing the movement
# commands - does not need to
while True:
pass
except ThreadKiller as e:
# There is a certain amount of unreliability in the method used to kill
# threads (See thread_decorator.py) so the code keeps sending exceptions
# until the thread dies or calls acknowledge. After a call to
# acklowledge the thread is allowed as much time as it needs to cleanup.
acknowledge(e)
stop_motors()
final = []
# Resolve the various instructions where we could have been interupted
if isinstance(instruction, Move):
# Inside move figure out how far we still need to go and store that
# for next time. (If it is too small search for the junction
# anywhere between 0 and 20)
dist = instruction.dist - get_odometry()
if dist <= 20:
final = [Move(20, 100)]
else:
final = [Move(dist, 50)]
# If there is a rotate next also take that
if chosen_path and isinstance(chosen_path[0], Rotate):
final.append(chosen_path.pop(0))
elif isinstance(instruction, Dump):
# If it's a dump assume we've managed to send the message (It's
# highly unlikley we will be interupted exactly between poping a
# Dump and sending the message). Wait for the confirmation before
# finishing
while True:
with dumped_lock:
if DUMPED:
DUMPED = False
break
elif isinstance(instruction, Rotate):
# Rotate follows the same theory as move while also dealing with
# whether we were turning left or right
if instruction.angle <= 180:
angle = instruction.angle - get_odometry(rotating=True)
if angle <= 20:
final = [Rotate(20, 100)]
else:
final = [Rotate(angle, 50)]
else:
angle = instruction.angle + get_odometry(rotating=True)
if angle >= 340:
final = [Rotate(340, 100)]
else:
final = [Rotate(angle, 50)]
elif isinstance(instruction, FromDesk):
# Same as move and rotate
final = [FromDesk(instruction.is_left, instruction.angle - get_odometry(rotating=True), 50)]
elif isinstance(instruction, ToDesk):
# Here also dispense the letter given we are at a desk
final = [ToDesk(instruction.is_left, instruction.angle - get_odometry(rotating=True)),
chosen_path.pop(0), chosen_path.pop(0)] # atm it dispenses the letter even after recall
# Save the generated command
with final_cmd_lock:
global FINAL_CMD
FINAL_CMD = final
# Save the current path (Will be used for resume)
with chosen_path_lock:
global CHOSEN_PATH
CHOSEN_PATH = chosen_path
# Store the node we are going to reach next (Will be used for callback)
with next_node_lock:
for instructione in chosen_path:
if isinstance(instructione, Report):
NEXT_NODE = instructione.where
break
sys.exit()
if left_result and right_result:
left_touched = left_result[0]
right_touched = right_result[0]
if left_touched and right_touched:
print "front"
interface.setMotorPwm(motors[0],0)
interface.setMotorPwm(motors[1],0)
# while not interface.motorRotationSpeedReferenceReached(0):
# time.sleep(0.1)
time.sleep(1)
interface.motorDisable(0)
interface.motorDisable(3)
interface.motorEnable(0)
interface.motorEnable(3)
time.sleep(1)
print("stopped")
forward(-10)
time.sleep(0.5)
left(90)
time.sleep(0.5)
elif left_touched:
print "left"
elif right_touched:
print "right"
interface.terminate()
#!/usr/bin/env python3 import move import Directions move.forward(500).join() #move.rotate(360, Directions.ROT_LEFT)
line_sensor.wait_for_line(seek_time)
if line_sensor.value < 0.5:
ret = True
break
else:
direction = not direction
if direction:
seek_count += 1
continue
return ret
parser.add_option("-s", "--speed", type="float", dest="speed")
(options, args) = parser.parse_args()
if options.speed != None:
speed = options.speed
line_sensor.when_line = lambda: move.forward()
line_sensor.when_no_line = lambda: move.stop()
signal.signal(signal.SIGINT, exit_gracefully)
while True:
move.forward(speed)
line_sensor.wait_for_no_line()
if not seek_line():
print("Can't find any line")
break
exit_gracefully()
#!/usr/bin/env python3
import move
import signal
from gpiozero import DistanceSensor
min_distance = 0.15
sonic_sensor = DistanceSensor(
echo=18, trigger=17, threshold_distance=min_distance)
speed = 0.5
def exit_gracefully():
exit()
sonic_sensor.when_in_range = lambda: move.stop()
sonic_sensor.when_out_of_range = lambda: move.forward()
signal.signal(signal.SIGINT, exit_gracefully)
move.forward(speed)
while True:
sonic_sensor.wait_for_in_range()
move.backward(speed)
sonic_sensor.wait_for_out_of_range()
move.right(speed, 0.75)
exit_gracefully()