Merge pull request #13 from Red-Dragons-UFSCar/testeControlador5v5

Alterações de Controlador para o 5x5 LARC 2021

action.py

@@ -24,7 +24,7 @@ def shoot(robot,ball,leftSide=True,friends=[],enemys=[]):
 
     robot.simSetVel(v,w)
 
-def defenderSpin(robot,ball,leftSide=True,friends=[],enemys=[]):
+def defenderSpin2(robot,ball,leftSide=True,friends=[],enemys=[]):
     if leftSide:
         arrivalTheta=arctan2(90-ball.yPos,235-ball.xPos) #? Angle between the ball and point (150,65)
     else:
@@ -58,26 +58,75 @@ def defenderSpin(robot,ball,leftSide=True,friends=[],enemys=[]):
 
     robot.simSetVel(v,w)
 
+def defenderSpin(robot,ball,leftSide=True,friends=[],enemys=[]):
+    if leftSide:
+        arrivalTheta=arctan2(90-ball.yPos,235-ball.xPos) #? Angle between the ball and point (150,65)
+    else:
+        arrivalTheta=arctan2(90-ball.yPos,15-ball.xPos) #? Angle between the ball and point (0,65)
+    #robot.target.update(ball.xPos,ball.yPos,0)
+    robot.target.update(ball.xPos,ball.yPos,arrivalTheta)
+
+    if not friends: #? No friends to avoid
+        v,w=univecController(robot,robot.target,avoidObst=False,n=16, d=2)
+    else: #? Both friends to avoid
+        #robot.obst.update(robot,friend1,friend2,enemy1,enemy2,enemy3)
+        robot.obst.update2(robot, ball, friends, enemys)
+        v,w=univecController(robot,robot.target,True,robot.obst,n=4, d=4)
+
+    d = robot.dist(ball)
+    if robot.spin and d < 10:
+        if not robot.teamYellow:
+            if robot.yPos > 90:
+                v = 0
+                w = -30
+            else:
+                v = 0
+                w = 30
+        else:
+            if robot.yPos > 90:
+                v = 0
+                w = 30
+            else:
+                v = 0
+                w = -30
+    if d < 30 and ball.xPos > robot.xPos:
+        if robot.teamYellow:
+            dx = 15-robot.xPos
+        else:
+            dx = 235 - robot.xPos
+        dy = tan(robot.theta)*dx + robot.yPos
+        if dy > 70 and dy < 110:
+            if robot.index == 2 or robot.index == 1:
+                robot.simSetVel2(50*robot.face, 50*robot.face)
+            else:
+                robot.simSetVel(v,w)
+        else:
+            robot.simSetVel(v,w)
+    else:
+        robot.simSetVel(v,w)
+
 #TODO #2 Need more speed to reach the ball faster than our enemy
 def screenOutBall(robot,ball,staticPoint,leftSide=True,upperLim=200,lowerLim=0,friend1=None,friend2=None):
+    xPos = ball.xPos + ball.vx*100*22/60 # Só mudei isso
+    yPos = ball.yPos + ball.vy*100*22/60
     #Check if ball is inside the limits
-    if ball.yPos >= upperLim:
+    if yPos >= upperLim:
         yPoint = upperLim
 
-    elif ball.yPos <= lowerLim:
+    elif yPos <= lowerLim:
         yPoint = lowerLim
 
     else:
-        yPoint = ball.yPos
+        yPoint = yPos
     #Check the field side
     if leftSide:
-        if robot.yPos <= ball.yPos:
+        if robot.yPos <= yPos:
             arrivalTheta=pi/2
         else:
             arrivalTheta=-pi/2
         robot.target.update(staticPoint,yPoint,arrivalTheta)
     else:
-        if robot.yPos <= ball.yPos:
+        if robot.yPos <= yPos:
             arrivalTheta=pi/2
         else:
             arrivalTheta=-pi/2

behaviours.py

@@ -7,7 +7,7 @@ class Univector:
     def __init__(self):
         #? Constants learned from EP
         self.d_e=6                                      # Constante relacionada ao tamanho das espirais
-        self.k_r=7                                    # Constante de suavização do campo
+        self.k_r=3                                    # Constante de suavização do campo
         self.delta=5                                  # Variancia da gaussiana de obstaculo
         self.k_o=0.5                                   # Constante de proporcionalidade velocidade do obstaculo
         self.d_min=5 #* => modified: EP = 3.48          # Distancia mínima em que o campo se torna puro repulsivo

execution.py

@@ -35,7 +35,7 @@ def univecController(robot,target,avoidObst=True,obst=None,n=8,d=2,stopWhenArriv
         #robotLockedCorner(target, robot)
     navigate=Univector() #? Defines the navigation algorithm
     dl=0.000001          #? Constant to approximate phi_v
-    k_w=1.9                #? Feedback constant (k_w=1 means no gain)
+    k_w=1.8                #? Feedback constant (k_w=1 means no gain)
     k_p=1                #? Feedback constant (k_p=1 means no gain)
 
     #% Correção de ângulo caso o robô esteja jogando com a face de trás

simClasses.py

@@ -129,8 +129,8 @@ def __init__(self):
 
     #% This method gets position of the ball in FIRASim
     def simGetPose(self, data_ball):
-        self.xPos = data_ball.x + data_ball.vx*100*12/60
-        self.yPos = data_ball.y + data_ball.vy*100*12/60
+        self.xPos = data_ball.x + data_ball.vx*100*8/60
+        self.yPos = data_ball.y + data_ball.vy*100*8/60
 
         # check if prev is out of field, in this case reflect ball moviment to reproduce the collision
         if self.xPos > 235:
@@ -174,7 +174,10 @@ def __init__(self, index, actuator, mray):
         self.vTheta=0
         self.vL=0                            #? Left wheel velocity (cm/s) => updated on simClasses.py -> simSetVel()
         self.vR=0                            #? Right wheel velocity (cm/s) =>  updated on simClasses.py -> simSetVel()
-        self.vMax=35                             #! Robot max velocity (cm/s)
+        if self.index == 0:
+            self.vMax=35                             #! Robot max velocity (cm/s)
+        else:
+            self.vMax=50
         self.rMax=3*self.vMax                #! Robot max rotation velocity (rad*cm/s)
         self.L=7.5                           #? Base length of the robot (cm)
         self.LSimulador=6.11                 #? Base length of the robot on coppelia (cm)

strategy.py

@@ -39,8 +39,8 @@ def coach(self):
 
     def basicStgDef(self):
         """Basic original strategy"""
-        action.screenOutBall(self.robot3, self.ball, 135, leftSide=not self.mray, upperLim=85, lowerLim=5)
-        action.screenOutBall(self.robot4, self.ball, 135, leftSide=not self.mray, upperLim=175, lowerLim=95)
+        action.screenOutBall(self.robot3, self.ball, 150, leftSide=not self.mray, upperLim=85, lowerLim=5)
+        action.screenOutBall(self.robot4, self.ball, 150, leftSide=not self.mray, upperLim=175, lowerLim=95)
         if not self.mray:
             if self.ball.xPos < 40 and self.ball.yPos > 50 and self.ball.yPos < 130:
                 action.defenderPenalty(self.robot0, self.ball, leftSide=not self.mray)