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()
12-25
06-05
1129
![](https://csdnimg.cn/release/blogv2/dist/pc/img/readCountWhite.png)