40 lines
1.2 KiB
Python
40 lines
1.2 KiB
Python
|
from i2c_handler import I2CSlaveHandler, Packet
|
||
|
|
|
||
|
|
class MyRobotHandler(I2CSlaveHandler):
|
||
|
|
def process_packet(self, packet: Packet):
|
||
|
|
"""
|
||
|
|
Example: Override process_packet to perform actions based on packet command/type.
|
||
|
|
"""
|
||
|
|
cmd = packet[0]
|
||
|
|
target = packet[1]
|
||
|
|
speed = (packet[2] << 8) | packet[3]
|
||
|
|
# Convert speed from unsigned to signed if needed
|
||
|
|
if speed & 0x8000:
|
||
|
|
speed -= 0x10000
|
||
|
|
|
||
|
|
print(f"Custom handler: Command 0x{cmd:02X}, Target 0x{target:02X}, Speed {speed}")
|
||
|
|
|
||
|
|
# Example: Perform a motor action for command 0x01
|
||
|
|
if cmd == 0x01: # Set Motor Speed
|
||
|
|
print(f"Setting motor {target} speed to {speed}")
|
||
|
|
# TODO: Add your motor control code here
|
||
|
|
|
||
|
|
elif cmd == 0xFF: # Emergency Stop
|
||
|
|
print("Emergency stop received!")
|
||
|
|
# TODO: Add your emergency stop code here
|
||
|
|
|
||
|
|
else:
|
||
|
|
print("Unknown command")
|
||
|
|
|
||
|
|
# Usage example
|
||
|
|
def main():
|
||
|
|
handler = MyRobotHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41)
|
||
|
|
print("MyRobotHandler I2C slave started")
|
||
|
|
try:
|
||
|
|
handler.handle()
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
handler.deinit()
|
||
|
|
print("I2C slave stopped")
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|