diff --git a/rocketpy/control/controller.py b/rocketpy/control/controller.py index e81e70915..45e32c815 100644 --- a/rocketpy/control/controller.py +++ b/rocketpy/control/controller.py @@ -1,3 +1,4 @@ +import math from inspect import signature from typing import Iterable @@ -17,7 +18,7 @@ def __init__( self, interactive_objects, controller_function, - sampling_rate, + sampling_rate=math.inf, initial_observed_variables=None, name="Controller", ): @@ -72,7 +73,8 @@ def __init__( relevant information in the `observed_variables` list. .. note:: The function will be called according to the sampling rate - specified. + specified. If unspecified, the default sampling rate is set to infinity, meaning that the + controller function will be called at every step of the simulation. sampling_rate : float The sampling rate of the controller function in Hertz (Hz). This means that the controller function will be called every diff --git a/rocketpy/rocket/rocket.py b/rocketpy/rocket/rocket.py index 84655b6d7..ce7df410a 100644 --- a/rocketpy/rocket/rocket.py +++ b/rocketpy/rocket/rocket.py @@ -2002,6 +2002,50 @@ def add_thrust_eccentricity(self, x, y): self.thrust_eccentricity_y = y return self + def add_discrete_controller( + self, + controller_function, + refresh_rate, + interactive_objects=None, + initial_observed_variables=None, + name="Controller", + ): + """Creates a new discrete controller, storing its parameters such as + controller function, refresh rate, and interactive objects. The controller + will be called at the specified refresh rate during the simulation.""" + + controller = _Controller( + controller_function=controller_function, + sampling_rate=refresh_rate, + interactive_objects=interactive_objects, + initial_observed_variables=initial_observed_variables, + name=name, + ) + + self._add_controllers(controller) + + def add_continuous_controller( + self, + controller_function, + interactive_objects=None, + initial_observed_variables=None, + name="Controller", + ): + """Creates a new continuous controller, storing its parameters such as + controller function and interactive objects. The controller will + be called at every time step of the simulation.""" + + controller = _Controller( + controller_function=controller_function, + sampling_rate=math.inf, + interactive_objects=interactive_objects, + initial_observed_variables=initial_observed_variables, + name=name, + ) + + self._add_controllers(controller) + return controller + def draw(self, vis_args=None, plane="xz", *, filename=None): """Draws the rocket in a matplotlib figure. diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py index 2293d9706..45e5189a4 100644 --- a/rocketpy/simulation/flight.py +++ b/rocketpy/simulation/flight.py @@ -777,7 +777,14 @@ def __simulate(self, verbose): self.y_sol = phase.solver.y if verbose: print(f"Current Simulation Time: {self.t:3.4f} s", end="\r") - + for controller in self._continuous_controllers: + controller( + self.t, + self.y_sol, + self.solution[:-1], + self.sensors, + self.env, + ) if self.__check_simulation_events(phase, phase_index, node_index): break # Stop if simulation termination event occurred @@ -1576,6 +1583,9 @@ def __init_equations_of_motion(self): def __init_controllers(self): """Initialize controllers and sensors""" self._controllers = self.rocket._controllers[:] + self._continuous_controllers = [ + c for c in self._controllers if math.isinf(c.sampling_rate) + ] self.sensors = self.rocket.sensors.get_components() # reset controllable object to initial state (only airbrakes for now) @@ -4488,6 +4498,9 @@ def add_parachutes(self, parachutes, t_init, t_end): def add_controllers(self, controllers, t_init, t_end): for controller in controllers: + # Skip node creation for continuous controllers + if math.isinf(controller.sampling_rate): + continue # Calculate start of sampling time nodes controller_time_step = 1 / controller.sampling_rate controller_node_list = [