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)