ydy0615
74a2d8c355
new file: Legs_controller.py modified: README.md new file: __pycache__/Legs_controller.cpython-310.pyc new file: __pycache__/Legs_controller.cpython-313.pyc new file: __pycache__/balance.cpython-310.pyc new file: __pycache__/gradio.cpython-310.pyc new file: __pycache__/gradio.cpython-313.pyc new file: app_ui.py new file: balance.py new file: build/.cmake/api/v1/query/client-vscode/query.json new file: build/.cmake/api/v1/reply/cache-v2-ae4a9db768b4bbb36baa.json new file: build/.cmake/api/v1/reply/cmakeFiles-v1-389eb8769a8295e7571d.json new file: build/.cmake/api/v1/reply/codemodel-v2-5ea8cfc0b9263cbe5ae5.json new file: build/.cmake/api/v1/reply/directory-.-Debug-f5ebdc15457944623624.json new file: build/.cmake/api/v1/reply/index-2025-09-11T04-53-20-0515.json new file: build/.cmake/api/v1/reply/target-imu_py-Debug-3913d741f2156d7faae9.json new file: build/.cmake/api/v1/reply/toolchains-v1-3105704d088db7adbfb5.json new file: build/CMakeCache.txt new file: build/CMakeFiles/3.22.1/CMakeCXXCompiler.cmake new file: build/CMakeFiles/3.22.1/CMakeDetermineCompilerABI_CXX.bin new file: build/CMakeFiles/3.22.1/CMakeSystem.cmake new file: build/CMakeFiles/3.22.1/CompilerIdCXX/CMakeCXXCompilerId.cpp new file: build/CMakeFiles/3.22.1/CompilerIdCXX/a.out new file: build/CMakeFiles/CMakeDirectoryInformation.cmake new file: build/CMakeFiles/CMakeOutput.log new file: build/CMakeFiles/Makefile.cmake new file: build/CMakeFiles/Makefile2 new file: build/CMakeFiles/TargetDirectories.txt new file: build/CMakeFiles/cmake.check_cache new file: build/CMakeFiles/imu_py.dir/DependInfo.cmake new file: build/CMakeFiles/imu_py.dir/build.make new file: build/CMakeFiles/imu_py.dir/cmake_clean.cmake new file: build/CMakeFiles/imu_py.dir/compiler_depend.make new file: build/CMakeFiles/imu_py.dir/compiler_depend.ts new file: build/CMakeFiles/imu_py.dir/depend.make new file: build/CMakeFiles/imu_py.dir/flags.make new file: build/CMakeFiles/imu_py.dir/link.txt new file: build/CMakeFiles/imu_py.dir/progress.make new file: build/CMakeFiles/progress.marks new file: build/Makefile new file: build/cmake_install.cmake new file: build/compile_commands.json new file: dm_imu/bsp_crc.cpp new file: dm_imu/bsp_crc.h new file: dm_imu/imu_data.csv new file: dm_imu/imu_driver.cpp new file: dm_imu/imu_driver.h new file: dm_imu/imu_plot.png new file: dm_imu/plot_imu.py new file: dm_imu/test_imu.cpp new file: imu_data.csv new file: pybind_imu/CMakeLists.txt new file: pybind_imu/README.md new file: pybind_imu/build/CMakeCache.txt new file: pybind_imu/build/CMakeFiles/3.22.1/CMakeCXXCompiler.cmake new file: pybind_imu/build/CMakeFiles/3.22.1/CMakeDetermineCompilerABI_CXX.bin new file: pybind_imu/build/CMakeFiles/3.22.1/CMakeSystem.cmake new file: pybind_imu/build/CMakeFiles/3.22.1/CompilerIdCXX/CMakeCXXCompilerId.cpp new file: pybind_imu/build/CMakeFiles/3.22.1/CompilerIdCXX/a.out new file: pybind_imu/build/CMakeFiles/CMakeDirectoryInformation.cmake new file: pybind_imu/build/CMakeFiles/CMakeOutput.log new file: pybind_imu/build/CMakeFiles/Makefile.cmake new file: pybind_imu/build/CMakeFiles/Makefile2 new file: pybind_imu/build/CMakeFiles/TargetDirectories.txt new file: pybind_imu/build/CMakeFiles/cmake.check_cache new file: pybind_imu/build/CMakeFiles/imu_py.dir/DependInfo.cmake new file: pybind_imu/build/CMakeFiles/imu_py.dir/build.make new file: pybind_imu/build/CMakeFiles/imu_py.dir/cmake_clean.cmake new file: pybind_imu/build/CMakeFiles/imu_py.dir/compiler_depend.make new file: pybind_imu/build/CMakeFiles/imu_py.dir/compiler_depend.ts new file: pybind_imu/build/CMakeFiles/imu_py.dir/depend.make new file: pybind_imu/build/CMakeFiles/imu_py.dir/flags.make new file: pybind_imu/build/CMakeFiles/imu_py.dir/home/allenyuan/balance/dm_imu/bsp_crc.cpp.o new file: pybind_imu/build/CMakeFiles/imu_py.dir/home/allenyuan/balance/dm_imu/bsp_crc.cpp.o.d new file: pybind_imu/build/CMakeFiles/imu_py.dir/home/allenyuan/balance/dm_imu/imu_driver.cpp.o new file: pybind_imu/build/CMakeFiles/imu_py.dir/home/allenyuan/balance/dm_imu/imu_driver.cpp.o.d new file: pybind_imu/build/CMakeFiles/imu_py.dir/link.txt new file: pybind_imu/build/CMakeFiles/imu_py.dir/progress.make new file: pybind_imu/build/CMakeFiles/imu_py.dir/pybind_imu.cpp.o new file: pybind_imu/build/CMakeFiles/imu_py.dir/pybind_imu.cpp.o.d new file: pybind_imu/build/CMakeFiles/progress.marks new file: pybind_imu/build/Makefile new file: pybind_imu/build/cmake_install.cmake new file: pybind_imu/build/imu_py.cpython-310-aarch64-linux-gnu.so new file: pybind_imu/example.py new file: pybind_imu/pybind_imu.cpp new file: src/example.py new file: test_imu new file: u2can/DM_CAN.py new file: u2can/DM_Motor_Test.py new file: u2can/LICENSE new file: u2can/README.md new file: u2can/__pycache__/DM_CAN.cpython-310.pyc new file: u2can/__pycache__/DM_CAN.cpython-312.pyc new file: u2can/__pycache__/DM_CAN.cpython-313.pyc new file: u2can/__pycache__/gradio.cpython-313.pyc new file: u2can/__pycache__/motor_interface.cpython-313.pyc new file: u2can/motor_interface.py new file: u2can/requirements.txt new file: ui.log
116 lines
2.9 KiB
Python
116 lines
2.9 KiB
Python
import math
|
|
from DM_CAN import *
|
|
import serial
|
|
import time
|
|
import imu_py
|
|
|
|
Motor1=Motor(DM_Motor_Type.DM4340,0x01,0x11)
|
|
Motor2=Motor(DM_Motor_Type.DM4340,0x02,0x12)
|
|
Motor3=Motor(DM_Motor_Type.DM4340,0x03,0x13)
|
|
Motor4=Motor(DM_Motor_Type.DM4340,0x04,0x14)
|
|
|
|
Wheel1=Motor(DM_Motor_Type.DMH6215,0x05,0x15)
|
|
Wheel2=Motor(DM_Motor_Type.DMH6215,0x06,0x16)
|
|
Wheel3=Motor(DM_Motor_Type.DMH6215,0x07,0x17)
|
|
Wheel4=Motor(DM_Motor_Type.DMH6215,0x08,0x18)
|
|
|
|
serial_device = serial.Serial('/dev/ttyACM1', 921600, timeout=0.5)
|
|
imu = imu_py.DmImu("/dev/ttyACM0", 921600)
|
|
imu.start()
|
|
|
|
MC1=MotorControl(serial_device)
|
|
MC1.addMotor(Motor1)
|
|
MC1.addMotor(Motor2)
|
|
MC1.addMotor(Motor3)
|
|
MC1.addMotor(Motor4)
|
|
|
|
MC1.enable(Motor1)
|
|
MC1.enable(Motor2)
|
|
MC1.enable(Motor3)
|
|
MC1.enable(Motor4)
|
|
i=0
|
|
|
|
print(Motor1.getTorque())
|
|
print(Motor2.getTorque())
|
|
print(Motor3.getTorque())
|
|
print(Motor4.getTorque())
|
|
|
|
time.sleep(0.01)
|
|
|
|
MC1.control_Pos_Vel(Motor1,-0.85,0.5)
|
|
MC1.control_Pos_Vel(Motor2,0.85,0.5)
|
|
MC1.control_Pos_Vel(Motor3,0.85,0.5)
|
|
MC1.control_Pos_Vel(Motor4,-0.85,0.5)
|
|
|
|
print("#####################")
|
|
|
|
time.sleep(5)
|
|
|
|
offs1=0.0
|
|
offs2=0.0
|
|
offs3=0.0
|
|
offs4=0.0
|
|
for i in range(1,10000):
|
|
MC1.control_Pos_Vel(Motor1,-0.85+offs1,12)
|
|
MC1.control_Pos_Vel(Motor2,0.85-offs2,12)
|
|
MC1.control_Pos_Vel(Motor3,0.85-offs3,12)
|
|
MC1.control_Pos_Vel(Motor4,-0.85+offs4,12)
|
|
|
|
data = imu.getData()
|
|
if data['pitch']>0.2:
|
|
offs1=offs1+0.0001*(data['pitch'])*pow(data['pitch'],0.5)
|
|
offs2=offs2+0.0001*(data['pitch'])*pow(data['pitch'],0.5)
|
|
if data['pitch']<-0.2:
|
|
offs3=offs3+0.0001*(-data['pitch'])*pow(-data['pitch'],0.5)
|
|
offs4=offs4+0.0001*(-data['pitch'])*pow(-data['pitch'],0.5)
|
|
if data['roll']<-0.2:
|
|
offs1=offs1+0.0001*(-data['roll'])*pow(-data['roll'],0.2)
|
|
offs4=offs4+0.0001*(-data['roll'])*pow(-data['roll'],0.2)
|
|
if data['roll']>0.2:
|
|
offs2=offs2+0.0001*(data['roll'])*pow(data['roll'],0.2)
|
|
offs3=offs3+0.0001*(data['roll'])*pow(data['roll'],0.2)
|
|
add = min(offs1,offs2,offs3,offs4)
|
|
offs1=offs1-add
|
|
offs2=offs2-add
|
|
offs3=offs3-add
|
|
offs4=offs4-add
|
|
if offs1>0.5: offs1=0.5
|
|
if offs2>0.5: offs2=0.5
|
|
if offs3>0.5: offs3=0.5
|
|
if offs4>0.5: offs4=0.5
|
|
print(f"euler: (roll={data['roll']:.2f}, pitch={data['pitch']:.2f}, yaw={data['yaw']:.2f})")
|
|
print(offs3,offs4,offs1,offs2,data['pitch'])
|
|
time.sleep(0.001)
|
|
|
|
MC1.disable(Wheel1)
|
|
MC1.disable(Wheel2)
|
|
MC1.disable(Wheel3)
|
|
MC1.disable(Wheel4)
|
|
|
|
MC1.refresh_motor_status(Motor1)
|
|
MC1.refresh_motor_status(Motor2)
|
|
MC1.refresh_motor_status(Motor3)
|
|
MC1.refresh_motor_status(Motor4)
|
|
|
|
time.sleep(0.1)
|
|
|
|
print(Motor1.getTorque())
|
|
print(Motor2.getTorque())
|
|
print(Motor3.getTorque())
|
|
print(Motor4.getTorque())
|
|
|
|
MC1.control_Pos_Vel(Motor1,0.0,0.5)
|
|
MC1.control_Pos_Vel(Motor2,0.0,0.5)
|
|
MC1.control_Pos_Vel(Motor3,0.0,0.5)
|
|
MC1.control_Pos_Vel(Motor4,0.0,0.5)
|
|
|
|
time.sleep(3)
|
|
|
|
MC1.disable(Motor1)
|
|
MC1.disable(Motor2)
|
|
MC1.disable(Motor3)
|
|
MC1.disable(Motor4)
|
|
|
|
|
|
serial_device.close()
|
|
# imu.stop() |