def look_at(self, x, y, min_speed = 50, max_speed = 80):
"""
Generates commands for the robot to rotate to a specific angle
:param x: X coordinate of the point to turn to
:param y: Y coordinate of the point to turn to
:return: returns a CommandDict with the next command
"""
min_rot_speed = min_speed
max_rot_speed = max_speed
rob_pos = np.array([self.robot.x, self.robot.y])
target_pos = np.array([x, y])
vec = target_pos - rob_pos
av = vec / np.linalg.norm(vec)
rv = Plan.angle_to_vector(self.robot.angle)
dot = np.dot(av, rv)
cross = np.cross(rv, av)
consol.log('dot', dot, 'Plan')
consol.log('cross', cross, 'Plan')
speed = np.interp(dot, [0.0, 1.0], [max_rot_speed, min_rot_speed])
direction = "Right" if cross < 0 else "Left"
kick = "None"
return CommandDict(speed, direction, kick)
def has_clear_shot(self):
obstacle_width=25
(target_x, target_y) = self.goalCentre()
their_defender = self.world.their_defender
#If their defender is not on the pitch, return True:
consol.log("defender pos", (their_defender.x, their_defender.y), "TakeShot")
consol.log("defender angle", their_defender.angle, "TakeShot")
if their_defender._vector.x == their_defender._vector.y and their_defender._vector.x == 0:
return True
obstacle_x = their_defender.x
obstacle_y = their_defender.y
d_y = self.robot.y - target_y
d_x = self.robot.x - target_x
if d_x == 0:
d_x = 0.1
m = d_y/float(d_x)
c = self.robot.y - m*self.robot.x
#Compare y-coords when x is equal:
ball_y_at_obstacle = m*obstacle_x + c
if math.fabs(ball_y_at_obstacle - obstacle_y)<obstacle_width:
return False
return True
def rotate_fade(self, angle, min_speed = 50, max_speed = 80):
"""
Generates commands for the robot to rotate to a specific angle
:param angle: Radians to turn to
:return: a CommandDict with the next command
"""
min_rot_speed = min_speed
max_rot_speed = max_speed
av = Plan.angle_to_vector(angle)
rv = Plan.angle_to_vector(self.robot.angle)
dot = np.dot(av, rv)
cross = np.cross(rv, av)
consol.log('dot', dot, 'Plan')
consol.log('cross', cross, 'Plan')
speed = np.interp(dot, [0.0, 1.0], [max_rot_speed, min_rot_speed])
direction = "Right" if cross < 0 else "Left"
kick = "None"
return CommandDict(speed, direction, kick)
def update(self, model_positions):
"""
Main planner update function. Should be called once per frame.
:param model_positions: A dictionary containing the positions of the objects on the pitch. (See VisionWrapper.model_positions)
:return: The next command to issue to the robot.
"""
# Update the world state with the given positions
self.world.update_positions(model_positions)
# Update whether the robot is marked as busy on consol
self.robot.is_busy()
if self.world.ball != None:
consol.log("Catcher", self.robot.catcher, "Robot")
if self.current_plan.isValid() and not self.current_plan.isFinished():
return self.current_plan.nextCommand()
else:
# If self.current_plan is invalid, then choose a new plan and return a command from it
for plan in self.plans:
if plan.isValid():
prevPlan = self.current_plan
same_pl = False
if self.current_plan is plan:
same_pl = True
self.current_plan = plan
if not same_pl:
self.current_plan.initi(prevPlan)
# self.current_plan.reset()
return self.current_plan.nextCommand()
else:
return CommandDict.stop()
def set_motor(self, motor, speed, scale=False, delay=0):
# If the motor is already running at this speed, do not send the command
# this creates bugs when enabling comms so i commented it out
#if speed == self.current_motor_speeds[motor]:
# return
if scale:
scaled_speed = self.get_scaled_speed(motor, speed)
else:
scaled_speed = speed
if motor == 'left':
log("Left speed", speed, "Comms")
if motor == 'right':
log("Right speed", speed, "Comms")
# floats are to large to transfer over serial as sting
int_val = int(scaled_speed)
self._write_serial("s {0} {1} {2}".format(self.motorPins[motor], int_val, delay))
lp = 0.3
self.current_motor_speeds[motor] = (1.0 - lp) *self.current_motor_speeds[motor] + lp *speed
def is_busy(self):
if hasattr(self, 'busy_until'):
busy = datetime.datetime.now() < self.busy_until
else:
busy = False
consol.log("Is Busy", busy, "Robot")
return busy
def isValid(self):
"""
Current constraints are:
- Robot must have the ball
- Shot must not be blocked(not implemented)
"""
consol.log("Clear shot", self.has_clear_shot(), "TakeShot")
return self.robot.has_ball(self.world.ball) and (not self.robot.is_busy())
def enabled(self, e):
if not e:
self.stop()
self._enabled = e
log('Communication enabled (press C to toggle)', self.enabled, "Comms")
def __init__(self, calibration):
consol.log('use y r b d p and click on objects in video to calibrate', None)
self.color = 'plate'
# self.pre_options = pre_options
self.calibration = calibration
self.maskWindowName = "Mask " + self.color
self.frame = None
self.setWindow()
def __init__(self, calibration):
consol.log('use y r b d p and click on objects in video to calibrate', None)
self.color = 'red'
# self.pre_options = pre_options
self.calibration = calibration
self.maskWindowName = "Mask " + self.color
self.frameWindowName = "Frame window"
self.frame = None
self.setWindow()
def isValid(self):
"""
Current constraints are:
- Robot must have the ball
- Robot must be within 50px of the wall
"""
consol.log("distance_from_wall", self.distance_from_wall(), "RetreatFromWall")
is_valid = self.robot.has_ball(self.world.ball) and self.distance_from_wall() <= 50 and (not self.robot.is_busy())
consol.log("is_valid", is_valid, "RetreatFromWall")
return is_valid
def update():
ra = robot_api.robot_api
dt = time() - FrameEst.last_time
fest = FrameEst(dt, ra.current_motor_speeds["right"], ra.current_motor_speeds["left"], time())
consol.log("time", time(), "Future")
FrameEst.est = [x for x in FrameEst.est if x.time > time() - FrameEst.vision_late] + [fest]
FrameEst.last_time = time()
def __init__(self, calibration, name="Camera calibration"):
consol.log('set camera settings so that all objects are detected',
None)
self.calibration = calibration
self.frameWindowName = name
self.frame = None
self.setting = 'camera'
self.setWindow()
def ball_distance_from_wall(self):
"Returns the distance of the ball from the wall nearest to it."
cur_y = self.world.ball.y
bottom_dist = cur_y
top_dist = self.max_y - cur_y
consol.log("top_dist", top_dist, "GrabAll")
consol.log("bottom_dist", bottom_dist, "GrabAll")
if top_dist < bottom_dist:
return top_dist
else:
return bottom_dist
def is_shot_blocked(world, our_robot, their_robot):
'''
Checks if our robot could shoot past their robot
'''
predicted_y = predict_y_intersection(
world, their_robot.x, our_robot, full_width=True, bounce=True)
if predicted_y is None:
return True
consol.log('##########', (predicted_y, their_robot.y, their_robot.length), 'Utils')
b = abs(predicted_y - their_robot.y) < their_robot.length
consol.log('is blocked', b, 'Utils')
return b
def isValid(self):
"""
Current constraints are:
- Robot must have the ball
- Shot must not be blocked
"""
#return True
consol.log("Clear shot", self.has_clear_shot(), "ShootAll")
ball_dist = self.robot.get_euclidean_distance_to_point(*self.get_ball_pos()) > 200 and self.world.ball.x != 0
if ball_dist:
self.robot.hball = False
return self.robot.hball
def isMatched(self, robot, ball):
max_e_dist = 40
their_defender = self.world.their_defender
(target_x, target_y) = self.goalCentre()
ball_y = self.world.ball.y
ball_x = self.world.ball.x
zone = self.world.pitch.zones[self.robot.zone]
zone = self.world.pitch.zones[self.robot.zone]
between = (ball_x - target_x) * (ball_x - self.midX) < 0.0
if between or ball_x < 1.0:
def_ang = their_defender._vector.angle
def_pos = their_defender._vector
p1 = array( [self.midX, 0.0] )
p2 = array( [self.midX, self.max_y] )
ad = array([math.cos(def_ang), math.sin(def_ang)])
p3 = array( [def_pos.x, def_pos.y] )
p4 = array( p3 + ad)
si = MyMath.seg_intersect( p1,p2, p3,p4)
#log_dot(si, 'yellow', 'haha')
#log('inter', si, 'MatchY')
g_to_mid = self.midX - target_x
y = si[1]
y = clip(y, max_e_dist, self.max_y - max_e_dist)
consol.log('facing', g_to_mid * ad[0] > 0.0, 'MatchY')
if g_to_mid * ad[0] > 0.0:
return math.fabs(robot.y - y) < ERROR
else:
return math.fabs(robot.y - self.midY) < ERROR
else:
y = clip(ball.y, max_e_dist, self.max_y - max_e_dist)
return math.fabs(robot.y - y) < ERROR
def nextCommand(self):
their_defender = self.world.their_defender
(target_x, target_y) = self.goalCentre()
ball_y = self.world.ball.y
ball_x = self.world.ball.x
zone = self.world.pitch.zones[self.robot.zone]
zone = self.world.pitch.zones[self.robot.zone]
max_e_dist = 40
between = (ball_x - target_x) * (ball_x - self.midX) < 0.0
if between or ball_x < 1.0:
def_ang = their_defender._vector.angle
def_pos = their_defender._vector
p1 = array( [self.midX, 0.0] )
p2 = array( [self.midX, self.max_y] )
ad = array([math.cos(def_ang), math.sin(def_ang)])
p3 = array( [def_pos.x, def_pos.y] )
p4 = array( p3 + ad)
si = MyMath.seg_intersect( p1,p2, p3,p4)
#log_dot(si, 'yellow', 'haha')
#log('inter', si, 'MatchY')
g_to_mid = self.midX - target_x
y = si[1]
y = clip(y, max_e_dist, self.max_y - max_e_dist)
consol.log('facing', g_to_mid * ad[0] > 0.0, 'MatchY')
if g_to_mid * ad[0] > 0.0:
command = self.go_to_asym(zone.center()[0], y, max_speed=100, min_speed=50)
else:
command = self.go_to_asym(self.midX, self.midY, max_speed=100, min_speed=50)
else:
y = clip(ball_y, max_e_dist, self.max_y - max_e_dist)
command = self.go_to_asym(zone.center()[0], ball_y, max_speed=100, min_speed=50)
return command
def __init__(self, device_path=None, baud_rate=None):
# check if there are valid parameters
self._enabled = False
if (device_path is not None and baud_rate is not None):
try:
self.serial = Serial(device_path, baud_rate, timeout=0.1)
except:
log("Error in initalizing serial connection. Is the path correct?", False, 'Comms')
#alias the _write_serial function so we don't throw errors
self._write_serial = self._write_serial_debug
#just for printing
self.enabled = False
global robot_api
robot_api = self
def has_clear_shot(self):
edge = 50
if self.robot.y < 50 or self.robot.y > self.max_y - 50:
return False
obstacle_width=25
(target_x, target_y) = self.goalCentre()
their_defender = self.world.their_defender
pos_diff = math.fabs(their_defender._vector.y - self.robot.y)
#if no defender
if their_defender._vector.y < 1.0:
consol.log("clear shoot", True, "ShootAll")
return True
rob_y = self.robot.y
relo = rob_y - target_y
reld = their_defender._vector.y - target_y
consol.log("clear shoot", relo * reld < 0.0, "ShootAll")
return relo * reld < 0.0 and pos_diff > 50
#If their defender is not on the pitch, return True:
consol.log("defender pos", (their_defender.x, their_defender.y), "ShootAll")
consol.log("defender angle", their_defender.angle, "ShootAll")
if their_defender._vector.x == their_defender._vector.y and their_defender._vector.x == 0:
return True
obstacle_x = their_defender.x
obstacle_y = their_defender.y
d_y = self.robot.y - target_y
d_x = self.robot.x - target_x
if d_x == 0:
d_x = 0.1
m = d_y/float(d_x)
c = self.robot.y - m*self.robot.x
#Compare y-coords when x is equal:
ball_y_at_obstacle = m*obstacle_x + c
if math.fabs(ball_y_at_obstacle - obstacle_y)<obstacle_width:
return False
return True
def isValid(self):
"""
Current constraints are:
- Ball must be within the robot's zone
- Robot must not have the ball
- Ball must be near a wall
"""
# See if self.has_ball exists:
if not hasattr(self, 'has_ball'):
self.has_ball = True
consol.log("Ball dist from wall", self.ball_distance_from_wall(), "GrabNearWall")
near_dist = 40 # Note DO NOT CHANGE WITHOUT ALSO CHANGING IN GRABBALLPLAN
if self.world.ball is not None and self.world.ball.velocity <= 3:
isValid = self.world.pitch.is_within_bounds(self.robot, self.world.ball.x, self.world.ball.y) and \
((not self.robot.has_ball(self.world.ball)) or (not self.has_ball)) and \
self.ball_distance_from_wall <= near_dist and (not self.robot.is_busy())
else:
isValid = False
consol.log("isValid", isValid, "GrabNearWall")
return isValid
def nextCommand(self):
# Plan is always finished to allow switching to other plans at any point.
self.finished = True
rotation_error = math.pi/15
(gx, gy) = self.goalCentre()
consol.log("(gx, gy)", (gx,gy), "TakeShot")
#If we are facing the goal, shoot!
consol.log("Scalar product", self.robot.get_dot_to_target(gx, gy), "TakeShot")
if self.robot.get_dot_to_target(gx, gy) > 0.985:
if self.frames_passed >= self.frames_before_shot:
self.finished = True
self.robot.catcher = "open"
self.robot.set_busy_for(1.1)
return self.kick()
else:
self.frames_passed += 1
return self.stop()
else:
command = self.look_at(gx, gy, max_speed=55, min_speed=40)
return command
def get_future():
d = [0, 0]
a = 0
consol.log("frames", [(int(x.len * 1000), int(x.sl), int(x.sr)) for x in FrameEst.est], "Future")
for i in FrameEst.est:
das = (
np.interp((i.sr - i.sl) * 0.5, [-100, -10, 10, 100], [-FrameEst.rot_speed, 0, 0, FrameEst.rot_speed])
* i.len
)
a += das
dds = np.interp((i.sl + i.sr) * 0.5, [-100, -40, 40, 100], [-FrameEst.speed, 0, 0, FrameEst.speed]) * i.len
# assuming average angle
d += rot_vec([dds, 0], a * 0.5)
# consol.log('future rel', (d,a), 'Future')
return (d, a)
def shoot(self, gx, gy):
angle = self.angle_to_goal(self.robot.x, self.robot.y, gx, gy)
command = self.rotate_fade(angle, min_speed = 50, max_speed = 70)
consol.log("Goal angle",angle,"Attacking")
# Check if we're done rotating
consol.log("Delta_angle", math.fabs(angle - self.robot.angle), "Attacking")
if math.fabs(angle - self.robot.angle) > math.pi/24:
consol.log("Command", command, "Attacking")
return command
# If we are then kick the ball
else:
self.finished = True
self.robot.catcher = "open"
self.robot.target_y = None
consol.log("Command", "Kick", "Attacking")
return self.kick()
def _write_serial(self, data):
if not self.enabled:
return
ack = False
num_attempts = 0
# Test code that will drop the majority of commands to test fault tollerance
data_bytes = str.encode(data)
data_bytes += '\r'
self.serial.write(data_bytes)
log("Sent command", format(data), "Comms")
log_time('Comms')
return
while not ack:
data_bytes = str.encode(data)
data_bytes += '\r'
self.serial.write(data_bytes)
num_attempts += 1
log("Sent command", format(data), "Comms")
log_time('Comms')
try:
ack_str = self.serial.read(4)
log("Ack string", ack_str, "Comms")
print ('ack:' +ack_str)
if ack_str == "ack6":
ack = True
else:
ack = False
except SerialException as ex:
ack = False
log("Ack", ack, "Comms")
if num_attempts >= 40:
raise Exception("Too many attempts to send command")
def nextCommand(self):
# Plan is always finished to allow switching to other plans at any point
# E.g. making a shot if an opening is available.
self.finished = True
consol.log("(mx, my)", (self.mx,self.my), "WallShotPlan")
#If we are not at the move target, move there:
distance = self.robot.get_euclidean_distance_to_point(self.mx, self.my)
consol.log("Distance to move target", distance, "WallShotPlan")
if distance > 18 and not self.has_moved:
command = self.go_to_asym(self.mx, self.my, forward=False, max_speed = 85, min_speed=50)
return command
#Otherwise, make a wall shot:
else:
#Flag that move is complete:
self.has_moved = True
#Aim at x= slightly in fron of centre of opponents zone, y=nearer edge of pitch
centre_x = self.world.pitch.zones[self.world.their_defender.zone].center()[0]
if self.world.their_defender.zone == 0:
target_x = centre_x
elif self.world.their_defender.zone == 3:
target_x = centre_x
if self.robot.y > self.world.pitch.zones[self.robot.zone].center()[1]:
target_y = self.max_y + 10
else:
target_y = -10
consol.log("Shoot_target", (target_x, target_y), "WallShotPlan")
consol.log("Dot to target", self.robot.get_dot_to_target(target_x, target_y), "WallShotPlan")
if self.robot.get_dot_to_target(target_x, target_y) > 0.991:
if self.frames_passed >= self.frames_before_shot:
self.finished = True
self.robot.catcher = "open"
self.robot.set_busy_for(1.1)
return self.kick()
else:
self.frames_passed += 1
return self.stop()
else:
self.frames_passed = 0
command = self.look_at(target_x, target_y, max_speed=55, min_speed=40)
return command
def update(self, command):
if command is None:
return
speed = command["speed"]
direction = command["direction"]
kick = command["kick"]
log_time('dict_control')
log('speed', speed, 'dict_control')
log('direction', direction, 'dict_control')
log('kick', kick, 'dict_control')
if direction == "Forward":
log_time('dict_control', 'time go forward called')
if 'speedr' in command:
speed_right = command["speedr"]
self.robot_api.go_forward_asym(speed, speed_right)
else:
self.robot_api.go_forward(speed)
elif (direction == "Backward"):
self.robot_api.go_backward(speed)
elif (direction == "Right"):
self.robot_api.turn_right(speed)
elif (direction == "Left"):
self.robot_api.turn_left(speed)
elif (direction == "None"):
for i in range(0, 10):
self.robot_api.stop()
time.sleep(.001)
if (kick == "Kick"):
for i in range(0, 10):
self.robot_api.kick(100)
time.sleep(.001)
elif (kick == "Prepare"):
log_time("dict_control", "prepare")
for i in range(0, 10):
self.robot_api.prepare_catch()
time.sleep(.001)
elif (kick == "Catch"):
for i in range (0, 10):
self.robot_api.catch(100)
time.sleep(.001)
def nextCommand(self):
# Plan is always finished to allow switching to other plans at any point.
self.finished = True
rotation_error = math.pi/15
(gx, gy) = self.goalCentre()
consol.log("(gx, gy)", (gx,gy), "TakeShot")
command = None
dedge = 60
timeout = self.get_time() > 1.0
consol.log('state', self.state, 'ShootAll')
if self.state == 'init':
if timeout:
self.nstate = 'go1'
command = CommandDict.stop()
elif self.state == 'go1':
p = (self.midX, self.max_y -dedge)
if self.arrived(*p):
self.nstate = 'go2'
elif self.has_clear_shot():
self.nstate = 'swing'
self.res_timer()
else:
command = self.move_to(*p)
elif self.state == 'go2':
p = (self.midX, dedge)
if self.arrived(*p):
self.nstate = 'go1'
elif self.has_clear_shot():
self.nstate = 'swing'
self.res_timer()
else:
command = self.move_to(*p)
elif self.state == 'swing':
p = self.goalCentre()
#if not self.has_clear_shot():
# self.nstate = 'go1'
#else:
if self.robot.get_dot_to_target(gx, gy) > 0.85:
command = self.kick_new()
self.res_timer()
self.nstate = 'wait'
else:
command = self.look_at(gx, gy, max_speed=50, min_speed=40)
elif self.state == 'wait':
if timeout:
self.robot.hball = False
self.finished = True
command = CommandDict.stop()
'''
elif self.state == 'swing':
p = self.goalCentre()
if not self.has_clear_shot():
self.nstate = 'wall'
elif timeout:
command = self.kick_new()
else:
command = self.move_to(*p)
elif self.state == 'testwall':
if timeout:
self.nstate = 'wall'
command = self.catch()
elif self.state == 'wall':
p = (self.midX, self.midY)
if self.arrived(*p):
self.nstate = 'wallsh'
self.res_timer()
else:
command = self.move_to(*p)
elif self.state == 'wallsh':
p = ((self.robot.x + gx) * 0.5, 0.0)
if timeout:
command = self.kick_new()
self.res_timer()
else:
command = self.move_to(*p)
'''
self.state = self.nstate
return command
def draw(self, frame, model_positions, actions, regular_positions, fps,
dState, aState, a_action, d_action, grabbers, our_color, our_side,
key=None, preprocess=None):
"""
Draw information onto the GUI given positions from the vision and post processing.
NOTE: model_positions contains coordinates with y coordinate reversed!
"""
# Get general information about the frame
frame_height, frame_width, channels = frame.shape
# turn stuff off and on, i linked launch class in a variable called launch
robot = self.launch.controller.robot_api
if key == ord('c'):
Planner.planner.current_plan.initi(None)
robot.enabled = not robot.enabled
World.world.our_attacker.hball = False
if key == ord('k'):
self.consol_enabled = not self.consol_enabled
if key == 27:
robot.enabled = False
consol.log('Press k to disable/enable consol', True)
#draw console
if(self.consol_enabled):
consol.draw()
# Draw the calibration gui
self.calibration_gui.show(frame, key)
# Draw dividors for the zones
self.draw_zones(frame, frame_width, frame_height)
their_color = list(TEAM_COLORS - set([our_color]))[0]
key_color_pairs = zip(
['our_defender', 'their_defender', 'our_attacker', 'their_attacker'],
[our_color, their_color]*2)
self.draw_ball(frame, regular_positions['ball'])
for key, color in key_color_pairs:
self.draw_robot(frame, regular_positions[key], color)
# Draw fps on the canvas
if fps is not None:
self.draw_text(frame, 'FPS: %.1f' % fps, 0, 10, BGR_COMMON['green'], 1)
if preprocess is not None:
# print preprocess
preprocess['normalize'] = self.cast_binary(
cv2.getTrackbarPos(self.NORMALIZE, self.VISION))
preprocess['background_sub'] = self.cast_binary(
cv2.getTrackbarPos(self.BG_SUB, self.VISION))
if grabbers:
self.draw_grabbers(frame, grabbers, frame_height)
# Extend image downwards and draw states.
blank = np.zeros_like(frame)[:200, :, :]
frame_with_blank = np.vstack((frame, blank))
self.draw_states(frame_with_blank, aState, (frame_width, frame_height))
if model_positions and regular_positions:
pass
for key in ['our_defender', 'our_attacker', 'their_defender', 'their_attacker']: #'ball',
if model_positions[key] and regular_positions[key]:
self.data_text(
frame_with_blank, (frame_width, frame_height), our_side, key,
model_positions[key].x, model_positions[key].y,
model_positions[key].angle, model_positions[key].velocity)
self.draw_velocity(
frame_with_blank, (frame_width, frame_height),
model_positions[key].x, model_positions[key].y,
model_positions[key].angle, model_positions[key].velocity)
# Draw center of uncroppped frame (test code)
# cv2.circle(frame_with_blank, (266,147), 1, BGR_COMMON['black'], 1)
cv2.imshow(self.VISION, frame_with_blank)
def mouse_call(self, event,x,y,flags,param):
#global ix,iy,drawing,mode
consol.log('param', param, 'Find HSV')
if event == cv2.EVENT_LBUTTONDOWN:
consol.log_time('Find HSV', 'mouse click')
frame_hsv = cv2.cvtColor(self.frame, cv2.COLOR_BGR2HSV)
col = self.get_pixel_col(x, y)
# fliped on purpose
hsv = frame_hsv[y][x]
consol.log('pixel color (hsv)', hsv, 'Find HSV')
hsv_delta = np.array([15, 50, 50])
hsv_min = hsv - hsv_delta
hsv_max = hsv + hsv_delta
consol.log('max (hsv)', hsv_max, 'Find HSV')
consol.log('min (hsv)', hsv_min, 'Find HSV')
self.set_slider(hsv_min, hsv_max)
consol.log('pixel color', col, 'Find HSV')
consol.log('pixel xy', [x, y], 'Find HSV')
consol.log('frame size', [len(self.frame[0]), len(self.frame)], 'Find HSV')
def nextCommand(self):
maxs =70
mins = 50
#compute inputs
distance = self.robot.get_euclidean_distance_to_point(self.world.ball.x, self.world.ball.y)
dot = self.robot.get_dot_to_target(self.world.ball.x, self.world.ball.y)
ball_catchable = distance < DISTANCE_ERROR and dot > 0.96 #input
wall_ball = self.ball_distance_from_wall() < 50 #input
near_center = self.robot.get_euclidean_distance_to_point(self.world.ball.x, self.midY) < 80 #input
consol.log('state', self.state, 'GrabAll')
#FSM body
command = None
if self.state == 'pick':
'''
if wall_ball:
self.robot.catcher = "closed"
self.nstate = 'goto center'
'''
if ball_catchable:
command = self.catch()
else:
command = self.go_to_asym(self.world.ball.x, self.world.ball.y, forward=True, max_speed = maxs, min_speed=mins, mid_x=True)
elif self.state == 'goto center':
if near_center:
self.nstate = 'pick wall'
#command = CommandDict.catch()
elif ball_catchable:
command = self.catch()
elif not wall_ball:
self.nstate = 'pick'
else:
command = self.go_to_asym(self.world.ball.x, self.midY, forward=True, max_speed = 90, min_speed=mins)
elif self.state == 'pick wall':
consol.log_time('GrabAll', 'wall move')
if not wall_ball:
self.nstate = 'pick'
elif ball_catchable:
command = self.catch()
else:
command = self.go_to_asym(self.world.ball.x, self.world.ball.y, forward=True, max_speed = maxs, min_speed=mins, mid_y= True)
'''
if self.robot.catcher != "prepared":
self.robot.catcher = "prepared"
return CommandDict.prepare()
'''
# If we need to move to the ball, then get the command and return it
# command = self.go_to(self.world.ball.x, self.world.ball.y, speed=75)
self.state = self.nstate
return command
def mouse_call(self, event,x,y,flags,param):
#global ix,iy,drawing,mode
consol.log('param', param, 'Find YUV')
if event == cv2.EVENT_LBUTTONDOWN:
consol.log_time('Find YUV', 'mouse click')
frame_yuv = cv2.cvtColor(self.frame, cv2.COLOR_BGR2YUV)
col = self.get_pixel_col(x, y)
# fliped on purpose
yuv = frame_yuv[y][x]
consol.log('pixel color (yuv)', yuv, 'Find YUV')
yuv_delta = np.array([15, 50, 50])
yuv_min = yuv - yuv_delta
yuv_max = yuv + yuv_delta
consol.log('max (yuv)', yuv_max, 'Find YUV')
consol.log('min (yuv)', yuv_min, 'Find YUV')
self.set_slider(yuv_min, yuv_max)
consol.log('pixel color', col, 'Find YUV')
consol.log('pixel xy', [x, y], 'Find YUV')
consol.log('frame size', [len(self.frame[0]), len(self.frame)], 'Find YUV')
def arrived(self, x, y):
consol.log('distance to destination', self.robot.get_euclidean_distance_to_point(x, y), 'ShootAll')
return self.robot.get_euclidean_distance_to_point(x, y) < 50
