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