Skip to content

Commit d338244

Browse files
pandiannanopandiannano
committed
feature/unit/8servos: 8-channel servo driver
1 parent 62b83b8 commit d338244

File tree

1 file changed

+213
-0
lines changed

1 file changed

+213
-0
lines changed

‎m5stack/libs/unit/servos8.py

Lines changed: 213 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,213 @@
1+
from machine import I2C
2+
from .pahub import PAHUBUnit
3+
from .unit_helper import UnitError
4+
import struct
5+
from time import sleep_ms
6+
import sys
7+
8+
if sys.platform != "esp32":
9+
from typing import Union
10+
11+
SERVOS_8_ADDR = 0x25
12+
13+
DIGITAL_IN_MODE = 0x00
14+
DIGITAL_OUT_MODE = 0x01
15+
ADC_IN_MODE = 0x02
16+
SERVO_CTRL_MODE = 0x03
17+
RGB_LED_MODE = 0x04
18+
PWM_DUTY_MODE = 0x05
19+
20+
MODE_REG = 0x00
21+
DIGITAL_OUT_REG = 0x10
22+
DIGITAL_IN_REG = 0x20
23+
ADC_8IN_REG = 0x30
24+
ADC_12IN_REG = 0x40
25+
SERVO_ANGLE_REG = 0x50
26+
SERVO_PULSE_REG = 0x60
27+
RGB_LED_REG = 0x70
28+
PWM_DUTY_REG = 0x90
29+
SERVO_CURRENT_REG = 0xA0
30+
FW_VER_REG = 0xFE
31+
I2C_ADDR_REG = 0xFF
32+
33+
34+
class SERVOS8Unit:
35+
36+
def __init__(self, i2c: Union[I2C, PAHUBUnit], addr = SERVOS_8_ADDR):
37+
"""
38+
slave_addr : 1 to 127
39+
"""
40+
self.servos8_i2c = i2c
41+
self.i2c_addr = addr
42+
if addr >= 1 and addr <= 127:
43+
self.i2c_addr = addr
44+
self.available()
45+
46+
def available(self):
47+
if not (self.i2c_addr in self.servos8_i2c.scan()):
48+
raise UnitError("8 Servos unit maybe not connect")
49+
50+
def get_mode(self, channel):
51+
"""
52+
get mode.
53+
channel: 0 to 7
54+
"""
55+
if channel >= 0 and channel <= 7:
56+
return self.servos8_i2c.readfrom_mem(self.i2c_addr, MODE_REG+channel, 1)[0]
57+
58+
def set_mode(self, mode, channel):
59+
"""
60+
set mode.
61+
channel: 0 to 7
62+
mode: DIGITAL_INPUT_MODE=0
63+
DIGITAL_OUTPUT_MODE=1
64+
ADC_INPUT_MODE=2
65+
SERVO_CTL_MODE=3
66+
RGB_LED_MODE=4
67+
PWM_DUTY_MODE=5
68+
"""
69+
if channel >= 0 and channel <= 7:
70+
self.servos8_i2c.writeto_mem(self.i2c_addr, MODE_REG+channel, bytearray([mode]))
71+
72+
def get_digital_input(self, channel):
73+
"""
74+
get digital input.
75+
channel: 0 to 7
76+
"""
77+
if channel >= 0 and channel <= 7:
78+
if DIGITAL_IN_MODE == self.get_mode(channel):
79+
return bool(self.servos8_i2c.readfrom_mem(self.i2c_addr, (DIGITAL_IN_REG + channel), 1)[0])
80+
81+
def set_output_value(self, value, channel):
82+
"""
83+
set digital output.
84+
channel: 0 to 7
85+
value : 0 or 1
86+
"""
87+
if channel >= 0 and channel <= 7:
88+
if DIGITAL_OUT_MODE == self.get_mode(channel):
89+
self.servos8_i2c.writeto_mem(self.i2c_addr, (DIGITAL_OUT_REG + channel), bytearray([value]))
90+
91+
def get_8bit_adc_raw(self, channel):
92+
"""
93+
get adc 8 bit.
94+
channel: 0 to 7
95+
return : 0 to 255
96+
"""
97+
if channel >= 0 and channel <= 7:
98+
if ADC_IN_MODE == self.get_mode(channel):
99+
return self.servos8_i2c.readfrom_mem(self.i2c_addr, (ADC_8IN_REG + channel), 1)[0]
100+
101+
def get_12bit_adc_raw(self, channel):
102+
"""
103+
get adc 12 bit.
104+
channel: 0 to 7
105+
return : 0 to 4095
106+
"""
107+
if channel >= 0 and channel <= 7:
108+
if ADC_IN_MODE == self.get_mode(channel):
109+
buf = self.servos8_i2c.readfrom_mem(self.i2c_addr, (ADC_12IN_REG + (channel * 2)), 2)
110+
return struct.unpack('<h', buf)[0]
111+
112+
def set_servo_angle(self, angle, channel):
113+
"""
114+
set servo angle.
115+
angle : 0 to 180
116+
channel: 0 to 7
117+
"""
118+
if channel >= 0 and channel <= 7:
119+
if SERVO_CTRL_MODE == self.get_mode(channel):
120+
self.servos8_i2c.writeto_mem(self.i2c_addr, (SERVO_ANGLE_REG + channel), bytearray([angle]))
121+
122+
def get_servo_angle(self, channel):
123+
"""
124+
get servo angle.
125+
channel: 0 to 7
126+
return : 0 to 180
127+
"""
128+
if channel >= 0 and channel <= 7:
129+
if SERVO_CTRL_MODE == self.get_mode(channel):
130+
return self.servos8_i2c.readfrom_mem(self.i2c_addr, (SERVO_ANGLE_REG + channel), 1)[0]
131+
132+
def set_servo_pulse(self, pulse, channel):
133+
"""
134+
set servo pulse.
135+
pulse : 500 to 2500
136+
channel: 0 to 7
137+
"""
138+
if channel >= 0 and channel <= 7:
139+
if SERVO_CTRL_MODE == self.get_mode(channel):
140+
pulse = struct.pack('<h', pulse)
141+
self.servos8_i2c.writeto_mem(self.i2c_addr, (SERVO_PULSE_REG + (channel * 2)), pulse)
142+
143+
def get_servo_pulse(self, channel):
144+
"""
145+
get servo pulse.
146+
channel: 0 to 7
147+
return : 500 to 2500
148+
"""
149+
if channel >= 0 and channel <= 7:
150+
if SERVO_CTRL_MODE == self.get_mode(channel):
151+
buf = self.servos8_i2c.readfrom_mem(self.i2c_addr, (SERVO_PULSE_REG + (channel * 2)), 2)
152+
return (struct.unpack('<h', buf)[0] // 10)
153+
154+
def set_rgb_led(self, rgb, channel):
155+
"""
156+
set rgb led.
157+
rgb: 0x000000 to 0xffffff
158+
channel: 0 to 7
159+
"""
160+
r = (rgb >> 16) & 0xff
161+
g = (rgb >> 8) & 0xff
162+
b = (rgb & 0xff)
163+
if channel >= 0 and channel <= 7:
164+
if RGB_LED_MODE == self.get_mode(channel):
165+
self.servos8_i2c.writeto_mem(self.i2c_addr, (RGB_LED_REG + (channel * 3)), bytearray([r, g, b]))
166+
167+
def get_rgb_led(self, channel):
168+
"""
169+
get rgb led.
170+
channel: 0 to 7
171+
return : (0 to 255, 0 to 255, 0 to 255)
172+
"""
173+
if channel >= 0 and channel <= 7:
174+
if RGB_LED_MODE == self.get_mode(channel):
175+
return list(self.servos8_i2c.readfrom_mem(self.i2c_addr, (RGB_LED_REG + (channel * 3)), 3))
176+
177+
def set_pwm_dutycycle(self, duty, channel):
178+
"""
179+
set pwm dutycycle.
180+
duty : 0 to 100
181+
channel: 0 to 7
182+
"""
183+
if channel >= 0 and channel <= 7:
184+
if PWM_DUTY_MODE == self.get_mode(channel):
185+
duty = max(min(duty, 100), 0)
186+
self.servos8_i2c.writeto_mem(self.i2c_addr, (PWM_DUTY_REG + channel), bytearray([duty]))
187+
188+
def get_input_current(self):
189+
"""
190+
get input current.
191+
"""
192+
buf = self.servos8_i2c.readfrom_mem(self.i2c_addr, SERVO_CURRENT_REG, 4)
193+
return round(struct.unpack('<f', buf)[0], 3)
194+
195+
def get_device_spec(self, mode):
196+
"""
197+
get device firmware version and i2c address.
198+
mode : 0xFE and 0xFF
199+
"""
200+
if mode >= FW_VER_REG and mode <= I2C_ADDR_REG:
201+
return self.servos8_i2c.readfrom_mem(self.i2c_addr, mode, 1)[0]
202+
203+
def set_i2c_address(self, addr):
204+
"""
205+
set i2c address.
206+
addr: 1 to 127
207+
"""
208+
if addr >= 1 and addr <= 127:
209+
if addr != self.i2c_addr:
210+
self.servos8_i2c.writeto_mem(self.i2c_addr, I2C_ADDR_REG, bytearray([addr]))
211+
self.i2c_addr = addr
212+
sleep_ms(150)
213+

0 commit comments

Comments
 (0)