feat: add motor impl

motor.py

@@ -0,0 +1,63 @@
+from i2c_handler import I2CSlaveHandler, Packet
+from machine import Pin
+
+# -high + low -> nach vorne
+# andersrum nach hinten
+
+in1_1 = Pin(18, Pin.OUT) # front right -
+in1_2 = Pin(19, Pin.OUT) # front right +
+in1_3 = Pin(20, Pin.OUT) # front left -
+in1_4 = Pin(21, Pin.OUT) # front left + 
+
+in2_1 = Pin(13, Pin.OUT) # back right -
+in2_2 = Pin(12, Pin.OUT) # back right +
+in2_3 = Pin(11, Pin.OUT) # back left -
+in2_4 = Pin(10, Pin.OUT) # back left +
+
+def set_motor(direction, pin1, pin2):
+    if direction == 0x00:  # off
+        pin1.value(0)
+        pin2.value(0)
+    elif direction == 0x01:  # forward
+        pin1.value(1)
+        pin2.value(0)
+    elif direction == 0x02:  # backward
+        pin1.value(0)
+        pin2.value(1)
+
+class MotorHandler(I2CSlaveHandler):
+    def process_packet(self, packet: Packet):
+        """
+        Example: Override process_packet to perform actions based on packet command/type.
+        """
+        if packet[0] == 0x01:
+            # Motor directions
+            fl, fr, bl, br = packet[2], packet[3], packet[4], packet[5]
+
+            # Map motors to their control pins
+            motors = {
+                'fl': (fl, in1_3, in1_4),
+                'fr': (fr, in1_1, in1_2),
+                'bl': (bl, in2_3, in2_4),
+                'br': (br, in2_1, in2_2)
+            }
+
+            # Set each motor
+            for name, (direction, pin1, pin2) in motors.items():
+                set_motor(direction, pin1, pin2)
+
+        else:
+            print("Unknown command")
+
+# Usage example
+def main():
+    handler = MotorHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41)
+    print("MotorHandler I2C slave started")
+    try:
+        handler.handle()
+    except KeyboardInterrupt:
+        handler.deinit()
+        print("I2C slave stopped")
+
+if __name__ == "__main__":
+    main()