def look_for_token(robot, zone): """ Go to zone's corner and return markers seen there. Turns the robot so that it then scans the corner by turning through 90 degrees. """ if zone == robot.zone: x_offset = 1.4 y_offset = 0.2 + 0.5 second_token_x = x_offset if zone in [0, 3] else 8 - x_offset second_token_y = y_offset if zone in [0, 1] else 8 - y_offset theta = pi / 2 * robot.zone log(robot, "Moving to corner of zone %d..." % (zone)) move_to_point(robot, second_token_x, second_token_y, theta) else: zx, zy = SCAN_POINTS[zone] target_theta = (1.5 * pi + zone * pi / 2) % (pi + pi) move_to_point(robot, zx, zy, target_theta) log(robot, "Moved to corner of zone " + str(zone) + ".") robot.sound.play('Radar') for i in xrange(3): markers = robot.see(res=RESOLUTION) for marker in markers: n = marker.info.code if n in xrange(28): robot.position.reset_to(position_from_wall(marker)) elif our_token(marker, robot.zone): robot.sound.stop() return marker turn(robot) else: robot.sound.stop() return None
def look_for_token(robot, zone): """ Go to zone's corner and return markers seen there. Turns the robot so that it then scans the corner by turning through 90 degrees. """ if zone == robot.zone: x_offset = 1.4 y_offset = 0.2 + 0.5 second_token_x = x_offset if zone in [0, 3] else 8 - x_offset second_token_y = y_offset if zone in [0, 1] else 8 - y_offset theta = pi/2 * robot.zone log(robot, "Moving to corner of zone %d..." % (zone)) move_to_point(robot, second_token_x, second_token_y, theta) else: zx, zy = SCAN_POINTS[zone] target_theta = (1.5*pi + zone*pi/2) % (pi+pi) move_to_point(robot, zx, zy, target_theta) log(robot, "Moved to corner of zone " + str(zone) + ".") robot.sound.play('Radar') for i in xrange(3): markers = robot.see(res=RESOLUTION) for marker in markers: n = marker.info.code if n in xrange(28): robot.position.reset_to(position_from_wall(marker)) elif our_token(marker, robot.zone): robot.sound.stop() return marker turn(robot) else: robot.sound.stop() return None
def move_to_point(robot, x, y, target_theta=None, smart=False): """ Given the robot's current tracked position, moves to point (x, y), where x and y are metres from the origin. """ if not valid_point(x, y): log(robot, 'Cant go there') return False log(robot, "Moving to point x=%.1f y=%.1f" % (x, y)) dist, angle = directions_for_point(robot, x, y) robot.sound.play('Valkyries') log(robot, "dist=%.1f angle=%.1f" % (dist, angle)) turn(robot, angle) # if smart and not avoid_obstacles(robot, x, y, target_theta): move_straight(robot, dist) if target_theta is not None: d_theta = target_theta - robot.position.theta if d_theta > pi: d_theta -= pi + pi elif d_theta < -pi: d_theta += pi + pi print(d_theta) turn(robot, d_theta) log(robot, "done.") robot.sound.stop() return True
def move_to_point(robot, x, y, target_theta=None, smart=False): """ Given the robot's current tracked position, moves to point (x, y), where x and y are metres from the origin. """ if not valid_point(x, y): log(robot, 'Cant go there') return False log(robot, "Moving to point x=%.1f y=%.1f" % (x, y)) dist, angle = directions_for_point(robot, x, y) robot.sound.play('Valkyries') log(robot, "dist=%.1f angle=%.1f" % (dist, angle)) turn(robot, angle) # if smart and not avoid_obstacles(robot, x, y, target_theta): move_straight(robot, dist) if target_theta is not None: d_theta = target_theta - robot.position.theta if d_theta > pi: d_theta -= pi+pi elif d_theta < -pi: d_theta += pi+pi print(d_theta) turn(robot, d_theta) log(robot, "done.") robot.sound.stop() return True
def search(): # Function to turn the bot for searching the bot # global variable declaration global search_angle, direction, rotation_time if rotation_time > 360: rotation_time = 0 rotation_time += search_angle movements.turn(direction, search_angle)
def line_up_to_marker(robot, marker, dist=0.4): """ Moves the robot 'dist' metres in front of a given marker. """ log(robot, "Lining up to marker:") robot.sound.play('Valkyries') dist, angle1, angle2 = directions_for_marker(marker, d=dist) log(robot, "dist=%.2f, angle1=%.2f, angle2=%.2f" % (dist, angle1, angle2)) turn(robot, angle1) move_straight(robot, dist) turn(robot, angle2) log(robot, "Lined up to Marker.") robot.sound.stop()
def approx(robot): z = robot.zone angles = [pi / 6, pi / 3, pi / 2] distances = [1, 0.5, 0.3, 0.1, 0.5] if z == 0: for phi in angles: for i in range(3): for j in [1, -1]: print 'turning %.2f' % phi turn(robot, phi * j) sleep(5) else: for dist in distances: for i in [1, -1]: print 'moving %.2f' % dist * i move_straight(robot, dist * i) sleep(5)
def approx(robot): z = robot.zone angles = [pi/6, pi/3, pi/2] distances = [1, 0.5, 0.3, 0.1, 0.5] if z == 0: for phi in angles: for i in range(3): for j in [1, -1]: print 'turning %.2f' % phi turn(robot, phi*j) sleep(5) else: for dist in distances: for i in [1, -1]: print 'moving %.2f' % dist*i move_straight(robot, dist*i) sleep(5)
import manual_controls import movements if manual_controls.user_input == 'w': movements.move('forward') elif manual_controls.user_input == 's': movements.move('reverse') elif manual_controls.user_input == 'a': movements.turn('left') elif manual_controls.user_input == 'd': movements.turn('right') elif manual_controls.user_input == 'u': movements.turn('u_turn') elif manual_controls.user_input == 'r': movements.lift('up') elif manual_controls.user_input == 'f': movements.lift('down')
def action_three(): movements.turn() return render_template('controls.html')
def tracking(): # Function to process the captured frame # define all the global variables global ball_captured, direction, orange, goal # Setting values based on the ball capture if (ball_captured % 2) == 0: color_range = orange threshold_y = 300 else: color_range = goal threshold_y = 250 # to start receiving the frames form the camera for image in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True): # save the image as a numpy array frame = image.array # clear the buffer memory rawCapture.truncate(0) # convert the frame to HSV co-ordinates hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # mask -> to apply filter to the image based on color range mask = cv2.inRange(hsv, color_range[0], color_range[1]) # erode -> to remove small blobs in the image mask = cv2.erode(mask, kernel, iterations=1) # dilate -> to sharpen the edges mask = cv2.dilate(mask, kernel, iterations=1) # contours -> set of points which are in white contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] if contours: # maximum area -> the ball object = max(contours, key=len) # calculating the x-center and y-center (x, y) = ((max(object[:, :, 0]) + min(object[:, :, 0])) // 2, (max(object[:, :, 1]) + min(object[:, :, 1])) // 2) # to check for captured condition if y >= threshold_y: # to close/open the gate depending on the condition movements.gate(ball_captured) ball_captured += 1 break # to check if the ball is on the left side elif x < ((res[0] // 2) - threshold): direction = 'l' movements.turn(direction, turn_angle) print("Left") # to check if the ball is on the right side elif x > ((res[0] // 2) + threshold): direction = 'r' movements.turn(direction, turn_angle) print("Right") # if the ball is within the threshold region elif y < threshold_y: movements.move('f', distance) print("moving towards object") else: # search for the ball print("Searching") search()
def avoid_obstacles(robot, x, y, theta): # This will need more arguments markers = robot.see() theta = robot.position.theta for marker in markers: if marker.info.code in range(28, 32): markers_ = robot.see() for m in markers_: # Leaves m being a robot marker if m.info.code in range(28, 32): break else: log(robot, 'Lost opponent\'s robot') return # Works out direction of movement of that robot x0, y0 = marker_pos(marker, robot.position) x1, y1 = marker_pos(m, robot.position) dx = x1 - x0 dy = y1 - y0 DX = marker.centre.world.x - m.centre.world.x X = robot.position.x Y = robot.position.y dist_to_target = hypot(x - X, y - Y) if dx < 0.1 and dy < 0.1: # The robot is still if m.dist <= dist_to_target: # It's too close # This works log(robot, 'Robot still, too close.') # Turn 45 deg away from opp. log(robot, radians(m.rot_y) - pi / 4) turn(robot, radians(m.rot_y) - pi / 4) # TO-DO: Turn towards the centre # Maybe check whether we can go to this point # Find the point to go to get past the opp. # move_straight(robot, .2) dist = hypot(0.5, m.dist) dx_ = sin(theta) * dist dy_ = cos(theta) * dist # move_straight(robot, hypot(0.5, m.dist)) # Use move_to_p. move_to_point(robot, X + dx_, Y + dy_, theta, False) # This gets us where we wanted return True else: return False else: print 'opponent moving' # opp_theta = (5*pi/2 - atan2(dy, dx)) % 2*pi opp_theta = pi / 6 - pi # Is it going towars us? if theta - pi / 9 <= ((opp_theta + pi) % (2 * pi)) <= theta + pi / 9: # Move in parallel turn(robot, opp_theta + pi - theta) #may result in head-on co. if m.dist <= dist_to_target: move_straight(robot, m.centre.world.y) # How long? return True else: print 'not avoiding' return False # May cause trouble # Get to target point elif hypot(X - x1, Y - y1) <= dist_to_target: # It's too close if m.centre.world.x >= 0.4: # How close we'll get to it # if we move forwards print 'we can move past it' move_straight(robot, m.dist) # How long? # This gets us where we wanted move_to_point(robot, x, y, theta, False) return True else: # Let it pass print 'waiting' camera_delay = 1.5 d_left = 0.4 - m.centre.world.x t_left = d_left / (DX / camera_delay) if t_left < 5: # Don't wait too long sleep(t_left) return False return True else: return False elif marker.info.code in range(40, 52): return False if our_token(marker, robot.zone): pass # This should happen quite often else: pass # Do something? else: log(robot, 'Seen nothing')
from movements import turn, move from motor_a import lift from ultrasonic_sensor import is_blocked from time import sleep from threading import Thread white = False black = False def colorCheck(): while True: white = is_white() black = is_black() sleep(2) t = Thread(target=colorCheck) t.start() while True: sleep(2) print("test me here") if not is_blocked() and not black: move('forward') elif black: turn("custom", 100) lift('up') move('forward') lift('down')
def avoid_obstacles(robot, x, y, theta): # This will need more arguments markers = robot.see() theta = robot.position.theta for marker in markers: if marker.info.code in range(28, 32): markers_ = robot.see() for m in markers_: # Leaves m being a robot marker if m.info.code in range(28, 32): break else: log(robot, 'Lost opponent\'s robot') return # Works out direction of movement of that robot x0, y0 = marker_pos(marker, robot.position) x1, y1 = marker_pos(m, robot.position) dx = x1 - x0 dy = y1 - y0 DX = marker.centre.world.x - m.centre.world.x X = robot.position.x Y = robot.position.y dist_to_target = hypot(x-X, y-Y) if dx < 0.1 and dy < 0.1: # The robot is still if m.dist <= dist_to_target: # It's too close # This works log(robot, 'Robot still, too close.') # Turn 45 deg away from opp. log(robot, radians(m.rot_y)-pi/4) turn(robot, radians(m.rot_y)-pi/4) # TO-DO: Turn towards the centre # Maybe check whether we can go to this point # Find the point to go to get past the opp. # move_straight(robot, .2) dist = hypot(0.5, m.dist) dx_ = sin(theta) * dist dy_ = cos(theta) * dist # move_straight(robot, hypot(0.5, m.dist)) # Use move_to_p. move_to_point(robot, X+dx_, Y+dy_, theta, False) # This gets us where we wanted return True else: return False else: print 'opponent moving' # opp_theta = (5*pi/2 - atan2(dy, dx)) % 2*pi opp_theta = pi/6 - pi # Is it going towars us? if theta - pi/9 <= ((opp_theta+pi) % (2*pi)) <= theta + pi/9: # Move in parallel turn(robot, opp_theta+pi-theta) #may result in head-on co. if m.dist <= dist_to_target: move_straight(robot, m.centre.world.y) # How long? return True else: print 'not avoiding' return False # May cause trouble # Get to target point elif hypot(X-x1, Y-y1) <= dist_to_target: # It's too close if m.centre.world.x >= 0.4: # How close we'll get to it # if we move forwards print 'we can move past it' move_straight(robot, m.dist) # How long? # This gets us where we wanted move_to_point(robot, x, y, theta, False) return True else: # Let it pass print 'waiting' camera_delay = 1.5 d_left = 0.4 - m.centre.world.x t_left = d_left / (DX/camera_delay) if t_left < 5: # Don't wait too long sleep(t_left) return False return True else: return False elif marker.info.code in range(40, 52): return False if our_token(marker, robot.zone): pass # This should happen quite often else: pass # Do something? else: log(robot, 'Seen nothing')