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