mirror of
https://gitee.com/StackForce/stackforce_openmv
synced 2025-09-26 22:19:11 +08:00
35 lines
1.2 KiB
Python
35 lines
1.2 KiB
Python
import serial
|
|
import time
|
|
|
|
class MotorController:
|
|
def __init__(self):
|
|
self.ser = None # Initialize serial attribute
|
|
|
|
def initialize_serial(self, port='COM56', baudrate=115200, timeout=1):
|
|
"""Initialize the serial connection."""
|
|
self.ser = serial.Serial(port, baudrate, timeout=timeout)
|
|
|
|
def send_target(self, m0_target, m1_target):
|
|
command = f'T{m0_target} {m1_target}\n'
|
|
self.ser.write(command.encode())
|
|
|
|
def send_mode(self, m0_mode, m1_mode):
|
|
command = f'M{m0_mode} {m1_mode}\n'
|
|
self.ser.write(command.encode())
|
|
|
|
def send_pid(self, m0_p, m0_i, m0_d, m1_p, m1_i, m1_d):
|
|
command = f'P{m0_p} {m0_i} {m0_d} {m1_p} {m1_i} {m1_d}\n'
|
|
self.ser.write(command.encode())
|
|
|
|
def receive_motor_data(self):
|
|
if self.ser.in_waiting > 0:
|
|
line = self.ser.readline().decode('utf-8').strip()
|
|
if line.startswith('D'):
|
|
data = line[1:].split()
|
|
m0_angle = float(data[0])
|
|
m0_speed = float(data[1])
|
|
m1_angle = float(data[2])
|
|
m1_speed = float(data[3])
|
|
return m0_angle, m0_speed, m1_angle, m1_speed
|
|
return None
|