1 files changed, 28 insertions, 0 deletions
diff --git a/mg996r.py b/mg996r.py
new file mode 100644
index 0000000..cc70c24
--- /dev/null
+++ b/mg996r.py
@@ -0,0 +1,28 @@
+import logging
+
+from time import sleep
+from orangepwm import OrangePwm
+from pyA20.gpio import gpio
+from pyA20.gpio import port
+
+
+class MG996R:
+    def __init__(self, pin: str, prev: int = 360):
+        self.pwm = OrangePwm(50, getattr(port, pin))
+        self.prev = prev
+        gpio.init()
+
+        logging.debug(f'Initialized MG996R class with starting degree of {prev}')
+
+    def move(self, deg, delay: int = 0):
+        distance = abs(self.prev - deg)
+        sleep_time = distance / 60 * .5 + delay
+
+        duty = deg / 18 + 2
+        logging.debug(f'Moving to {deg} degrees, duty {duty:.0f}%, sleeping for {sleep_time:.1f} sec.')
+
+        self.prev = deg
+
+        self.pwm.start(duty)
+        sleep(sleep_time + delay)
+        self.pwm.stop()