97 lines
2.8 KiB
Python
97 lines
2.8 KiB
Python
|
class Packet:
|
||
|
|
"""Represents an 8-byte I2C packet."""
|
||
|
|
LENGTH = 8
|
||
|
|
|
||
|
|
def __init__(self, data=None):
|
||
|
|
if data is None:
|
||
|
|
data = [0] * self.LENGTH
|
||
|
|
if len(data) != self.LENGTH:
|
||
|
|
raise ValueError(f"Packet must be {self.LENGTH} bytes")
|
||
|
|
self.bytes = data
|
||
|
|
|
||
|
|
@classmethod
|
||
|
|
def from_bytes(cls, byte_list):
|
||
|
|
"""Create Packet from list of bytes."""
|
||
|
|
return cls(byte_list[:cls.LENGTH])
|
||
|
|
|
||
|
|
def __getitem__(self, idx):
|
||
|
|
return self.bytes[idx]
|
||
|
|
|
||
|
|
def __setitem__(self, idx, value):
|
||
|
|
self.bytes[idx] = value
|
||
|
|
|
||
|
|
def __repr__(self):
|
||
|
|
fields = [f"Byte {i}: 0x{b:02X}" for i, b in enumerate(self.bytes)]
|
||
|
|
return "\n".join(fields)
|
||
|
|
|
||
|
|
def as_list(self):
|
||
|
|
return self.bytes
|
||
|
|
|
||
|
|
def is_valid(self):
|
||
|
|
"""Verify checksum (XOR of bytes 0-6 equals byte 7)."""
|
||
|
|
cs = 0
|
||
|
|
for b in self.bytes[:-1]:
|
||
|
|
cs ^= b
|
||
|
|
return cs == self.bytes[-1]
|
||
|
|
|
||
|
|
def create_motor_state_packet(fl=None, fr=None, bl=None, br=None):
|
||
|
|
"""
|
||
|
|
Create an 8-byte packet to set motor states for I2C transfer.
|
||
|
|
Packet format:
|
||
|
|
[0x01, 0x00, FL, FR, BL, BR, 0x00, checksum]
|
||
|
|
- 0x01: Command for Motor State Packet
|
||
|
|
- 0x00: Target/channel (set as needed)
|
||
|
|
- FL, FR, BL, BR: Motor states (0x00 = off, 0x01 = forward, 0x02 = backward)
|
||
|
|
- checksum: XOR of bytes 0-6
|
||
|
|
"""
|
||
|
|
def getByteForState(state):
|
||
|
|
if state:
|
||
|
|
return 0x01
|
||
|
|
elif not state:
|
||
|
|
return 0x02
|
||
|
|
else:
|
||
|
|
return 0x00
|
||
|
|
|
||
|
|
packet = [0x01, 0x00, getByteForState(fl), getByteForState(fr), getByteForState(bl), getByteForState(br), 0x00]
|
||
|
|
checksum = 0
|
||
|
|
for b in packet:
|
||
|
|
checksum ^= b
|
||
|
|
packet.append(checksum)
|
||
|
|
return Packet.from_bytes(bytes(packet))
|
||
|
|
|
||
|
|
def create_tail_wagging_packet(wagging=False):
|
||
|
|
"""
|
||
|
|
Create an 8-byte packet to set wagging for I2C transfer.
|
||
|
|
Packet format:
|
||
|
|
[0x02, 0x00, Wagging, 0x00, 0x00, 0x00, 0x00, checksum]
|
||
|
|
- 0x02: Command for Wagging Packet
|
||
|
|
- 0x00: Target/channel (set as needed)
|
||
|
|
- Wagging: 0x00 = false, 0x01 = true
|
||
|
|
- checksum: XOR of bytes 0-6
|
||
|
|
"""
|
||
|
|
packet = [0x02, 0x00, 0x01 if wagging else 0x00, 0x00, 0x00, 0x00, 0x00]
|
||
|
|
checksum = 0
|
||
|
|
for b in packet:
|
||
|
|
checksum ^= b
|
||
|
|
packet.append(checksum)
|
||
|
|
return Packet.from_bytes(bytes(packet))
|
||
|
|
|
||
|
|
def create_set_led_mode(mode=0):
|
||
|
|
"""
|
||
|
|
Create an 8-byte packet to set led mode for I2C transfer.
|
||
|
|
Packet format:
|
||
|
|
[0x02, 0x00, Mode, 0x00, 0x00, 0x00, 0x00, checksum]
|
||
|
|
- 0x02: Command for led mode
|
||
|
|
- 0x00: Target/channel (set as needed)
|
||
|
|
- Wagging: 0x00 = standard, 0x01 = rot, 0x02 = rainbow
|
||
|
|
- checksum: XOR of bytes 0-6
|
||
|
|
"""
|
||
|
|
packet = [0x02, 0x00, mode & 0xFF, 0x00, 0x00, 0x00, 0x00]
|
||
|
|
checksum = 0
|
||
|
|
for b in packet:
|
||
|
|
checksum ^= b
|
||
|
|
packet.append(checksum)
|
||
|
|
return Packet.from_bytes(bytes(packet))
|
||
|
|
|
||
|
|
|