Change Defender control

src/control/defender.py

@@ -32,30 +32,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 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)
-    v3 = self.kp * (dTarget-0.02) ** 2 + robot.vref if dTarget > 0.02 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