Fix GoalKeeper control

src/control/goalKeeper.py

@@ -33,33 +33,42 @@ def output(self, robot):
     if robot.field is None: return 0,0
     # Ângulo de referência
     th = robot.field.F(robot.pose)
+
     # Erro angular
     eth = angError(th, robot.th)
+
     # Tempo desde a última atuação
     dt = self.interval.getInterval()
-    self.ieth = sat(self.ieth + eth*dt, 5)
+
     # Derivada da referência
-    dth = filt(angError(th, self.lastth) / dt, 100)
+    dth = sat(angError(th, self.lastth) / dt, 15)
 
-    # Lei de controle da velocidade angular
-    w = dth + self.kw * np.sign(eth) * np.sqrt(np.abs(eth)) #+ self.kw/100.0 * self.ieth 
+    # Computa phi
+    phi = robot.field.phi(robot.pose)
+
+    # Computa gamma
+    gamma = robot.field.gamma(dth, robot.velmod, phi)
+
+    # Computa omega
+    omega = self.kw * np.sign(eth) * np.sqrt(np.abs(eth))
 
     # Velocidade limite de deslizamento
-    v1 = self.amax / np.abs(w) if w != 0 else math.inf
+    if phi != 0: v1 = (-self.kw * np.sqrt(np.abs(eth))  + np.sqrt(self.kw**2 + 4 * np.abs(phi) * self.amax)) / (2*np.abs(phi))
+    if phi == 0: v1 = self.amax / np.abs(omega)
 
     # Velocidade limite das rodas
-    v2 = self.vmax - self.L * np.abs(w) / 2
+    v2 = (2*self.vmax - self.L * self.kw * np.sqrt(np.abs(eth))) / (2 + self.L * np.abs(phi))
 
     # Velocidade limite de aproximação
-    dTarget = norm(robot.pos, robot.field.Pb) if robot.pos[0] > -.6 else np.abs(robot.field.Pb[1] - robot.pos[1])
-    e = (dTarget-0.015) ** 2
-    self.iep = self.iep + dt*e if self.iep < 1.5 else 0
-
-    v3 = self.kp * e + self.kp/80 * self.iep  + robot.vref if dTarget > 0.015 else 0
+    v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref
 
     # Velocidade linear é menor de todas
+    vels = np.array([v1,v2,v3])
     v  = max(min(v1, v2, v3), 0)
-
+
+    # Lei de controle da velocidade angular
+    w = v * phi + omega
+
     # Atualiza a última referência
     self.lastth = th
     robot.lastControlLinVel = v