87 lines
2.7 KiB
Python
87 lines
2.7 KiB
Python
"""
|
|
**********************************************************************
|
|
* 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") |