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