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()