feat: added some features

control/pca9685/servocontroller.py

@@ -0,0 +1,87 @@
+"""
+**********************************************************************
+* Filename    : Servo.py
+* Description : Driver module for servo, with PCA9685
+* Author      : Cavon
+* Brand       : SunFounder
+* E-mail      : service@sunfounder.com
+* Website     : www.sunfounder.com
+* Update      : Cavon    2016-09-13    New release
+*               Cavon    2016-09-21    Change channel from 1 to all
+**********************************************************************
+"""
+
+from .pca9685 import PWM, map_range
+
+
+class ServoController:
+    """Servo driver class"""
+    _MIN_PULSE_WIDTH = 600
+    _MAX_PULSE_WIDTH = 2400
+    _DEFAULT_PULSE_WIDTH = 1500
+    _FREQUENCY = 60
+
+    _DEBUG = False
+    _DEBUG_INFO = 'DEBUG "Servo.py":'
+
+    def __init__(self, lock=True, bus_number=None, address=0x40):
+        """ Init a servo on specific channel, this offset """
+        self._debug_("Debug on")
+        self.lock = lock
+
+        self.pwm = PWM(bus_number=bus_number, address=address)
+        self.frequency = self._FREQUENCY
+
+    def _debug_(self, message):
+        if self._DEBUG:
+            print(self._DEBUG_INFO, message)
+
+    def setup(self):
+        self.pwm.setup()
+
+    def _angle_to_analog(self, angle):
+        ''' Calculate 12-bit analog value from giving angle '''
+        pulse_wide = map_range(angle, 0, 180, self._MIN_PULSE_WIDTH, self._MAX_PULSE_WIDTH)
+        analog_value = int(float(pulse_wide) / 1000000 * self.frequency * 4096)
+        self._debug_('Angle %d equals Analog_value %d' % (angle, analog_value))
+        return analog_value
+
+    @property
+    def frequency(self):
+        return self._frequency
+
+    @frequency.setter
+    def frequency(self, value):
+        self._frequency = value
+        self.pwm.frequency = value
+
+    def write(self, channel, angle):
+        ''' Turn the servo with giving angle. '''
+        if self.lock:
+            if angle > 180:
+                angle = 180
+            if angle < 0:
+                angle = 0
+        else:
+            if angle < 0 or angle > 180:
+                raise ValueError("Servo \"{0}\" turn angle \"{1}\" is not in (0, 180).".format(channel, angle))
+        val = self._angle_to_analog(angle)
+        self.pwm.write(channel, 0, val)
+        self._debug_('Turn angle = %d' % angle)
+
+    @property
+    def debug(self):
+        return self._DEBUG
+
+    @debug.setter
+    def debug(self, debug):
+        ''' Set if debug information shows '''
+        if debug in (True, False):
+            self._DEBUG = debug
+        else:
+            raise ValueError('debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'.format(debug))
+
+        if self._DEBUG:
+            print(self._DEBUG_INFO, "Set debug on")
+        else:
+            print(self._DEBUG_INFO, "Set debug off")