--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/python/servos.py	Mon May 15 16:36:08 2017 +0200
@@ -0,0 +1,45 @@
+import pca9685
+import math
+
+
+TRIMS = [
+    0, -15, 0, # front left
+    0, 0,
+    10, -20, 0, # hind left
+    0, -10, .0, # hind right
+    0, 0,
+    0, -5, 0, # front right
+]
+
+
+class Servos:
+    def __init__(self, i2c, address=0x40, freq=50, min_us=600, max_us=2400,
+                 degrees=180):
+        self.period = 1000000 / freq
+        self.min_duty = self._us2duty(min_us)
+        self.max_duty = self._us2duty(max_us)
+        self.degrees = degrees
+        self.freq = freq
+        self.pca9685 = pca9685.PCA9685(i2c, address)
+        self.pca9685.freq(freq)
+
+    def _us2duty(self, value):
+        return int(4095 * value / self.period)
+
+    def position(self, index, degrees=None, radians=None, us=None, duty=None):
+        span = self.max_duty - self.min_duty
+        if degrees is not None:
+            duty = self.min_duty + span * degrees / self.degrees
+        elif radians is not None:
+            duty = self.min_duty + span * radians / math.radians(self.degrees)
+        elif us is not None:
+            duty = self._us2duty(us)
+        elif duty is not None:
+            pass
+        else:
+            return self.pca9685.duty(index)
+        duty = min(self.max_duty, max(self.min_duty, int(duty + TRIMS[index])))
+        self.pca9685.duty(index, duty)
+
+    def release(self, index):
+        self.pca9685.duty(index, 0)