def initi(self, prevPlan):
consol.log_time('GrabAll', 'initi')
robot_api.robot_api.prepare_catch()
self.robot.catcher = "prepared"
self.state = 'pick'
self.nstate = 'pick'
self.robot.hball = False
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 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 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 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 initi(self, prev_plan):
self.res_timer()
self.state = 'init'
self.nstate = 'init'
consol.log_time('ShootAll', 'initi')
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 initi(self, prevPlan):
robot_api.robot_api.prepare_catch()
self.robot.catcher = "prepared"
consol.log_time('GRAB', 'initi')
