Fix UFC control

unball · Feb 28, 2024 · 504ed7b · 504ed7b
1 parent be84a99
commit 504ed7b
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/control/UFC.py b/src/control/UFC.py
@@ -49,11 +49,11 @@ def output(self, robot):
       omega = self.kw * np.sign(eth) * np.sqrt(np.abs(eth)) + gamma
 
       # Velocidade limite de deslizamento
-      if phi != 0: v1 = (-np.abs(omega) + np.sqrt(omega**2 + 4 * np.abs(phi) * self.amax)) / (2*np.abs(phi))
+      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 = (2*self.vmax - self.L * np.abs(omega)) / (2 + self.L * np.abs(phi))
+      v2 = (2*self.vmax - self.L * self.kw * np.sqrt(np.abs(eth))) / (2 + self.L * np.abs(phi))
 
       # Velocidade limite de aproximação
       v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref