feat: add servo handling

servo_handler.py

@@ -0,0 +1,52 @@
+from i2c_handler import I2CSlaveHandler, Packet
+from servo import Servo
+
+s1 = Servo(0)
+s1.middle()
+
+def servo_Map(x, in_min, in_max, out_min, out_max):
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
+ 
+def servo_Angle(angle, servo):
+    if angle < 0:
+        angle = 0
+    if angle > 180:
+        angle = 180
+    servo.goto(round(servo_Map(angle,0,180,0,1024))) # Convert range value to angle value
+    
+def move_body_parts(angle, servo):
+    for i in range(90, 90 + angle, 5):
+        servo_Angle(i, servo)
+        utime.sleep(0.05)
+    for i in range(90 + angle, 90 - angle, -5):
+        servo_Angle(i, servo)
+        utime.sleep(0.05)
+    for i in range(90 - angle, 90, 5):
+        servo_Angle(i, servo)
+        utime.sleep(0.05)
+
+class ServoHandler(I2CSlaveHandler):
+    def process_packet(self, packet: Packet):
+        """
+        Example: Override process_packet to perform actions based on packet command/type.
+        """
+        if packet[0] == 0x01:
+            if packet[2] == 0x00:
+                s1.middle()
+            if packet[2] == 0x01:
+                s1.move_body_parts(15, s1)
+        else:
+            print('Unknown command')
+
+# Usage example
+def main():
+    handler = ServoHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41)
+    print("ServoHandler I2C slave started")
+    try:
+        handler.handle()
+    except KeyboardInterrupt:
+        handler.deinit()
+        print("I2C slave stopped")
+
+if __name__ == "__main__":
+    main()