def compute(self):
for i in xrange(self.numButtons):
self.buttons[i] = self.joystick.get_button(i)
self.buttonpressed[
i] = self.buttons[i] and not self.last_buttons[i]
for i in xrange(self.numAxes):
self.axes[i] = self.joystick.get_axis(i)
self.last_buttons = list(self.buttons)
# for i in [0,1,3]:
# if abs(self.axes[i]) < 0.05:
# self.axes[i] = 0
self.x = self.axes[3]
self.y = self.axes[1]
self.rot = self.axes[0]
self.rad = math.hypot(self.x, self.y)
self.rad = limitToRange(self.rad, 0, 1)
self.ang = math.atan2(self.y, self.x)
self.x = self.rad * math.cos(self.ang)
self.y = self.rad * math.sin(self.ang)
if self.buttonpressed[0]:
if self.arm_mode:
self.arm_mode = False
self.joyserver.arm_park(2)
else:
self.arm_mode = True
self.joyserver.move(0, 0, 0)
self.arm_pos = DEF_ARM_POS
if self.buttonpressed[1]:
if self.arm_mode:
self.arm_mode = False
else:
self.arm_mode = True
self.joyserver.move(0, 0, 0)
if self.arm_mode:
prev_arm_pos = deepcopy(self.arm_pos)
self.arm_pos += self.axes[4] * Vec3d(0, 0, -ARM_SPEED) / RATE
self.arm_pos += self.axes[1] * Vec3d(0, -ARM_SPEED, 0) / RATE
self.arm_pos += self.axes[0] * Vec3d(ARM_SPEED, 0, 0) / RATE
try:
self.joyserver.arm_set_pos(self.arm_pos,
-.2 * (self.axes[2] + 1), 180)
except (ValueError, AssertionError):
# If we tried to set the arm to a position that it can't reach
self.arm_pos = prev_arm_pos
else:
heading = self.ang + math.pi / 2
if heading > math.pi:
heading -= 2 * math.pi
heading *= 57.2957795
heading = math.floor(heading + 0.5)
heading *= -1
self.joyserver.move(self.rad, heading, self.rot)
print 'data', (self.rad, heading, -self.rot)
self.joyserver.set_suction(int(self.axes[5] * 90 + 90))
print 'data', self.buttons, self.axes
def detect_blocks(self, level):
bd = block_detector(self.s)
# First move away from rails
self.move_to(Vec3d(11, -1, 10), 0, 180)
self.move_to(Vec3d(-3.5, 4, 17), 0.04 * 3.14, 180)
bd.grab_left_frame()
self.move_to(Vec3d(3, 4, 17), 0.04 * 3.14, 180)
bd.grab_right_frame()
istop = level == 'top'
return bd.get_blocks(top=istop, display=False)
def arm_move_then_park():
# right/left, forward, height
# right side
arm.move_to(Vec3d(11, -1, 10), 0, 180)
arm.move_to(Vec3d(0, 10, 10), 0, 180)
time.sleep(3)
arm.move_to(Vec3d(0, 10, 10), 3.14 / 6, 180)
arm.move_to(Vec3d(0, 10, 10), -3.14 / 6, 180)
arm.move_to(Vec3d(0, 10, 10), 0, 0)
arm.move_to(Vec3d(0, 10, 10), 0, 180)
arm.move_to(Vec3d(11, -1, 10), 0, 180)
arm.park()
def handle_commands(s, arm, line):
# Read input as json
obj = json.loads(line)
if "arm" in obj:
# If command is arm, move arm to requested location
try:
arm.move_to(
Vec3d(obj.get("arm")[0],
obj.get("arm")[1],
obj.get("arm")[2]),
obj.get("arm")[3] * 3.14, 0)
except ValueError:
# If a ValueError exception is thrown, nothing is done.
print "Out of reach"
except AssertionError:
# If a AssertionError exception is thrown, then turn remote off
print "Out of reach, closing remote"
wsServerRead.terminate()
wsServerWrite.terminate()
elif "suction" in obj:
# If command is suction, either turn the suction on or off
if obj.get("suction"):
s.set_release_suction(False)
s.set_suction(True)
else:
s.set_suction(False)
s.set_release_suction(True)
elif "move" in obj:
# If command is move, move the robot in the desired direction
# Assume max velocity
# params: speed 0 <= speed <= 1
# params: direction -180 <= dir <= 180
# params: angular (default to straight ahead)
s.move_pid(obj.get("move")[0], obj.get("move")[1], 0)
elif "stop" in obj:
# tell robot to stop
s.move_pid(0, 0, 0)
elif "rotate" in obj:
# deal with turn commands
# give it a velocity of 0
s.move_pid(0, 0, obj.get("rotate"))
else:
print "invalid command", obj
def drop_block(self, **kwargs):
rail_drop = kwargs.get('rail', False)
side = kwargs.get('side', 'right')
extend_wrist = rail_drop
if extend_wrist:
wrist = pi / 2
else:
wrist = 0
self.arm.move_to(
Vec3d(DROP_XY[side][0], DROP_XY[side][1], HEIGHT_TO_CLEAR_LOADER),
wrist, 180, 0.75)
if rail_drop:
self.arm.move_to(
Vec3d(RAIL_DROP_XY[side][0], RAIL_DROP_XY[side][1],
HEIGHT_TO_DROP_RAIL), wrist, 180, SPEED)
self.s.set_suction(False)
self.s.set_release_suction(True)
time.sleep(0.15)
self.s.set_release_suction(False)
# To clear rails before returning
if rail_drop:
self.arm.move_to(
Vec3d(DROP_XY[side][0], DROP_XY[side][1],
HEIGHT_TO_CLEAR_LOADER), wrist, 180, FAST_SPEED)
def to_servos(cuppos, wrist, wristrotate):
wristpos = cuppos + Vec3d(0, 0, wristToCup)
rot = revkin(wristpos)
wrist += -pi / 2
# If positive wrist flexes up, positive shoulder and elbow rotations will
# add directly to wrist
wrist += rot[1] + rot[2]
servo = [
base_r2p(rot[0]),
shoulder_r2p(rot[1]),
elbow_r2p(rot[2]),
wrist_r2p(wrist), wristrotate
]
servo = [max(int(round(p)), 0) for p in servo]
# print servo
return servo
def start(self):
self.ldr.initial_zero_lift(use_widen=False)
arm.move_to(Vec3d(11, -1, 10), 0, 180)
# arm.move_to(Vec3d(0, 10, 10), 0, 180)
'''
for loc in['near_half', 'far_half']:
self.bp.pick_block(0, 'top', loc)
self.bp.drop_block(side='right', rail=False)
'''
# self.bp.pick_block(2, 'top', 'far_half')
# self.bp.drop_block_right()
# self.bp.pick_block(3, 'top', 'near_half')
# self.bp.drop_block_right()
# self.bp.pick_block(3, 'top', 'far_half')
# self.bp.drop_block_right()
# self.bp.pick_block(0, 'top', 'full')
# self.bp.drop_block_right()
# self.bp.pick_block(7, 'top', 'far_half')
# self.bp.drop_block_right()
# self.bp.pick_block(0, 'top', 'near_half')
# self.bp.drop_block_right()
# self.bp.pick_block(0, 'top', 'far_half')
# self.bp.drop_block_right()
# self.bp.pick_block(7, 'top', 'near_half')
# self.bp.drop_block_right()
# self.bp.pick_block(7, 'top', 'far_half')
# self.bp.drop_block_right()
# '''
# for level in ['bottom']:
for level in ['top', 'bottom']:
for col in range(8):
self.bp.pick_block(col, level)
self.bp.drop_block(side='right', rail=False)
# '''
'''
self.ldr.lift(1.5)
for level in ['top']:
for col in range(8):
self.bp.pick_block(col, level)
self.bp.drop_block(side='right', rail=False)
'''
logger.info("Done!")
def __init__(self):
# assert not s.read_start_switch()
self.ldr = Loader(s)
self.rs = RailSorter(s, arm)
# flag to determine if the loader is enabled
# this allows for a pure navigational run when set to False
self.use_loader = True
# set a threshold for white vs black values from the QTR sensor
self.qtr_threshold = 800
# These will be set after start switch press
self.course = None
self.dir_mod = None
# Initialize before button press
self.ldr.initial_zero_lift(use_widen=False)
self.ldr.lift(1.9)
arm.move_to(Vec3d(11, -1, 10), 0, 180)
# Basically run reset.sh here, remember to zero extend and widen encoders
# python utils/loader_control.py --compress=4
s.set_width_motor(int(100 * 1.5), 'ccw')
time.sleep(6)
s.stop_width_motor()
# python utils/loader_control.py --retract=3 --power=130
s.set_loader_motor(0, int(100 * 3 / 4), 'bw')
s.set_loader_motor(1, 100, 'bw')
time.sleep(5)
s.stop_loader_motor(0)
s.stop_loader_motor(1)
for enc_id in range(3):
s.zero_loader_encoder(enc_id)
arm.park()
self.ldr.close_flaps()
from head.spine.arm import get_arm
from head.spine.Vec3d import Vec3d
from head.spine.ourlogging import setup_logging
setup_logging(__file__)
logger = logging.getLogger(__name__)
with get_spine() as s:
with get_arm(s) as arm:
ldr = Loader(s)
# Initialize before button press
ldr.initial_zero_lift()
ldr.lift(1.9)
import IPython
IPython.embed()
arm.move_to(Vec3d(11, -1, 10), 0, 180)
ldr.widen(0.1)
arm.park()
# raw_input('press enter')
'''
# Before colliding with barge
ldr.widen(1)
ldr.lift(0.2)
raw_input('press enter')
# After lining up
# arm.move_to(Vec3d(11, -1, 10), 0, 180)
# ldr.widen(0.1)
ldr.lift(1.8)
raw_input('press enter')
def park_arm(self):
arm.move_to(Vec3d(11, -4, 10), 1.3, 180)
arm.park()
def arm_to_vertical(self):
arm.move_to(Vec3d(11, -4, 10), 1.3, 180)
if self.course == 'A':
arm.move_to(Vec3d(-11, -4, 10), 1.3, 180)
time.sleep(1)
def pick_block(self, col, level, desc='full'):
# lateral, forward, height
def get_lateral(thecol):
far_left_lateral = 11.645 + 1 # col index 0
far_right_lateral = -11.645 + 1 # col index 7
lateral_inc = (far_right_lateral - far_left_lateral) / 7
return far_left_lateral + lateral_inc * thecol
lateral = get_lateral(col)
# Special case!
if col == 0 and desc == 'far_half':
lateral += 1
logging.info(lateral)
if level == 'bottom':
level_height = BOTTOM_LEVEL_HEIGHT
elif level == 'top':
level_height = TOP_LEVEL_HEIGHT
else:
raise ValueError
if desc == 'full':
forward = FULL_BLOCK_FORWARD
elif desc == 'near_half':
forward = NEAR_HALF_BLOCK_FORWARD
elif desc == 'far_half':
forward = FAR_HALF_BLOCK_FORWARD
else:
raise ValueError
# Special case!
if (col in [1] and desc == 'far_half'):
lateral += .5
if (col in [0] and desc == 'far_half'):
# To mitigate collision with right bar, move to a spot
# to the left first and then move right
self.arm.move_to(Vec3d(lateral - 4, forward + 1, level_height + 4),
0, 180, SPEED * 1.5)
forward += 1
if col == 0 and desc == 'full':
forward -= 2
'''
if col == 0 and desc == 'far_half':
forward += 1
'''
if col in [7] and desc == 'far_half' and level == 'bottom':
forward += 1
if (col in [7] and desc == 'far_half'):
self.arm.move_to(Vec3d(lateral, forward - 3, level_height + 5), 0,
180, SPEED)
self.arm.move_to(Vec3d(lateral, forward, level_height + 5), 0, 180,
SPEED)
else:
self.arm.move_to(Vec3d(lateral, forward, level_height + 4), 0, 180,
SPEED)
self.s.set_suction(True)
self.arm.move_to(Vec3d(lateral, forward, level_height - 1), -pi / 20,
180, SPEED)
time.sleep(0.15)
# Special case!
# To avoid the math domain errors on retract
if (col in [1, 6, 7] and desc == 'far_half'):
forward = FULL_BLOCK_FORWARD
wrist_rotate = 0
if (col in [0] and desc == 'far_half'):
forward = NEAR_HALF_BLOCK_FORWARD
lateral -= 1.5
if (col in [0, 1] and desc == 'far_half'):
wrist_rotate = pi / 5
if '_half' in desc:
this_speed = SPEED * 2
else:
this_speed = SPEED
self.arm.move_to(Vec3d(lateral, forward, HEIGHT_TO_CLEAR_LOADER),
wrist_rotate, 180, this_speed)
if (col in [7] and desc == 'near_half'):
self.arm.move_to(
Vec3d(lateral, forward - 2, HEIGHT_TO_CLEAR_LOADER), 0, 180,
SPEED)
if (col in [7] and desc == 'far_half'):
self.arm.move_to(
Vec3d(lateral, forward - 4, HEIGHT_TO_CLEAR_LOADER), 0, 180,
SPEED)
def start(self):
# Moves from start square to corner near Zone A
self.move_to_corner()
logger.info("In corner")
logger.info("Free RAM: %s" % s.get_teensy_ram())
self.arm_to_vertical()
logger.info("Attempting to determine bin order")
binStuff = railorder(self.course)
bin_order = binStuff.get_rail_order(self.course)
# Give the rail sorter the bins in the correct order
if self.course == 'B':
print(bin_order)
self.rs.set_rail_zone_bins(list(bin_order))
else:
print(reversed(bin_order))
self.rs.set_rail_zone_bins(list(reversed(bin_order)))
arm.move_to(Vec3d(11, -5, 10), 1.3, 180)
arm.park()
logger.info("Free RAM: %s" % s.get_teensy_ram())
# Move to Zone B from the corner
self.align_zone_b()
logger.info("At zone B")
# Set proper lift height
if self.course == 'B':
self.ldr.lift(4.7)
elif self.course == 'A':
self.ldr.lift(4.8)
else:
raise ValueError
logger.info("Free RAM: %s" % s.get_teensy_ram())
# Load the blocks from zone B
if self.use_loader is True:
# self.wait_until_arm_limit_pressed()
self.ldr.load(strafe_dir={
'B': 'right',
'A': 'left'
}[self.course])
else:
self.wait_until_arm_limit_pressed()
self.ldr.close_flaps()
logger.info("Free RAM: %s" % s.get_teensy_ram())
self.go_to_rail_cars()
# I took a picture of everything that happens up to this point
self.rs.unload_rail(self.course)
arm.park()
logger.info("Free RAM: %s" % s.get_teensy_ram())
# rotate to the side and dump any extra blocks to clear the loader
s.set_width_motor(150, 'ccw')
time.sleep(1)
s.set_width_motor(0, 'ccw')
if self.course == 'A':
self.rotate_90('right')
elif self.course == 'B':
self.rotate_90('left')
else:
raise ValueError
self.ldr.dump_blocks()
# rotate back to barge
self.rotate_90('left')
# Testing Sea blocks loading
self.ldr.lift(1.9)
# move forward to barge
s.move_pid(1, 0, 0)
time.sleep(1.5)
s.stop()
# align horizontally for pickup
dist = 18
if self.course == 'A':
ultrasonic_go_to_position(s,
left=dist,
unit='cm',
left_right_dir=85)
else:
ultrasonic_go_to_position(s,
right=dist,
unit='cm',
left_right_dir=85)
# bump barge
trapezoid(s.move_pid, (0, 0, 0), (1, 0, 0), (0, 0, 0), 2.5)
# try again
dist = 18
if self.course == 'A':
ultrasonic_go_to_position(s,
left=dist,
unit='cm',
left_right_dir=82)
else:
ultrasonic_go_to_position(s,
right=dist,
unit='cm',
left_right_dir=82)
# load a couple blocks?
if self.use_loader is True:
self.ldr.load_sea_blocks(strafe_dir={
'B': 'right',
'A': 'left'
}[self.course])
else:
self.wait_until_arm_limit_pressed()
dist = 19
if self.course == 'A':
ultrasonic_go_to_position(s,
left=dist,
unit='cm',
left_right_dir=85)
else:
ultrasonic_go_to_position(s,
right=dist,
unit='cm',
left_right_dir=85)
trapezoid(s.move_pid, (0, 180, 0), (1, 180, 0), (0, 180, 0),
1.0)
# turn around to face the sea zone
self.rotate_180()
# bump barge to square up /w back of robot
s.move_pid(1, 180, 0)
time.sleep(1.5)
s.stop()
# drive to the sea zone
trapezoid(s.move_pid, (0, 0, 0), (1, 0, 0), (0, 0, 0), 4.0)
# unload blocks
logging.info("Unloading at sea zone")
if self.use_loader is True:
self.ldr.dump_blocks()
import math import pygame from head.spine.Vec3d import Vec3d from copy import deepcopy import Pyro4 RATE = 10 SERV_URL = "PYRO:[email protected]:9091" # Currently 10cm in front of arm center and 10cm up DEF_ARM_POS = Vec3d(0, 10, 10) ARM_SPEED = 10 def limitToRange(a, b, c): if a < b: a = b if a > c: a = c return a class Main: def __init__(self): self.SCREEN_WIDTH = 800 self.SCREEN_HEIGHT = 450 self.screen = pygame.display.set_mode( (self.SCREEN_WIDTH, self.SCREEN_HEIGHT)) # you have to change the URI below to match your own host/port. self.joyserver = Pyro4.Proxy(SERV_URL)