通过 UDP 更改数据更改钟摆位置

Question

我在 python 中有一个代码，通过 UDP 通信，我从 Raspberry Pi 4 接收数据，这些数据是 1 和 0，来自超声波传感器的信号接收（如果距离小于 5 厘米，则为 0如果它更大，则为 1)。 这是代码的一部分，另一部分考虑了钟摆的动态模型，在函数中我有两个所需的位置。这段代码的目的是让钟摆根据通过 UDP 接收的数据改变位置，但显然当通过 UDP 接收数据时，钟摆只占据第一个位置而不会改变到另一个位置，应该进行此更改在整个模拟过程中。这个问题有什么解决办法吗？ 这是我的代码：

import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import socket

def recibir_datos():
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind(('192.168.1.171', 15200))
    sock.settimeout(0.01)
    try:
        datos, addr = sock.recvfrom(1024)
        return int(datos.decode())
    except socket.timeout:
        return None

datos = []
dato_anterior = 0  # Valor inicial del dato anterior
def actualizar(i):
    global dato_anterior
    dato = recibir_datos()
    #print(dato)
    if dato is not None:  # Si se recibió un dato, se agrega a la lista de datos
        datos.append(dato)
        dato_anterior = dato  # Se actualiza el dato anterior
        
    else:  # Si no se recibió un dato, se agrega el dato anterior a la lista de datos
        datos.append(dato_anterior)
    ax3.clear()
    ax3.plot(t[:len(datos)], datos,'b',label='$Sensor$')
    ax3.set_xlim([0, tf])
    ax3.set_ylim([0, 1])
    ax3.set_xlabel('Tiempo [segundos]')
    ax3.set_ylabel('Señal del sensor')
    ax3.legend(loc = 'best')
    plt.title("SEÑAL DEL SENSOR", 
          fontdict={'family': 'serif', 
                    'color' : 'darkblue',
                    'weight': 'bold',
                    'size': 12})
    plt.grid(True)
    if len(datos) >= len(t):  # Detener la animación al finalizar la simulación
        ani_sensor.event_source.stop()
    return ax3.lines

ti = 0;h = 0.001;tf = 8
t = np.arange(ti, tf, h)

# Definir las constantes del péndulo
m = 1.0    # masa del péndulo
g = 9.81   # aceleración debido a la gravedad
l = 1.0    # longitud del péndulo

Kp = 350.0    # ganancia proporcional
Ki = 10.0     # ganancia integral
Kd = 170.0     # ganancia derivativa

integral_error = 0.0
prev_error = 0.0

def pendulum_dynamics(t, y):
    global integral_error, prev_error,dato_anterior
    theta, omega = y
    # Here is the switch of the position
    if dato_anterior == 0:
        theta_target = 60
    elif dato_anterior == 1:
        theta_target = 30

    error = theta_target - theta
    integral_error += error * dt
    derivative_error = (error - prev_error) / dt
    control_torque = Kp * error + Ki * integral_error + Kd * derivative_error

    alpha = (1 / (m * l**2)) * (control_torque - m * g * l * np.sin(theta))

    prev_error = error
    return [omega, alpha]

y0 = [np.deg2rad(0), 0.0]

t_start = 0.0
t_end = 8.0
dt = 0.001
t_span = (t_start, t_end)

sol = solve_ivp(pendulum_dynamics, t_span, y0, rtol=1e-3, h0=1e-3, mxstep=1e-3, method='RK45')

fig, ax = plt.subplots()
line_pos, = ax.plot(sol.t, np.rad2deg(sol.y[0]), label='Posición')
ax.set_xlabel('Tiempo [s]')
ax.set_ylabel('Ángulo [grados]')
ax.set_ylim([-180, 180])
ax.legend()
plt.grid(True)

fig2, ax2 = plt.subplots()
line_vel, = ax2.plot(sol.t, np.rad2deg(sol.y[1]), label='Velocidad')
ax2.set_xlabel('Tiempo [s]')
ax2.set_ylabel('Velocidad angular [rad/s]')
ax2.legend()
plt.grid(True)

fig3, ax3 = plt.subplots()

def update_position(frame):
    line_pos.set_data(sol.t[:frame], np.rad2deg(sol.y[0, :frame]))
    return line_pos,

def update_velocity(frame):
    line_vel.set_data(sol.t[:frame], np.rad2deg(sol.y[1, :frame]))
    return line_vel,

ani_pos = FuncAnimation(fig, update_position, frames=len(sol.t), interval=10, blit=True)
ani_vel = FuncAnimation(fig2, update_velocity, frames=len(sol.t), interval=10, blit=True)
ani_sensor = FuncAnimation(fig3, actualizar, frames=len(t), interval=10, blit=True)
plt.show()

我试图让模型在每次迭代时检查通过 UDP 接收的数据，并利用该数据来选择它应该将钟摆发送到的位置。