def __call__(self, sys_output: float, sys_output_derivative: float = None):
"""
Update the control law for the PID
|Ref|--->|"piece of code"|--->|PID|--->|Plant|----->
^ |
|_______________________________|
:param sys_output: The output of the system that we want to regulate
:param sys_output_derivative: The derivative of the output (if have a direct measurement - to avoid noise)
:return: the update control law
"""
# Compute the proportional error
error = sys_output - self._reference
# Check if the error is an angle (the error between 2 angles cannot ever be superior to pi)
if self._is_angle:
while error > pi:
error -= (2 * pi)
while error < -pi:
error += (2 * pi)
# Compute the integral error-anti_windup
integral_error = integrate(x_dot=error - self._anti_windup,
x=self._integral_error,
dt=self._dt)
# Update the integral error for the next iteration
self._integral_error = integral_error
# If the derivative is given, then just save it
if sys_output_derivative is not None:
derivative_error = float(sys_output_derivative)
else:
# If the derivative of the signal is not given, then compute it
derivative_error = (sys_output - self._prev_output) / self._dt
# Save the current output for the next derivative gain update
self._prev_output = sys_output
# Compute the control law
output = -(self.Kp * error) - (self.Kd * derivative_error) - (
self.Ki * integral_error)
# Saturate the output between a minimum and maximum value
# and compute the value for the anti-windup system to feedback
if self._output_min is not None and output < self._output_min:
self._anti_windup = output - self._output_min
output = self._output_min
print("Executou")
elif self._output_max is not None and output > self._output_max:
self._anti_windup = output - self._output_min
output = self._output_max
print("Executou")
return output
def update(self, desired_velocity: float, dt: float = 0.01):
# The velocity of the target is the same as the desired velocity
self._gamma_dot = desired_velocity
# Integrate the gamma to get the current parameter of the virtual target
self._gamma = integrate(x_dot=self._gamma_dot, x=self._gamma, dt=dt)
def update(self,
desired_input: array = array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])):
"""
Method to update the state of the vehicle given a desired:
generalized vector of force and torques OR thrust to apply to each individual motor (depending
on the boolean specified in the constructor of the AUV object)
:param desired_input: generalized vector of force and torques OR thrust to apply to each individual motor
(depending on the boolean specified in the constructor of the quadrotor object)
Updates the values inside state and state_dot
"""
# --------------------------------------------------------
# -- Check if the input is generalized vector of forces --
# -- in the rigid body or the vector of thrusts applied --
# -- to each motor --
# --------------------------------------------------------
# Check whether the forces vector is a generalized vector of desired forces and torques
# to apply to the rigid body or already a vector of thrusts to apply to the vehicle thrusters
if not self.input_is_thrusts:
# Convert the desired general forces applied in the rigid body to the desired thrusts
# and check if we are getting the correct size
thrusts = self.thruster_dynamics.convert_general_forces_to_thrusts(
array(desired_input).reshape(6))
else:
# The "forces" vector received is not general forces but rather thrusts applied directly to the motors
# and check if we are getting as many desired thrust as the number of thrusters in our thruster model
thrusts = array(desired_input).reshape(
self.thruster_dynamics.number_of_thrusters)
# --------------------------------------------------------
# -- Propagate the desired thrust by the thruster model --
# --------------------------------------------------------
applied_thrusts = self.thruster_dynamics.apply_thrusters_dynamics(
thrusts)
applied_forces = self.thruster_dynamics.convert_thrusts_to_general_forces(
applied_thrusts)
# --------------------------------------------------------
# -- Compute the wind to use by the model --
# --------------------------------------------------------
# Update the ocean currents dynamics
if self.wind is None:
# Assume zero currents in [x, y, z] components
wind = zeros(3)
else:
wind = self.wind.get_currents()
# --------------------------------------------
# -- Compute the dynamics of the rigid body --
# --------------------------------------------
self.state_dot.v_1 = self.translational_dynamics(applied_forces[0:3])
self.state_dot.v_2 = self.rotational_dynamics(applied_forces[3:6])
# Integrate the dynamics
self.state.v_1 = integrate(x_dot=self.state_dot.v_1,
x=self.state.v_1,
dt=self.dt)
self.state.v_2 = integrate(x_dot=self.state_dot.v_2,
x=self.state.v_2,
dt=self.dt)
# ----------------------------------------------
# -- Compute the kinematics of the rigid body --
# -----------------------------------------------
self.state_dot.eta_1, self.state_dot.eta_2 = self.kinematics(wind=wind)
# Integrate the kinematics
self.state.eta_1 = integrate(x_dot=self.state_dot.eta_1,
x=self.state.eta_1,
dt=self.dt)
self.state.eta_2 = integrate(x_dot=self.state_dot.eta_2,
x=self.state.eta_2,
dt=self.dt)
# Wrap the angles (phi, theta, psi) between -pi and pi
for i in range(self.state.eta_2.size):
self.state.eta_2[i] = wrapAngle(self.state.eta_2[i])
def __call__(self,
sys_output: ndarray,
sys_output_derivative: ndarray = None):
# Compute the proportional error
error = sys_output - self._reference
# Check if the error is an angle (the error between 2 angles is bounded between -pi, pi)
for i in range(self.num_states):
if self._is_angle[i]:
while error[i] < -pi:
error[i] += (2 * pi)
while error[i] > pi:
error[i] -= (2 * pi)
# Compute the integral error-anti_windup
integral_error = integrate(x_dot=error - self._anti_windup,
x=self._integral_error,
dt=self._dt)
# Update the integral error for the next iteration
self._integral_error = integral_error
# If the derivative is given, then just save it
if sys_output_derivative is not None:
derivative_error: ndarray = array(sys_output_derivative).reshape(
self.num_states)
else:
# If the derivative of the signal is not given, then compute it
derivative_error: ndarray = (sys_output -
self._prev_output) / self._dt
# Save the current output for the next derivative gain update
self._prev_output = sys_output
# Compute the control law
output = -(self.Kp * error) - (self.Kd * derivative_error) - (
self.Ki * integral_error)
# Saturate the output between a minimum and maximum value
# and compute the value for the anti-windup system to feedback
if self._output_min is not None:
self._anti_windup[greater(
self._output_min, output)] = output[greater(
self._output_min, output)] - self._output_min[greater(
self._output_min, output)]
output[greater(self._output_min,
output)] = self._output_min[greater(
self._output_min, output)]
if self._output_max is not None:
self._anti_windup[greater(
output, self._output_max)] = output[greater(
output, self._output_max)] - self._output_min[greater(
output, self._output_max)]
output[greater(output,
self._output_max)] = self._output_max[greater(
output, self._output_max)]
# If the number of states is just one, return the control law as a float an not as a numpy array
if self.num_states == 1:
return float(output)
return output