def goBackToBeginning(self): if (self.sideOfField == "Away"): commandedX = -0.535 else: commandedX = 0.535 commandedY = 0.0 XErr = commandedX - self.robotX YErr = commandedY - self.robotY # Code for Proportional Controller for X error_Max_X = 2.0 vel_Max_X = 1.45 error_X = XErr kp_X = float(vel_Max_X)/float(error_Max_X) # units don't match, but that's okay vel_X = kp_X*error_X # Code for Proportional Controller for Y error_Max_Y = 2.0 vel_Max_Y = 1.45 error_Y = YErr kp_Y = float(vel_Max_Y)/float(error_Max_Y) # units don't match, but that's okay vel_Y = kp_Y*error_Y # Account for the angle of the robot vel_X_Body_Frame = math.cos(self.theta)*vel_X + math.sin(self.theta)*vel_Y vel_Y_Body_Frame = -1.0*(math.cos(self.theta)*float(vel_Y) - math.sin(self.theta)*float(vel_X)) # Send commands to the wheels v.goVel(vel_X_Body_Frame, vel_Y_Body_Frame, 0.0) # If Error is small, then we are at the start, so stop robot if (abs(XErr) <= 0.1 and abs(YErr) <= 0.1): v.goVel(0.0,0.0,0.0) self.state = "correctAngle" return
def seek(current_X_pos):
#move kp and other calculations to param file to only be done once
# if (current_X_pos - old_X_pos > .5):
# flag = True
# if flag:
# error_Max_X = 5
# vel_Max_X = 1
# error_X = commanded_X_pos - current_X_pos
# kp_X = vel_Max_X/error_Max_X # units don't match, but that's okay
# vel_X = kp_X*error_X
# error_Max_Y = 5
# vel_Max_Y = 1
# error_Y = commanded_Y_pos - current_Y_pos
# kp_Y = vel_Max_Y/error_Max_Y # units don't match, but that's okay
# vel_Y = kp_Y*error_Y
# error_Max_Ang = 3.14 #in radians
# vel_Max_Ang = 3
# error_Ang = commanded_Ang_pos - current_Ang_pos
# kp_Ang = vel_Max_Ang/error_Max_Ang # units don't match, but that's okay
# vel_Ang = kp_Ang*error_Ang
commanded_X_pos = 0
error_Max_X = 5
vel_Max_X = 1
error_X = commanded_X_pos - current_X_pos
kp_X = vel_Max_X/error_Max_X # units don't match, but that's okay
vel_X = kp_X*error_X
commanded_Y_pos = 0
current_Y_pos = 0
error_Max_Y = 5
vel_Max_Y = 1
error_Y = commanded_Y_pos - current_Y_pos
kp_Y = vel_Max_Y/error_Max_Y # units don't match, but that's okay
vel_Y = kp_Y*error_Y
commanded_Ang_pos = 0
current_Ang_pos = 0
error_Max_Ang = 3.14 #in radians
vel_Max_Ang = 3
error_Ang = commanded_Ang_pos - current_Ang_pos
kp_Ang = vel_Max_Ang/error_Max_Ang # units don't match, but that's okay
vel_Ang = kp_Ang*error_Ang
print "vel x is%d"%vel_X
v.goVel(vel_X,vel_Y,vel_Ang)
if (current_X_pos != 0):
current_X_pos = current_X_pos - .01
def stateMachine():
selectState()
if (Statics.state == "defendGoal"):
defendGoal()
elif (Statics.state == "rushBall"):
rushBall()
elif (Statics.state == "attackEnemy"):
attackEnemy()
elif (Statics.state == "correctAngle"):
correctAngle()
elif (Statics.state == "DoNothing"):
v.goVel(0, 0, 0)
else:
goToGoal()
def attackEnemy():
# Code for Proportional Controller
if (abs(Statics.XErr) <= 0.5):
error_Max_X = 1.9
else:
error_Max_X = 2.3
vel_Max_X = 1.4
error_X = Statics.XErr
kp_X = float(vel_Max_X) / float(
error_Max_X) # units don't match, but that's okay
vel_X = kp_X * error_X
# Code for Proportional Controller
if (abs(Statics.YErr) <= 0.5):
error_Max_Y = 1.9
else:
error_Max_Y = 2.3
vel_Max_Y = 1.4
error_Y = Statics.YErr
kp_Y = float(vel_Max_Y) / float(
error_Max_Y) # units don't match, but that's okay
vel_Y = kp_Y * error_Y
# Account for angle of robot
vel_X_Body_Frame = math.cos(Statics.theta) * vel_X + math.sin(
Statics.theta) * vel_Y
vel_Y_Body_Frame = (math.cos(Statics.theta) * float(vel_Y) -
math.sin(Statics.theta) * float(vel_X))
# Send Commands to Wheels
if (not Statics.inColideState):
v.goVel(vel_X_Body_Frame, vel_Y_Body_Frame, 0.0)
# Print Data
if (Statics.debug and Statics.samples == Statics.maxSamples):
Statics.samples = 0
print "error in X %f" % error_X
print "current_X_pos %f" % Statics.robotX
print "vel X %f" % vel_X
print
print "error in Y %f" % error_Y
print "current_Y_pos %f" % Statics.robotY
print "vel Y %f" % vel_Y
print "vel x body frame %f" % vel_X_Body_Frame
print "vel y body frame %f" % vel_Y_Body_Frame
def defendGoal():
# Code for Proportional Controller
error_Max_X = .5
vel_Max_X = 1.0
error_X = Statics.XErr
kp_X = float(vel_Max_X) / float(
error_Max_X) # units don't match, but that's okay
vel_X = kp_X * error_X
# Code for Proportional Controller
error_Max_Y = .5
vel_Max_Y = 1.0
error_Y = Statics.YErr
kp_Y = float(vel_Max_Y) / float(
error_Max_Y) # units don't match, but that's okay
vel_Y = kp_Y * error_Y
# Account for angle of the robot
vel_X_Body_Frame = math.cos(Statics.theta) * vel_X + math.sin(
Statics.theta) * vel_Y
vel_Y_Body_Frame = (math.cos(Statics.theta) * vel_Y -
math.sin(Statics.theta) * vel_X)
# Send Commands to wheels
v.goVel(vel_X_Body_Frame, vel_Y_Body_Frame, 0.0)
# Print Data
if (Statics.debug and Statics.samples == Statics.maxSamples):
Statics.samples = 0
print "error in X %f" % error_X
print "current_X_pos %f" % Statics.robotX
print "vel X %f" % vel_X
print
print "current_Y_pos %f" % Statics.robotY
print "error in Y %f" % error_Y
print "vel Y %f" % vel_Y
print "vel x body frame %f" % vel_X_Body_Frame
print "vel y body frame %f" % vel_Y_Body_Frame
def correctAngle():
# Code for Proportional Controller for Angle
error_Max_Ang = 3.14 #in radians
if (abs(Statics.AngleErr) <= 0.5):
vel_Max_Ang = 1.5
else:
vel_Max_Ang = 3.14
if (abs(Statics.theta - Statics.oldAngle) < 0.1):
vel_Max_Ang = 4.0
error_Ang = Statics.AngleErr
kp_Ang = float(vel_Max_Ang) / float(
error_Max_Ang) # units don't match, but that's okay
vel_Ang = kp_Ang * error_Ang
# Send commands to the wheels
v.goVel(0.0, 0.0, vel_Ang)
# Print Data
if (Statics.debug):
print "commanded_Ang_pos is %f" % Statics.commanded_Ang_pos
print "error in angle %f" % error_Ang
print "current Angle %f" % Statics.theta
print "angle vel %f" % vel_Ang
def correctAngle(self):
# Define where the enemy goal lies
if (self.sideOfField == "Away"):
enemyGoalCenterX = -1.4
else:
enemyGoalCenterX = 1.29
enemyGoalCenterY = 0
deltaX = enemyGoalCenterX - self.robotX
deltaY = enemyGoalCenterY - self.robotY
commanded_Ang_pos = float(math.atan2(deltaY, deltaX))
if (commanded_Ang_pos < 0):
commanded_Ang_pos += (math.pi * 2)
# Calculate the Error in the angle position from the desired
AngleErr = commanded_Ang_pos - self.theta
#check for smallest angle to commanded ang
if (AngleErr > math.pi):
AngleErr = AngleErr - math.pi * 2
elif (AngleErr < -math.pi):
AngleErr = AngleErr + math.pi * 2
# Code for Proportional Controller for Angle
error_Max_Ang = 3.14 #in radians
vel_Max_Ang = 2.9
error_Ang = AngleErr
kp_Ang = float(vel_Max_Ang) / float(
error_Max_Ang) # units don't match, but that's okay
vel_Ang = kp_Ang * error_Ang
# Send commands to the wheels
v.goVel(0.0, 0.0, vel_Ang)
# If Error is small, then the robot is facing the goal, so transition to attack
if (abs(AngleErr) <= 0.25):
v.goVel(0.0, 0.0, 0.0)
return
def selectState():
#######################################
""" Case 1: we are in Go To Goal """
#######################################
if (Statics.state == "goToGoal"):
# If the ball is far enough from our goal then we can stop defending and get back to rushing the ball
if ((Statics.sideOfField == "Away" and Statics.ballX < 0.70)
or (Statics.sideOfField == "Home" and Statics.ballX > -0.70)):
Statics.state = "rushBall"
if (Statics.showTransitions):
print "Entering rush ball"
commanded_Y_pos = 0.0
if (Statics.sideOfField == "Away"):
commanded_X_pos = float(1.25)
else:
commanded_X_pos = float(-1.35)
# Calculate Error from desired positions
Statics.XErr = commanded_X_pos - Statics.robotX
Statics.YErr = commanded_Y_pos - Statics.robotY
# Check for collision
if (checkIfColidesWithEnemy(commanded_X_pos, commanded_Y_pos)):
# if first enemy robot is to the right of the point, move left to avoid it
if (Statics.enemy1Y > commanded_Y_pos
or (Statics.enemy2Y != 0.0
and Statics.enemy2Y > commanded_Y_pos)):
commanded_Y_pos = commanded_Y_pos - 0.5
else:
commanded_Y_pos = commanded_Y_pos + 0.5
# If Error is small, then we are at the goal, transition to defend
if (abs(Statics.XErr) <= 0.2 and abs(Statics.YErr) <= 0.2):
v.goVel(0.0, 0.0, 0.0)
Statics.state = "defendGoal"
if (Statics.showTransitions):
print "Entering Defend Goal"
return
#######################################
""" Case 2: we are in Defend Goal """
#######################################
if (Statics.state == "defendGoal"):
# Analyze current Status of the robot
if (Statics.sideOfField == "Away"):
commanded_X_pos = 1.25
else:
commanded_X_pos = -1.36
if (Statics.ballY > .25):
commanded_Y_pos = .25
elif (Statics.ballY < -.25):
commanded_Y_pos = -.25
else:
commanded_Y_pos = Statics.ballY
# Calculate the Error from Desired Position
Statics.XErr = commanded_X_pos - Statics.robotX
Statics.YErr = commanded_Y_pos - Statics.robotY
# If the ball is far from the goal, rush it
if ((Statics.sideOfField == "Away" and Statics.ballX < 0.60)
or (Statics.sideOfField == "Home" and Statics.ballX > -0.60)):
Statics.state = "rushBall"
if (Statics.showTransitions):
print "Entering rush ball"
# If the ball is really close to the robot, also rush it
if (abs(Statics.ballX - Statics.robotX) < 0.2):
Statics.state = "rushBall"
Statics.suspendDefense = 1
if (Statics.showTransitions):
print "Entering rush ball"
# If the robot has reached where it is supposed to, stop it temporarily in that position
if (abs(Statics.XErr) < 0.2):
v.goVel(0.0, 0.0, 0.0)
return
#######################################
""" Case 3: we are in attack Enemy """
#######################################
if (Statics.state == "attackEnemy"):
# Analyze current status of robot
if (Statics.sideOfField == "Away"):
commanded_X_pos = -1.6
else:
commanded_X_pos = 1.49
commanded_Y_pos = float(0)
# If the ball is far from the robot, go back to rushing it
BotBallDisX = abs(Statics.robotX - Statics.ballX)
BotBallDisY = abs(Statics.robotY - Statics.ballY)
if (Statics.debug):
print "BotBallDist X is %f" % BotBallDisX
print "BotBallDist Y is %f" % BotBallDisY
if (BotBallDisX > 0.4 or BotBallDisY > 0.4):
Statics.state = "rushBall"
if (Statics.showTransitions):
print "Entering rush ball"
# Calculate the Error from the desired position
Statics.XErr = commanded_X_pos - Statics.robotX
Statics.YErr = commanded_Y_pos - Statics.robotY
# Check for collision
if (checkIfColidesWithEnemy(commanded_X_pos, commanded_Y_pos)):
# if first enemy robot is to the right of the point, move left to avoid it
if (Statics.enemy1Y > commanded_Y_pos
or (Statics.enemy2Y != 0.0
and Statics.enemy2Y > commanded_Y_pos)):
commanded_Y_pos = commanded_Y_pos - 0.5
else:
commanded_Y_pos = commanded_Y_pos + 0.5
Statics.xSamples = Statics.xSamples + 1
if (Statics.xSamples == 5):
Statics.xSamples = 0
Statics.oldX = Statics.robotX
Statics.ySamples = Statics.ySamples + 1
if (Statics.ySamples == 5):
Statics.ySamples = 0
Statics.oldY = Statics.robotY
# If Error is small, then that means we scored a goal, transition back to rush ball
if (abs(Statics.XErr) <= 0.15 and abs(Statics.YErr) <= 0.1):
v.goVel(0.0, 0.0, 0.0)
Statics.state = "rushBall"
if (Statics.showTransitions):
print "Entering rushball"
return
#######################################
""" Case 4: we are in rush ball """
#######################################
if (Statics.state == "rushBall"):
#print "In Rush ball Prel"
# If the ball is close to our goal, go back to defend it quickly
if (not Statics.suspendDefense and
(Statics.sideOfField == "Away" and Statics.ballX > 0.8)
or (Statics.sideOfField == "Home" and Statics.ballX < -0.8)):
Statics.state = "goToGoal"
if (Statics.showTransitions):
print "Entering Go To GOAL"
# If the robot surpasses the defense threshold, unsuspend the defense
if ((Statics.sideOfField == "Away" and Statics.robotX < 0.6)
or (Statics.sideOfField == "Home" and Statics.robotX > -0.6)):
Statics.suspendDefense = 0
# Calculate a position that is about 0.2m behind the ball and go there
deltaX = Statics.enemyGoalCenterX - Statics.ballX
deltaY = Statics.enemyGoalCenterY - Statics.ballY
commanded_X_pos = Statics.ballX + (
(-0.2) * deltaX / math.sqrt(deltaX * deltaX + deltaY * deltaY))
commanded_Y_pos = Statics.ballY + (
(-0.2) * deltaY / math.sqrt(deltaX * deltaX + deltaY * deltaY))
# Check for collision
if (checkIfColidesWithEnemy(commanded_X_pos, commanded_Y_pos)):
# if first enemy robot is to the right of the point, move left to avoid it
if (Statics.enemy1Y > commanded_Y_pos
or (Statics.enemy2Y != 0.0
and Statics.enemy2Y > commanded_Y_pos)):
commanded_Y_pos = commanded_Y_pos - 0.5
else:
commanded_Y_pos = commanded_Y_pos + 0.5
# Check if the robot is currently behind the ball
if (Statics.sideOfField == "Home"
and (Statics.ballX - Statics.robotX) >= 0.0):
robotBehindBall = 1
elif (Statics.sideOfField == "Away"
and (Statics.ballX - Statics.robotX) < 0.0):
robotBehindBall = 1
else:
robotBehindBall = 0
if (Statics.debug):
print "ballX is %f and robot X is %f" % (Statics.ballX,
Statics.robotX)
# Calculate Error from desired
Statics.XErr = commanded_X_pos - Statics.robotX
Statics.YErr = commanded_Y_pos - Statics.robotY
if (Statics.debug):
print "error X is %f" % Statics.XErr
print "error Y is %f" % Statics.YErr
Statics.xSamples = Statics.xSamples + 1
if (Statics.xSamples == 5):
Statics.xSamples = 0
Statics.oldX = Statics.robotX
Statics.ySamples = Statics.ySamples + 1
if (Statics.ySamples == 5):
Statics.ySamples = 0
Statics.oldY = Statics.robotY
if (Statics.printRushBall and Statics.samples == Statics.maxSamples):
print "robot X = %f and robot Y = %f" % (Statics.robotX,
Statics.robotY)
print "Cmndd X = %f and Cmndd Y = %f" % (commanded_X_pos,
commanded_Y_pos)
# If Error is small, then the robot is right behind the ball, so now correct angle
if (abs(Statics.XErr) <= 0.1 and abs(Statics.YErr) <= 0.1
and robotBehindBall): # and abs(Statics.AngleErr) <= 0.5):
v.goVel(0.0, 0.0, 0.0)
#Statics.state = "DoNothing"
Statics.state = "correctAngle"
if (Statics.showTransitions):
print "Entering correct Angle"
return
#######################################
""" Case 5: we are in Correct Angle """
#######################################
if (Statics.state == "correctAngle"):
calculateAngle()
Statics.angleSamples = Statics.angleSamples + 1
if (Statics.angleSamples == 5):
Statics.angleSamples = 0
Statics.oldAngle = Statics.theta
# If Error is small, then the robot is facing the goal, so transition to attack
if (abs(Statics.AngleErr) <= 0.25):
v.goVel(0.0, 0.0, 0.0)
# Kicker
#kicker()
Statics.state = "attackEnemy"
if (Statics.showTransitions):
print "Entering attack Enemy"
return
