From 8bad34c7c26fb679904ed3fc1dab9ee515e96797 Mon Sep 17 00:00:00 2001 From: Kyle Coble <53625197+Hackerman342@users.noreply.github.com> Date: Tue, 21 Mar 2023 18:16:33 -0400 Subject: [PATCH] Add PTO & h-bridge to Amiga PDO1 (#84) --- py/examples/vehicle_twist/main.py | 1 + py/farm_ng/canbus/packet.py | 113 +++++++++++++++++++++++++----- 2 files changed, 98 insertions(+), 16 deletions(-) diff --git a/py/examples/vehicle_twist/main.py b/py/examples/vehicle_twist/main.py index cc626c13..01055241 100644 --- a/py/examples/vehicle_twist/main.py +++ b/py/examples/vehicle_twist/main.py @@ -24,6 +24,7 @@ async def request_generator(twist: canbus_pb2.Twist2d) -> iter[canbus_pb2.SendVehicleTwistCommandReply]: while True: yield canbus_pb2.SendVehicleTwistCommandRequest(command=twist) + await asyncio.sleep(0.1) # Limit to 10 hz async def main(config: ClientConfig, twist: canbus_pb2.Twist2d) -> None: diff --git a/py/farm_ng/canbus/packet.py b/py/farm_ng/canbus/packet.py index bb600e7c..cbc2c8e1 100644 --- a/py/farm_ng/canbus/packet.py +++ b/py/farm_ng/canbus/packet.py @@ -42,6 +42,27 @@ class AmigaControlState(IntEnum): STATE_ESTOPPED = 6 +class ActuatorCommands(IntEnum): + passive = 0x0 + forward = 0x1 + stopped = 0x2 + reverse = 0x3 + + +def actuator_bits_cmd( + a0=ActuatorCommands.passive, a1=ActuatorCommands.passive, a2=ActuatorCommands.passive, a3=ActuatorCommands.passive +): + return a0.value + (a1.value << 2) + (a2.value << 4) + (a3.value << 6) + + +def actuator_bits_read(bits): + a0 = ActuatorCommands(bits & 0x3) + a1 = ActuatorCommands((bits >> 2) & 0x3) + a2 = ActuatorCommands((bits >> 4) & 0x3) + a3 = ActuatorCommands((bits >> 6) & 0x3) + return (a0, a1, a2, a3) + + class Packet: """Base class inherited by all CAN message data structures.""" @@ -67,7 +88,7 @@ def age(self): def make_amiga_rpdo1_proto( - state_req: AmigaControlState, cmd_speed: float, cmd_ang_rate: float + state_req: AmigaControlState, cmd_speed: float, cmd_ang_rate: float, pto_bits: int = 0x0, hbridge_bits: int = 0x0 ) -> canbus_pb2.RawCanbusMessage: """Creates a canbus_pb2.RawCanbusMessage. @@ -85,12 +106,21 @@ def make_amiga_rpdo1_proto( # TODO: add some checkers, or make python CHECK_API return canbus_pb2.RawCanbusMessage( id=AmigaRpdo1.cob_id + DASHBOARD_NODE_ID, - data=AmigaRpdo1(state_req=state_req, cmd_speed=cmd_speed, cmd_ang_rate=cmd_ang_rate).encode(), + data=AmigaRpdo1( + state_req=state_req, + cmd_speed=cmd_speed, + cmd_ang_rate=cmd_ang_rate, + pto_bits=pto_bits, + hbridge_bits=hbridge_bits, + ).encode(), ) class AmigaRpdo1(Packet): - """State, speed, and angular rate command (request) sent to the Amiga vehicle control unit (VCU)""" + """State, speed, and angular rate command (request) sent to the Amiga vehicle control unit (VCU). + + New in fw v0.1.9 / farm-ng-amiga v0.0.7: Add pto & hbridge control. Message data is now 8 bytes (was 5). + """ cob_id = 0x200 @@ -99,32 +129,59 @@ def __init__( state_req: AmigaControlState = AmigaControlState.STATE_ESTOPPED, cmd_speed: float = 0.0, cmd_ang_rate: float = 0.0, + pto_bits: int = 0x0, + hbridge_bits: int = 0x0, ): - self.format = "= v0.1.9." + # " New AmigaTpdo1 packets include more data. Support will be removed in farm_ng_amiga v0.0.9", + # stacklevel=2, + # ) + (self.state_req, cmd_speed, cmd_ang_rate) = unpack(self.legacy_format, data) + self.cmd_speed = cmd_speed / 1000.0 + self.cmd_ang_rate = cmd_ang_rate / 1000.0 + else: + (self.state_req, cmd_speed, cmd_ang_rate, self.pto_bits, self.hbridge_bits) = unpack(self.format, data) + self.cmd_speed = cmd_speed / 1000.0 + self.cmd_ang_rate = cmd_ang_rate / 1000.0 def __str__(self): return "AMIGA RPDO1 Request state {} Command speed {:0.3f} Command angular rate {:0.3f}".format( self.state_req, self.cmd_speed, self.cmd_ang_rate - ) + ) + " Command PTO bits 0x{:x} Command h-bridge bits 0x{:x}".format(self.pto_bits, self.hbridge_bits) class AmigaTpdo1(Packet): - """State, speed, and angular rate of the Amiga vehicle control unit (VCU)""" + """State, speed, and angular rate of the Amiga vehicle control unit (VCU). + + New in fw v0.1.9 / farm-ng-amiga v0.0.7: Add pto & hbridge control. Message data is now 8 bytes (was 5). + """ cob_id = 0x180 @@ -133,28 +190,52 @@ def __init__( state: AmigaControlState = AmigaControlState.STATE_ESTOPPED, meas_speed: float = 0.0, meas_ang_rate: float = 0.0, + pto_bits: int = 0x0, + hbridge_bits: int = 0x0, ): - self.format = "= v0.1.9." + # " New AmigaTpdo1 packets include more data. Support will be removed in farm_ng_amiga v0.0.9", + # stacklevel=2, + # ) + (self.state, meas_speed, meas_ang_rate) = unpack(self.legacy_format, data) + self.meas_speed = meas_speed / 1000.0 + self.meas_ang_rate = meas_ang_rate / 1000.0 + else: + (self.state, meas_speed, meas_ang_rate, self.pto_bits, self.hbridge_bits) = unpack(self.format, data) + self.meas_speed = meas_speed / 1000.0 + self.meas_ang_rate = meas_ang_rate / 1000.0 def __str__(self): return "AMIGA TPDO1 Amiga state {} Measured speed {:0.3f} Measured angular rate {:0.3f} @ time {}".format( self.state, self.meas_speed, self.meas_ang_rate, self.stamp.stamp - ) + ) + " PTO bits 0x{:x} h-bridge bits 0x{:x}".format(self.pto_bits, self.hbridge_bits) def parse_amiga_tpdo1_proto(message: canbus_pb2.RawCanbusMessage) -> AmigaTpdo1 | None: