Skip to content

Commit

Permalink
BF: Controlled system PT2 #1085
Browse files Browse the repository at this point in the history
  • Loading branch information
@amesin13
amesin13 committed Jan 6, 2025
1 parent a52b7bd commit 7b4449c
Showing 1 changed file with 24 additions and 22 deletions.
46 changes: 24 additions & 22 deletions src/mlpro/bf/systems/pool/second_order_system.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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


## -------------------------------------------------------------------------------------------------
Expand All @@ -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


## -------------------------------------------------------------------------------------------------
Expand All @@ -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

0 comments on commit 7b4449c

Please sign in to comment.