mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 00:53:45 +08:00
firestar5683
22a8575199
Keep official openpilot ancestry and dependency licenses. Store resources directly in Git, remove LFS setup requirements, and leave the downloadable big model outside the source tree.
45 lines
1.3 KiB
Python
45 lines
1.3 KiB
Python
# python helpers for the body panda
|
|
import struct
|
|
|
|
from panda import Panda
|
|
|
|
class PandaBody(Panda):
|
|
|
|
MOTOR_LEFT: int = 1
|
|
MOTOR_RIGHT: int = 2
|
|
|
|
def __init__(self, *args, **kwargs):
|
|
super().__init__(*args, **kwargs)
|
|
self._rpm_left: int = 0
|
|
self._rpm_right: int = 0
|
|
|
|
@property
|
|
def rpm_left(self) -> int:
|
|
return self._rpm_left
|
|
|
|
@rpm_left.setter
|
|
def rpm_left(self, value: int) -> None:
|
|
self._rpm_left = int(value)
|
|
self._handle.controlWrite(Panda.REQUEST_OUT, 0xb3, self._rpm_left, self._rpm_right, b'')
|
|
|
|
@property
|
|
def rpm_right(self) -> int:
|
|
return self._rpm_right
|
|
|
|
@rpm_right.setter
|
|
def rpm_right(self, value: int) -> None:
|
|
self._rpm_right = int(value)
|
|
self._handle.controlWrite(Panda.REQUEST_OUT, 0xb3, self._rpm_left, self._rpm_right, b'')
|
|
|
|
# ****************** Motor Control *****************
|
|
@staticmethod
|
|
def _ensure_valid_motor(motor: int) -> None:
|
|
if motor not in (PandaBody.MOTOR_LEFT, PandaBody.MOTOR_RIGHT):
|
|
raise ValueError("motor must be MOTOR_LEFT or MOTOR_RIGHT")
|
|
|
|
def motor_get_encoder_state(self, motor: int) -> tuple[int, float]:
|
|
self._ensure_valid_motor(motor)
|
|
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xe2, motor, 0, 8)
|
|
position, rpm_milli = struct.unpack("<ii", dat)
|
|
return position, rpm_milli / 1000.0
|