From 3aecfd6187199ce751430dd354c431edcdefdfbc Mon Sep 17 00:00:00 2001
From: Linkis_112 <jannis.nolte@freenet.de>
Date: Thu, 11 Sep 2025 20:29:20 +0200
Subject: [PATCH] feat: add servo handling

---
 servo.py         | 68 ++++++++++++++++++++++++++++++++++++++++++++++++
 servo_handler.py | 52 ++++++++++++++++++++++++++++++++++++
 2 files changed, 120 insertions(+)
 create mode 100644 servo.py
 create mode 100644 servo_handler.py

diff --git a/servo.py b/servo.py
new file mode 100644
index 0000000..0a51d82
--- /dev/null
+++ b/servo.py
@@ -0,0 +1,68 @@
+from machine import Pin, PWM
+
+class Servo:
+    """ A simple class for controlling a 9g servo with the Raspberry Pi Pico.
+ 
+    Attributes:
+ 
+        minVal: An integer denoting the minimum duty value for the servo motor.
+ 
+        maxVal: An integer denoting the maximum duty value for the servo motor.
+ 
+    """
+ 
+    def __init__(self, pin: int or Pin or PWM, minVal=2500, maxVal=7500):
+        """ Creates a new Servo Object.
+ 
+        args:
+ 
+            pin (int or machine.Pin or machine.PWM): Either an integer denoting the number of the GPIO pin or an already constructed Pin or PWM object that is connected to the servo.
+ 
+            minVal (int): Optional, denotes the minimum duty value to be used for this servo.
+ 
+            maxVal (int): Optional, denotes the maximum duty value to be used for this servo.
+ 
+        """
+ 
+        if isinstance(pin, int):
+            pin = Pin(pin, Pin.OUT)
+        if isinstance(pin, Pin):
+            self.__pwm = PWM(pin)
+        if isinstance(pin, PWM):
+            self.__pwm = pin
+        self.__pwm.freq(50)
+        self.minVal = minVal
+        self.maxVal = maxVal
+        
+ 
+    def deinit(self):
+        """ Deinitializes the underlying PWM object.
+ 
+        """
+        self.__pwm.deinit()
+ 
+    def goto(self, value: int):
+        """ Moves the servo to the specified position.
+ 
+        args:
+ 
+            value (int): The position to move to, represented by a value from 0 to 1024 (inclusive).
+ 
+        """
+        if value < 0:
+            value = 0
+        if value > 1024:
+            value = 1024
+        delta = self.maxVal-self.minVal
+        target = int(self.minVal + ((value / 1024) * delta))
+        self.__pwm.duty_u16(target)
+ 
+    def middle(self):
+        """ Moves the servo to the middle.
+        """
+        self.goto(512)
+ 
+    def free(self):
+        """ Allows the servo to be moved freely.
+        """
+        self.__pwm.duty_u16(0)
\ No newline at end of file
diff --git a/servo_handler.py b/servo_handler.py
new file mode 100644
index 0000000..f6e596a
--- /dev/null
+++ b/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()
\ No newline at end of file