def rush_goal(): print "rush goal" mutex.acquire() desiredPoint = param.AWAY_GOAL while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.4)) or \ (team1_robot_state.pos_y_est > (desiredPoint.y + 0.3) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.3)): if team1_robot_state.pos_x_est < ball.x: print "break" break targetAngle = MotionSkills.angleBetweenPoints(Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est), desiredPoint) radian180 = param.degreeToRadian(180) radian360 = param.degreeToRadian(360) radian5 = param.degreeToRadian(5) anglediff = (team1_robot_state.pos_theta_est - targetAngle + radian180) % radian360 - radian180 angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL) omega = angular_command.omega if(anglediff <= radian5 and anglediff >= -radian5): omega = 0 command = MotionSkills.go_to_point(team1_robot_state, desiredPoint) velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega, team1_robot_state.pos_theta_est) time.sleep(robot_update_delay) # kick.kick() velchange.goXYOmega(0,0,0)
def go_to_center(): mutex.acquire() desiredPoint = param.CENTER while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.05) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.05)) or \ (team1_robot_state.pos_y_est > (desiredPoint.y + 0.05) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.05)): targetAngle = MotionSkills.angleBetweenPoints(Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est), desiredPoint) radian180 = param.degreeToRadian(180) radian360 = param.degreeToRadian(360) radian5 = param.degreeToRadian(7) anglediff = (team1_robot_state.pos_theta_est - targetAngle + radian180) % radian360 - radian180 angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL) omega = angular_command.omega if(anglediff <= radian5 and anglediff >= -radian5): omega = 0 command = MotionSkills.go_to_point(team1_robot_state, desiredPoint) velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega , team1_robot_state.pos_theta_est) time.sleep(robot_update_delay) velchange.goXYOmega(0,0,0)
def go_to_point_behind_ball(): desiredPoint = MotionSkills.getPointBehindBall(ball, param.AWAY_GOAL) while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.13) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.13)) or \ (team1_robot_state.pos_y_est > (desiredPoint.y + 0.13) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.13)): robot_point = Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est) desiredPoint = MotionSkills.getPointBehindBall(ball, param.AWAY_GOAL) targetAngle = MotionSkills.angleBetweenPoints(Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est), param.AWAY_GOAL) radian180 = param.degreeToRadian(180) radian360 = param.degreeToRadian(360) radian5 = param.degreeToRadian(5) anglediff = (team1_robot_state.pos_theta_est - targetAngle + radian180) % radian360 - radian180 command = MotionSkills.go_to_point(team1_robot_state, desiredPoint) angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL) omega = angular_command.omega if(anglediff <= radian5 and anglediff >= -radian5): omega = 0 velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega, team1_robot_state.pos_theta_est) time.sleep(robot_update_delay) angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL) velchange.goXYOmega(0,0,angular_command.omega) time.sleep(angular_command.runTime) velchange.goXYOmega(0,0,0)
def test(): print "in test" mutex.acquire() desiredPoint = param.AWAY_GOAL while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.4)) or \ (team1_robot_state.pos_y_est > (desiredPoint.y + 0.3) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.3)): starting_point = str(int(param.meterToPixel(team1_robot_state.pos_x_est))) + ',' + str(int(param.meterToPixel(team1_robot_state.pos_y_est))) end_point = str(int(param.meterToPixel(param.AWAY_GOAL.x))) + ',' + str(int(param.meterToPixel(param.AWAY_GOAL.y))) print starting_point print end_point # visited, shortest = path.shortest_path(graph, '-10,0' , '0,0') # # print shortest # point = shortest[10] # x,y = point.split(',') # next_point = Point.Point(x, y) # # command = MotionSkills.go_to_point(team1_robot_state, next_point) # # velchange.goXYOmega(command.vel_x, command.vel_y, team1_robot_state.pos_theta_est) # time.sleep(robot_update_delay) # desiredPoint = Point.Point(param.pixelToMeter(0), param.pixelToMeter(0)) # go_to_point2(desiredPoint.x, desiredPoint.y, param.HOME_GOAL) # time.sleep(0.01) velchange.goXYOmega(0,0,0)
def defend_goal(): # vel_x = 0 # if float(ball_y) < 85 and float(ball_y) > -75: # if state.pos_y_est > param.pixelToMeter(float(ball_y)): # vel_x = -0.7 # else: # vel_x = 0.7 # velchange.goXYOmega(0,vel_x,0) # else: # velchange.goXYOmega(0,0,0) desiredPoint = Point.Point(param.HOME_GOAL.x - 0.32 , ball.y) # keep robot within the bounds of the goal if desiredPoint.y > param.HOME_GOAL.y + 0.4: desiredPoint.y = param.HOME_GOAL.y + 0.4 elif desiredPoint.y < param.HOME_GOAL.y - 0.4: desiredPoint.y = param.HOME_GOAL.y - 0.4 # move to the desiredPoint while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.1) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.1)) or \ (team1_robot_state.pos_y_est > (desiredPoint.y + 0.1) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.1)): command = MotionSkills.go_to_point(team1_robot_state, desiredPoint) angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL) omega = angular_command.omega velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega, team1_robot_state.pos_theta_est) time.sleep(goalie_update_delay) velchange.goXYOmega(0,0,0)
def defend_goal(): # vel_x = 0 # if float(ball_y) < 85 and float(ball_y) > -75: # if state.pos_y_est > param.pixelToMeter(float(ball_y)): # vel_x = -0.7 # else: # vel_x = 0.7 # velchange.goXYOmega(0,vel_x,0) # else: # velchange.goXYOmega(0,0,0) desiredPoint = Point.Point(param.HOME_GOAL.x - 0.42 , ball.y) # keep robot within the bounds of the goal if desiredPoint.y > param.HOME_GOAL.y + 0.3: desiredPoint.y = param.HOME_GOAL.y + 0.3 elif desiredPoint.y < param.HOME_GOAL.y - 0.3: desiredPoint.y = param.HOME_GOAL.y - 0.3 # move to the desiredPoint while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.13) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.13)) or \ (team1_robot_state.pos_y_est > (desiredPoint.y + 0.13) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.13)): desiredPoint = Point.Point(param.HOME_GOAL.x - 0.42 , ball.y) # keep robot within the bounds of the goal if desiredPoint.y > param.HOME_GOAL.y + 0.3: desiredPoint.y = param.HOME_GOAL.y + 0.3 elif desiredPoint.y < param.HOME_GOAL.y - 0.3: desiredPoint.y = param.HOME_GOAL.y - 0.3 targetAngle = MotionSkills.angleBetweenPoints(Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est), param.AWAY_GOAL) radian180 = param.degreeToRadian(180) radian360 = param.degreeToRadian(360) radian5 = param.degreeToRadian(7) anglediff = (team1_robot_state.pos_theta_est - targetAngle + radian180) % radian360 - radian180 angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL) omega = angular_command.omega if(anglediff <= radian5 and anglediff >= -radian5): omega = 0 command = MotionSkills.go_to_point(team1_robot_state, desiredPoint) velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega, team1_robot_state.pos_theta_est) time.sleep(goalie_update_delay) velchange.goXYOmega(0,0,0)
def go_to_home(): mutex.acquire() desiredPoint = Point.Point(param.HOME_GOAL.x - 0.5, 0) while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.1) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.1)) or \ (team1_robot_state.pos_y_est > (desiredPoint.y + 0.1) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.1)): command = MotionSkills.go_to_point(team1_robot_state, desiredPoint) angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL) omega = angular_command.omega velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega, team1_robot_state.pos_theta_est) time.sleep(robot_update_delay) velchange.goXYOmega(0,0,0)
def go_prepare_spin(): mutex.acquire() desiredPoint = Point.Point(param.pixelToMeter(90), 0) while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.1) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.1)) or \ (team1_robot_state.pos_y_est > (desiredPoint.y + 0.1) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.1)): command = MotionSkills.go_to_point(team1_robot_state, desiredPoint) angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL) omega = angular_command.omega velchange.goXYOmegaTheta(command.vel_x, command.vel_y, 2.0, team1_robot_state.pos_theta_est) time.sleep(robot_update_delay) angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL) velchange.goXYOmega(0,0,angular_command.omega) time.sleep(angular_command.runTime) velchange.goXYOmega(0,0,0)
def one_on_one(): mutex.acquire() state = "idle" start = default_timer() ball_prev_x = ball.x ball_prev_y = ball.y while True: if goal_scored() and state != "idle": print "goal scored", param.meterToPixel(ball.x) state = "reset" # If there's input ready, do something, else do something # else. Note timeout is zero so select won't block at all. while sys.stdin in select.select([sys.stdin], [], [], 0)[0]: command = sys.stdin.readline().strip().lower() if command == "r": state = "reset" elif command == "s": state = "defend" elif command == "k": velchange.goXYOmega(0,0,0) state = "idle" if state == "idle": print "state: idle" pass elif state == "defend": print "state: defend" if ball.x < 0: # transition state = "score" else: # save state ball_prev_x = ball.x ball_prev_y = ball.y # restart timer start = default_timer() # transition state = "defend_goal" elif state == "defend_goal": print "state: defend_goal" if ball.x < 0: # transition state = "score" continue if abs(ball.x - ball_prev_x) < 0.02 and abs(ball.y - ball_prev_y) < 0.02: if default_timer() - start > 0.5: # transition state = "score" else: defend_goal() else: start = default_timer() ball_prev_x = ball.x ball_prev_y = ball.y defend_goal() elif state == "score": print "state: score" go_to_point_behind_ball() rush_goal() state = "defend" elif state == "reset": print "state: reset" go_prepare_spin() state = "idle"
import sys import os sys.path.append('/home/odroid/ecen490/') os.system("echo 0 > /sys/class/gpio/gpio199/value;") import MotionControl.scripts.motor_control.velchange as velchange import MotionControl.scripts.motor_control.roboclaw as roboclaw velchange.goXYOmega(0,0,0)