From 7b4449c9b0beef7657e2f6277470240d28e84d56 Mon Sep 17 00:00:00 2001 From: Amerik Singh Date: Sun, 5 Jan 2025 16:20:51 +0100 Subject: [PATCH] BF: Controlled system PT2 #1085 --- .../bf/systems/pool/second_order_system.py | 46 ++++++++++--------- 1 file changed, 24 insertions(+), 22 deletions(-) diff --git a/src/mlpro/bf/systems/pool/second_order_system.py b/src/mlpro/bf/systems/pool/second_order_system.py index 3981cd489..ab7df25dd 100644 --- a/src/mlpro/bf/systems/pool/second_order_system.py +++ b/src/mlpro/bf/systems/pool/second_order_system.py @@ -14,10 +14,12 @@ ## - add C_SAMPLE_FREQ : Specifies how often the system is sampled in a control cycle ## - add self._dt: Sampling time ## - update _simulate_reaction(), _reset() +## -- 2025-01-05 0.6.0 ASP class PT2: Refactoring +## - changed self.K to self._K ## ------------------------------------------------------------------------------------------------- """ -Ver. 0.5.0 (2024-12-30) +Ver. 0.6.0 (2025-01-05) This module provides a simple demo system that represent second order system. Further infos : https://www.circuitbread.com/tutorials/second-order-systems-2-3 @@ -87,15 +89,15 @@ def __init__( self, Start value of the control variable """ - self.K = p_K - self._D =p_D - self._omega_0 =p_omega_0 + self._K = p_K + self._D = p_D + self._omega_0 = p_omega_0 self._sys_num = p_sys_num self._y_start = p_y_start self._y = np.zeros(p_max_cycle*self.C_SAMPLE_FREQ+1) self._dy = np.zeros(p_max_cycle*self.C_SAMPLE_FREQ+1) self._y[0] = self._y_start - self._cycle = 1 + self._current_cycle = 1 super().__init__( p_id = p_id, p_name = p_name, @@ -133,7 +135,7 @@ def _reset(self, p_seed=None): self._dy = self._dy*0 #set start value of control_varibale self._y[0] = self._y_start - self._cycle = 1 + self._current_cycle = 1 ## ------------------------------------------------------------------------------------------------- @@ -153,7 +155,7 @@ def _state_equation(self,p_y:float, p_dy:float, p_u:float)-> float: """ #Calculating the second derivative (acceleration) - return -2 * self._D * self._omega_0 * p_dy - self._omega_0**2 * p_y + self._omega_0**2 * p_u*self.K + return -2 * self._D * self._omega_0 * p_dy - self._omega_0**2 * p_y + self._omega_0**2 * p_u*self._K ## ------------------------------------------------------------------------------------------------- @@ -166,45 +168,45 @@ def _simulate_reaction(self, p_state: State, p_action: Action, p_step = None): new_state = State( p_state_space = self.get_state_space()) # get control Variable - u= p_action.get_elem(p_id=agent_id).get_values()[0] + u = p_action.get_elem(p_id=agent_id).get_values()[0] for step in range(self.C_SAMPLE_FREQ): # Calculation R1-Coefficient of the first derivative - k1_y = self._dy[self._cycle-1] * self._dt + k1_y = self._dy[self._current_cycle-1] * self._dt # Calculation R1-Coefficient of the second derivative - k1_dy = self._state_equation(self._y[self._cycle-1], self._dy[self._cycle-1], u) * self._dt + k1_dy = self._state_equation(self._y[self._current_cycle - 1], self._dy[self._current_cycle - 1], u) * self._dt # Calculation R2-Coefficient of the second derivative - k2_y = (self._dy[self._cycle-1] + 0.5 * k1_dy) * self._dt + k2_y = (self._dy[self._current_cycle - 1] + 0.5 * k1_dy) * self._dt # Calculation R2-Coefficient of the second derivative - k2_dy = self._state_equation(self._y[self._cycle-1] + 0.5 * k1_y, self._dy[self._cycle-1] + 0.5 * k1_dy, u) * self._dt + k2_dy = self._state_equation(self._y[self._current_cycle - 1] + 0.5 * k1_y, self._dy[self._current_cycle - 1] + 0.5 * k1_dy, u) * self._dt # Calculation R3-Coefficient of the first derivative - k3_y = (self._dy[self._cycle-1] + 0.5 * k2_dy) * self._dt + k3_y = (self._dy[self._current_cycle - 1] + 0.5 * k2_dy) * self._dt # Calculation R3-Coefficient of the second derivative - k3_dy = self._state_equation(self._y[self._cycle-1] + 0.5 * k2_y, self._dy[self._cycle-1] + 0.5 * k2_dy, u) * self._dt + k3_dy = self._state_equation(self._y[self._current_cycle - 1] + 0.5 * k2_y, self._dy[self._current_cycle - 1] + 0.5 * k2_dy, u) * self._dt # Calculation R4-Coefficient of the first derivative - k4_y = (self._dy[self._cycle-1] + k3_dy) * self._dt + k4_y = (self._dy[self._current_cycle - 1] + k3_dy) * self._dt # Calculation R4-Coefficient of the second derivative - k4_dy = self._state_equation(self._y[self._cycle-1] + k3_y, self._dy[self._cycle-1] + k3_dy, u) * self._dt + k4_dy = self._state_equation(self._y[self._current_cycle - 1] + k3_y, self._dy[self._current_cycle - 1] + k3_dy, u) * self._dt # update von y und dy - self._y[self._cycle] = self._y[self._cycle-1] + (k1_y + 2 * k2_y + 2 * k3_y + k4_y) / 6 - self._dy[self._cycle] = self._dy[self._cycle-1] + (k1_dy + 2 * k2_dy + 2 * k3_dy + k4_dy) / 6 + self._y[self._current_cycle] = self._y[self._current_cycle - 1] + (k1_y + 2 * k2_y + 2 * k3_y + k4_y) / 6 + self._dy[self._current_cycle] = self._dy[self._current_cycle - 1] + (k1_dy + 2 * k2_dy + 2 * k3_dy + k4_dy) / 6 # Limit output - self._y[self._cycle] = max(self.C_BOUNDARIES[0],self._y[self._cycle]) - self._y[self._cycle] = min(self.C_BOUNDARIES[1],self._y[self._cycle]) + self._y[self._current_cycle] = max(self.C_BOUNDARIES[0],self._y[self._current_cycle]) + self._y[self._current_cycle] = min(self.C_BOUNDARIES[1],self._y[self._current_cycle]) #set values of new state state - new_state.values = [self._y[self._cycle]] - self._cycle += 1 + new_state.values = [self._y[self._current_cycle]] + self._current_cycle += 1 return new_state