RickRongen/WallE
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

feat: added some features

Browse Source
This commit is contained in:
Rick Rongen
2020-05-17 19:38:08 +02:00
parent f69cc0b45c
commit 8b268ef48c
8 changed files with 425 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
control/pca9685/__init__.py Normal file
View File

@@ -0,0 +1,4 @@
from .pca9685 import PWM, map_range
from .servocontroller import ServoController
__all__ = ['PWM', 'ServoController', 'map_range']

151
control/pca9685/pca9685.py Normal file
View File

@@ -0,0 +1,151 @@
#!/usr/bin/python
'''
**********************************************************************
* Filename : PCA9685.py
* Description : A driver module for PCA9685
* Author : Cavon
* Brand : SunFounder
* E-mail : service@sunfounder.com
* Website : www.sunfounder.com
* Version : v2.0.0
**********************************************************************
'''
import smbus
import time
import math
def map_range(x, in_min, in_max, out_min, out_max):
"""To map the value from arange to another"""
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
class PWM:
"""A PWM control class for PCA9685."""
_MODE1 = 0x00
_MODE2 = 0x01
_SUBADR1 = 0x02
_SUBADR2 = 0x03
_SUBADR3 = 0x04
_PRESCALE = 0xFE
_LED0_ON_L = 0x06
_LED0_ON_H = 0x07
_LED0_OFF_L = 0x08
_LED0_OFF_H = 0x09
_ALL_LED_ON_L = 0xFA
_ALL_LED_ON_H = 0xFB
_ALL_LED_OFF_L = 0xFC
_ALL_LED_OFF_H = 0xFD
_RESTART = 0x80
_SLEEP = 0x10
_ALLCALL = 0x01
_INVRT = 0x10
_OUTDRV = 0x04
_DEBUG = False
_DEBUG_INFO = 'DEBUG "PCA9685.py":'
def __init__(self, bus_number=None, address=0x40):
self.address = address
if bus_number is None:
self.bus_number = 1
else:
self.bus_number = bus_number
self.bus = smbus.SMBus(self.bus_number)
self._frequency = 60
def _debug_(self, message):
if self._DEBUG:
print(self._DEBUG_INFO, message)
def setup(self):
'''Init the class with bus_number and address'''
self._debug_('Reseting PCA9685 MODE1 (without SLEEP) and MODE2')
self.write_all_value(0, 0)
self._write_byte_data(self._MODE2, self._OUTDRV)
self._write_byte_data(self._MODE1, self._ALLCALL)
time.sleep(0.005)
mode1 = self._read_byte_data(self._MODE1)
mode1 = mode1 & ~self._SLEEP
self._write_byte_data(self._MODE1, mode1)
time.sleep(0.005)
self._frequency = 60
def _write_byte_data(self, reg, value):
'''Write data to I2C with self.address'''
self._debug_('Writing value %2X to %2X' % (value, reg))
try:
self.bus.write_byte_data(self.address, reg, value)
except Exception as i:
print(i)
def _read_byte_data(self, reg):
'''Read data from I2C with self.address'''
self._debug_('Reading value from %2X' % reg)
try:
results = self.bus.read_byte_data(self.address, reg)
return results
except Exception as i:
print(i)
@property
def frequency(self):
return self._frequency
@frequency.setter
def frequency(self, freq):
"""Set PWM frequency"""
self._debug_('Set frequency to %d' % freq)
self._frequency = freq
prescale_value = 25000000.0
prescale_value /= 4096.0
prescale_value /= float(freq)
prescale_value -= 1.0
self._debug_('Setting PWM frequency to %d Hz' % freq)
self._debug_('Estimated pre-scale: %d' % prescale_value)
prescale = math.floor(prescale_value + 0.5)
self._debug_('Final pre-scale: %d' % prescale)
old_mode = self._read_byte_data(self._MODE1);
new_mode = (old_mode & 0x7F) | 0x10
self._write_byte_data(self._MODE1, new_mode)
self._write_byte_data(self._PRESCALE, int(math.floor(prescale)))
self._write_byte_data(self._MODE1, old_mode)
time.sleep(0.005)
self._write_byte_data(self._MODE1, old_mode | 0x80)
def write(self, channel, on, off):
"""Set on and off value on specific channel"""
self._debug_('Set channel "%d" to value "%d"' % (channel, off))
self._write_byte_data(self._LED0_ON_L + 4 * channel, on & 0xFF)
self._write_byte_data(self._LED0_ON_H + 4 * channel, on >> 8)
self._write_byte_data(self._LED0_OFF_L + 4 * channel, off & 0xFF)
self._write_byte_data(self._LED0_OFF_H + 4 * channel, off >> 8)
def write_all_value(self, on, off):
"""Set on and off value on all channel"""
self._debug_('Set all channel to value "%d"' % (off))
self._write_byte_data(self._ALL_LED_ON_L, on & 0xFF)
self._write_byte_data(self._ALL_LED_ON_H, on >> 8)
self._write_byte_data(self._ALL_LED_OFF_L, off & 0xFF)
self._write_byte_data(self._ALL_LED_OFF_H, off >> 8)
@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")

87
control/pca9685/servocontroller.py Normal file
View File

@@ -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")