diff --git a/src/control/goalKeeper.py b/src/control/goalKeeper.py index 05e6147d..09ae019f 100644 --- a/src/control/goalKeeper.py +++ b/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