自动导引车（AGV）控制系统Python

import gradio as gr
import utils
import serial
import numpy as np
import time
import matplotlib.pyplot as plt
import time
import ddsm115
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import Localization.wo as wo

plt.style.use('fivethirtyeight')

# 初始化AGV驱动，运动学模型和位置存储
drive = ddsm115.MotorControl(device="/dev/ttyUSB0")
drive.set_drive_mode(_id=1, _mode=2)
drive.set_drive_mode(_id=2, _mode=2)
kinematics = wo.Odometry(wheel_radius=0.05, wheelbase=0.432)
x_store = []
y_store = []
theta_store = []

# 控制模式，例如速度模式
mode = "velocity"

# 标志位，用于控制线路跟随和编码器反馈
line_follow = 0
encoder = 1

# AGV运动控制函数
def up():
    drive.send_rpm(1, -25)
    drive.send_rpm(2, 25)
    print("Robot moving forward")
    time.sleep(0.25)
    return "Forward"

# ... （其他运动控制函数）

def stop():
    global line_follow
    if line_follow == 1:
        line_follow = 0
        time.sleep(0.5)
    drive.send_rpm(1, 0)
    drive.send_rpm(2, 0)
    print("Robot stopped")
    return "Stop"

# AGV编码器反馈函数
def encoder():
    global encoder, x_store, y_store, theta_store
    global kinematics

    encoder = 1
    
    rpm_left = []
    rpm_right = []

    delta_t = 0.001
    rpm_L = 0
    rpm_R = 0
    while encoder == 1:
        try:
            rpm_L, cur_L = drive.get_motor_feedback(_id=2)
            rpm_R, cur_R = drive.get_motor_feedback(_id=1)
            print(f"Left RPM: {rpm_L} || Right RPM: {rpm_R}")

            omega_L = (rpm_L * 2 * np.pi)/60
            omega_R = (-(rpm_R) * 2 * np.pi)/60
            
            kinematics.non_linear_state_space(omega_L, omega_R, delta_t)

            x, y, theta = kinematics.get_pose()

            print(f"X: {x} m, Y: {y} m, Theta: {theta} rad")

            x_store.append(x)
            y_store.append(y)
            theta_store.append(theta)

            time.sleep(delta_t)
        except Exception as e:
            print(f"Error: {e}")

# ... （其他函数）

# AGV逆运动学控制函数
def inv_kinematics(x_destination = 0.0841, y_destination = -0.00043):
    global kinematics, x, y, theta
    delta_t = 0.001

    while True:
        omega_L, omega_R, l = kinematics.inverse_kinematics([x_destination, y_destination], lyapunov=True)

        rpm_L = (omega_L / (2*np.pi)) * 60
        rpm_R = (omega_R / (2*np.pi)) * 60

        if rpm_L > 100:
            rpm_L = 100
        if rpm_L < -100:
            rpm_L = -100
        if rpm_R > 100:
            rpm_R = 100
        if rpm_R < -100:
            rpm_R = -100

        drive.send_rpm(1, -(rpm_R))
        drive.send_rpm(2, rpm_L)

        rpm_L, cur_L = drive.get_motor_feedback(_id=2)
        rpm_R, cur_R = drive.get_motor_feedback(_id=1)

        omega_L = (rpm_L * 2 * np.pi)/60
        omega_R = (-(rpm_R) * 2 * np.pi)/60

        kinematics.non_linear_state_space(omega_L, omega_R, delta_t)

        x_t, y_t, theta_t = kinematics.get_pose()

        x_store.append(x_t)
        y_store.append(y_t)
        theta_store.append(theta_t)

        if l < 0.002:
            drive.send_rpm(0, -(rpm_R))
            drive.send_rpm(0, rpm_L)
            time.sleep(0.2)
            break
        
        time.sleep(delta_t)

# 创建 AGV 控制界面
with gr.Blocks() as demo:
    direction_text = gr.Textbox(label="Robot Direction")

    with gr.Column():
        with gr.Row():
            button_left_up = gr.Button(value="↖️")
            button_up = gr.Button(value="⬆️")
            button_right_up = gr.Button(value="↗️")
        with gr.Row():
            button_left_curve = gr.Button(value="↩️")
            button_stop = gr.Button(value="")
            button_right_curve = gr.Button(value="↪️")
        with gr.Row():
            button_left_down = gr.Button(value="↙️")
            button_down = gr.Button(value="⬇️")
            button_right_down = gr.Button(value="↘️")

    line_follow = gr.Button(value="Line Follower")
    encoders = gr.Button(value="Encoder")
    end_encoders = gr.Button(value="End Encoder")
    inv_kin = gr.Button(value="Inverse Kinematics")

    # 将按钮与相应的函数关联
    button_up.click(up, outputs=[direction_text])
    button_down.click(down, outputs=[direction_text])
    button_left_up.click(left_up, outputs=[direction_text])
    button_right_up.click(right_up, outputs=[direction_text])
    button_left_down.click(left_down, outputs=[direction_text])
    button_right_down.click(right_down, outputs=[direction_text])
    button_left_curve.click(left_curve, outputs=[direction_text])
    button_right_curve.click(right_curve, outputs=[direction_text])
    button_stop.click(stop, outputs=[direction_text])
    line_follow.click(line, outputs=[direction_text])
    encoders.click(encoder)
    end_encoders.click(end_encoder)
    inv_kin.click(inv_kinematics)

# 启动 AGV 控制界面
if __name__ == "__main__":
    try:
        demo.launch(share=True)

    except KeyboardInterrupt:
        drive.send_rpm(1, 0)
        drive.send_rpm(2, 0)
        demo.clear()
        demo.close()
    except Exception as e:
        demo.clear()
        demo.close()