def safe_to_move(self, dist_to_left, dist_to_rght,
robot_to_left, robot_to_rght):
def move_to_loc(self, robot_coord, target_coord, style, reverse_dir=0):
"""
@brief Moves the robot to a target coordinate by calling the
motor_worker function. Returns the thread running motor_worker so
the calling function can check if the motor is still moving with
the 'moving' field
@param robot_loc The current location of the robot in a Point object
@param target_loc The desired location for the robot in a Point object
@param style SINGLE: Standard steps
DOUBLE: Two coils on, more power and more strength
INTERLEAVE: Mix of single and double
MICROSTEP: More precise, gives 8x more steps to motor
@param reverse_dir Whether or not to reverse the direction of the
motor, perhaps because of a change in mounting orientation
"""
if self.moving = True:
print "Cannot move motor when motor is already moving"
return
#Distances of the robot from the left or right rails, respectively
dist_to_left = utils.get_pt2pt_dist(robot_coord, self.left_rail_coord)
dist_to_rght = utils.get_pt2pt_dist(robot_coord, self.rght_rail_coord)
dist_to_trgt = utils.get_pt2pt_dist(robot_coord, target_coord)
#Real distance to target in cm
real_dist_to_trgt = dist_to_trgt/self.scaled_ovr_real
#Compute direction in which the robot needs to move
#Point object is used as a vector in this case to use utils.dot
#Vector from robot to left rail
robot_to_left = Point(self.left_rail_coord.x-self.robot_coord.x,
self.left_rail_coord.y-self.robot_coord.y)
#Vector from robot to target
robot_to_trgt = Point(target_coord.x-self.robot_coord.x,
target_coord.y-self.robot_coord.y)
#Use dot product to find if the direction is towards left or right
#Direction: -1->left, 1->right
direction = -1
if utils.dot(robot_to_left, robot_to_trgt) < 0:
direction = 1
if reverse_dir is 1:
direction = direction * -1
motor_dir = direction
#Compute how many steps the motor should move
steps = (real_dist_to_trgt/gear_circum)*self.motor_steps
#Create the thread controlling the motor and run it
motor_thread = threading.Thread(target=motor_worker,
args=(self.motor, steps, motor_dir))
self.moving = True
motor_thread.start()
def __init__(self,
motor_steps=200,
gear_radius=1.29413,
left_rail_coord,
rght_rail_coord,
edge_length=0.0,
delay=.0055
):
"""
@brief Initializes the MotorController with the necessary information
@param motor_steps The steps per rotation of the motor
@param radius of the gear in cm
@param left_right_coord The coordinate of the left rail, from the
perspective of the robot. This is necessary to ensure that the robot
does not collide with the left rail (Point object)
@param rght_rail_coord The coordinate of the right rail, from the
perspective of the robot. This is necessary to ensure that the robot
does not collide with the right rail (Point object)
@param edge_length The length of the edge on which the robot traverses
in cm
@delay The delay between each step
"""
self.motor_steps = motor_steps
self.gear_radius = gear_radius
self.gear_circum = 2.0*3.14159265*gear_radius
self.left_rail_coord = left_rail_coord
self.rght_rail_coord = rght_rail_coord
#The distance between the rails in terms of the units used in
#the current camera angle
self.scaled_edge_length = utils.get_pt2pt_dist(left_rail_coord,
rght_rail_coord, 0)
self.edge_length = edge_length
self.scaled_ovr_real = self.scaled_edge_length/self.edge_length
#Since the motor is being controlled with threads, two threads cannot
#be controlling the motor at the same time
self.moving = False
self.stop = False
self.delay = delay
######## RASPBERRY PI PIN SETUP ########
GPIO.cleanup()
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
# Enable GPIO pins for ENA and ENB for stepper
enable_a = 21
enable_b = 18
# Enable pins for IN1-4 to control step sequence
coil_A_1_pin = 16
coil_A_2_pin = 25
coil_B_1_pin = 24
coil_B_2_pin = 23
# Set pin states
GPIO.setup(enable_a, GPIO.OUT)
GPIO.setup(enable_b, GPIO.OUT)
GPIO.setup(coil_A_1_pin, GPIO.OUT)
GPIO.setup(coil_A_2_pin, GPIO.OUT)
GPIO.setup(coil_B_1_pin, GPIO.OUT)
GPIO.setup(coil_B_2_pin, GPIO.OUT)
# Set ENA and ENB to high to enable stepper
GPIO.output(enable_a, True)
GPIO.output(enable_b, True)
def stream(tracker, camera=0, server=0):
"""
@brief Captures video and runs tracking and moves robot accordingly
@param tracker The BallTracker object to be used
@param camera The camera number (0 is default) for getting frame data
camera=1 is generally the first webcam plugged in
"""
######## GENERAL PARAMETER SETUP ########
MOVE_DIST_THRESH = 20 # distance at which robot will stop moving
SOL_DIST_THRESH = 150 # distance at which solenoid fires
PACKET_DELAY = 1 # number of frames between sending data packets to pi
OBJECT_RADIUS = 13 # opencv radius for circle detection
AXIS_SAFETY_PERCENT = 0.05 # robot stops if within this % dist of axis edges
packet_cnt = 0
tracker.radius = OBJECT_RADIUS
######## SERVER SETUP ########
motorcontroller_setup = False
if server:
# Create a TCP/IP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# Obtain server address by going to network settings and getting eth ip
server_address = ('169.254.171.10',10000) # CHANGE THIS
#server_address = ('localhost', 10000) # for local testing
print 'starting up on %s port %s' % server_address
sock.bind(server_address)
sock.listen(1)
connection, client_address = None, None
while True:
# Wait for a connection
print 'waiting for a connection'
connection, client_address = sock.accept()
break
######## CV SETUP ########
# create video capture object for
#cap = cv2.VideoCapture(camera)
cap = WebcamVideoStream(camera).start() # WEBCAM
#cap = cv2.VideoCapture('../media/goalie-test.mov')
#cap = cv2.VideoCapture('../media/bounce.mp4')
cv2.namedWindow(tracker.window_name)
# create trajectory planner object
# value of bounce determines # of bounces. 0 is default (no bounces)
# bounce not currently working correctly
planner = TrajectoryPlanner(frames=4, bounce=0)
# create FPS object for frame rate tracking
fps_timer = FPS(num_frames=20)
while(True):
# start fps timer
fps_timer.start_iteration()
######## CAPTURE AND PROCESS FRAME ########
ret, frame = True, cap.read() # WEBCAM
#ret, frame = cap.read() # for non-webcam testing
if ret is False:
print 'Frame not read'
exit()
# resize to 640x480, flip and blur
frame,img_hsv = tracker.setup_frame(frame=frame, w=640,h=480,
scale=1, blur_window=15)
######## TRACK OBJECTS ########
# use the HSV image to get Circle objects for robot and objects.
# object_list is list of Circle objects found.
# robot is single Circle for the robot position
# robot_markers is 2-elem list of Circle objects for robot markers
object_list = tracker.find_circles(img_hsv.copy(), tracker.track_colors,
tracker.num_objects)
robot, robot_markers = tracker.find_robot_system(img_hsv)
walls = tracker.get_rails(img_hsv, robot_markers, colors.Yellow)
planner.walls = walls
# Get the line/distances between the robot markers
# robot_axis is Line object between the robot axis markers
# points is list of Point objects of closest intersection w/ robot axis
# distanes is a list of distances of each point to the robot axis
robot_axis = utils.line_between_circles(robot_markers)
points, distances = utils.distance_from_line(object_list, robot_axis)
planner.robot_axis = robot_axis
######## TRAJECTORY PLANNING ########
# get closest object and associated point, generate trajectory
closest_obj_index = utils.min_index(distances) # index of min value
closest_line = None
closest_pt = None
if closest_obj_index is not None:
closest_obj = object_list[closest_obj_index]
closest_pt = points[closest_obj_index]
closest_line = utils.get_line(closest_obj, closest_pt) # only for viewing
planner.add_point(closest_obj)
# Get trajectory - list of elements for bounces, and final line traj
# Last line intersects with robot axis
traj_list = planner.get_trajectory_list(colors.Cyan)
traj = planner.traj
######## SEND DATA TO CLIENT ########
if packet_cnt != PACKET_DELAY:
packet_cnt = packet_cnt + 1
else:
packet_cnt = 0 # reset packet counter
# error checking to ensure will run properly
if len(robot_markers) is not 2 or robot is None or closest_pt is None:
pass
elif server:
try:
if motorcontroller_setup is False:
# send S packet for motorcontroller setup
motorcontroller_setup = True
######## SETUP MOTORCONTROLLER ########
axis_pt1 = robot_markers[0].to_pt_string()
axis_pt2 = robot_markers[1].to_pt_string()
data = 'SM '+axis_pt1+' '+axis_pt2+' '+robot.to_pt_string()
print data
connection.sendall(data)
# setup is done, send packet with movement data
else:
obj_robot_dist = utils.get_pt2pt_dist(robot, closest_obj)
print 'dist: ' + str(obj_robot_dist) # USE FOR CALIBRATION
######## SOLENOID ACTIVATION CODE ########
# check if solenoid should fire
if obj_robot_dist <= SOL_DIST_THRESH: # fire solenoid, dont move
print 'activate solenoid!'
# TODO SEND SOLENOID ACTIVATE
######## MOTOR CONTROL ########
# get safety parameters
rob_ax1_dist = utils.get_pt2pt_dist(robot_markers[0],robot)
rob_ax2_dist = utils.get_pt2pt_dist(robot_markers[1],robot)
axis_length = utils.get_pt2pt_dist(robot_markers[0],
robot_markers[1])
# ensure within safe bounds of motion relative to axis
# if rob_ax1_dist/axis_length <= AXIS_SAFETY_PERCENT or \
# rob_ax2_dist/axis_length <= AXIS_SAFETY_PERCENT:
# # in danger zone, kill motor movement
# print 'INVALID ROBOT LOCATION: stopping motor'
# data = 'KM'
# connection.sendall(data)
# if in danger zone near axis edge, move towards other edge
if rob_ax1_dist/axis_length <= AXIS_SAFETY_PERCENT:
print 'INVALID ROBOT LOCATION'
data = 'MM '+robot.to_pt_string()+' '+robot_markers[1].to_pt_string()
elif rob_ax2_dist/axis_length <= AXIS_SAFETY_PERCENT:
print 'INVALID ROBOT LOCATION'
data = 'MM '+robot.to_pt_string()+' '+robot_markers[0].to_pt_string()
# check if robot should stop moving
elif obj_robot_dist <= MOVE_DIST_THRESH: # obj close to robot
# Send stop command, obj is close enough to motor to hit
data = 'KM'
print data
connection.sendall(data)
pass
# Movement code
else: # far enough so robot should move
#### FOR TRAJECTORY ESTIMATION
# if planner.traj is not None:
# axis_intersect=shapes.Point(planner.traj.x2,planner.traj.y2)
# # Clamp the point to send to the robot axis
# traj_axis_pt = utils.clamp_point_to_line(
# axis_intersect, robot_axis)
# data = 'D '+robot.to_pt_string()+' '+traj_axis_pt.to_string()
# connection.sendall(data)
#### FOR CLOSEST POINT ON AXIS ####
if closest_pt is not None and robot is not None:
# if try to move more than length of axis, stop instead
if utils.get_pt2pt_dist(robot,closest_pt) > axis_length:
print 'TRYING TO MOVE OUT OF RANGE'
data = 'KM'
print data
connection.sendall(data)
else:
data = 'MM ' + robot.to_pt_string() + ' ' + \
closest_pt.to_string()
print data
connection.sendall(data)
except IOError:
pass # don't send anything
######## ANNOTATE FRAME FOR VISUALIZATION ########
frame = gfx.draw_lines(img=frame, line_list=walls)
frame = gfx.draw_robot_axis(img=frame, line=robot_axis) # draw axis line
frame = gfx.draw_robot(frame, robot) # draw robot
frame = gfx.draw_robot_markers(frame, robot_markers) # draw markers
frame = gfx.draw_circles(frame, object_list) # draw objects
# eventually won't need to print this one
frame = gfx.draw_line(img=frame, line=closest_line) # closest obj>axis
# draw full set of trajectories, including bounces
#frame = gfx.draw_lines(img=frame, line_list=traj_list)
#frame = gfx.draw_line(img=frame, line=traj) # for no bounces
frame = gfx.draw_point(img=frame, pt=closest_pt)
#frame=gfx.draw_line(frame,planner.debug_line) # for debug
######## FPS COUNTER ########
fps_timer.get_fps()
fps_timer.display(frame)
######## DISPLAY FRAME ON SCREEN ########
cv2.imshow(tracker.window_name,frame)
# quit by pressing q
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# release capture
cap.stop() # WEBCAM
#cap.release() # for testing w/o webcam
cv2.destroyAllWindows()
