mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Add servo shield example.
This commit is contained in:
parent
97cbeeefe2
commit
d6b048a9af
23
usr/examples/15-Servo-Shield/main.py
Normal file
23
usr/examples/15-Servo-Shield/main.py
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
# Servo Shield Example.
|
||||||
|
#
|
||||||
|
# This example demonstrates the servo shield. Please follow these steps:
|
||||||
|
#
|
||||||
|
# 1. Connect a servo to any PWM output.
|
||||||
|
# 2. Connect a 3.7v battery (or 5V source) to VIN and GND.
|
||||||
|
# 3. Copy pca9685.py and servo.py to OpenMV and reset it.
|
||||||
|
# 4. Connect and run this script in the IDE.
|
||||||
|
|
||||||
|
import time
|
||||||
|
from servo import Servos
|
||||||
|
from machine import I2C, Pin
|
||||||
|
|
||||||
|
i2c = I2C(sda=Pin('P5'), scl=Pin('P4'))
|
||||||
|
servo = Servos(i2c, address=0x40, freq=50, min_us=650, max_us=2800, degrees=180)
|
||||||
|
|
||||||
|
while True:
|
||||||
|
for i in range(0, 8):
|
||||||
|
servo.position(i, 0)
|
||||||
|
time.sleep(500)
|
||||||
|
for i in range(0, 8):
|
||||||
|
servo.position(i, 180)
|
||||||
|
time.sleep(500)
|
||||||
58
usr/examples/15-Servo-Shield/pca9685.py
Normal file
58
usr/examples/15-Servo-Shield/pca9685.py
Normal file
@ -0,0 +1,58 @@
|
|||||||
|
import utime
|
||||||
|
import ustruct
|
||||||
|
|
||||||
|
class PCA9685:
|
||||||
|
def __init__(self, i2c, address=0x40):
|
||||||
|
self.i2c = i2c
|
||||||
|
self.address = address
|
||||||
|
self.reset()
|
||||||
|
|
||||||
|
def _write(self, address, value):
|
||||||
|
self.i2c.writeto_mem(self.address, address, bytearray([value]))
|
||||||
|
|
||||||
|
def _read(self, address):
|
||||||
|
return self.i2c.readfrom_mem(self.address, address, 1)[0]
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
self._write(0x00, 0x00) # Mode1
|
||||||
|
|
||||||
|
def freq(self, freq=None):
|
||||||
|
if freq is None:
|
||||||
|
return int(25000000.0 / 4096 / (self._read(0xfe) - 0.5))
|
||||||
|
prescale = int(25000000.0 / 4096.0 / freq + 0.5)
|
||||||
|
old_mode = self._read(0x00) # Mode 1
|
||||||
|
self._write(0x00, (old_mode & 0x7F) | 0x10) # Mode 1, sleep
|
||||||
|
self._write(0xfe, prescale) # Prescale
|
||||||
|
self._write(0x00, old_mode) # Mode 1
|
||||||
|
utime.sleep_us(5)
|
||||||
|
self._write(0x00, old_mode | 0xa1) # Mode 1, autoincrement on
|
||||||
|
|
||||||
|
def pwm(self, index, on=None, off=None):
|
||||||
|
if on is None or off is None:
|
||||||
|
data = self.i2c.readfrom_mem(self.address, 0x06 + 4 * index, 4)
|
||||||
|
return ustruct.unpack('<HH', data)
|
||||||
|
data = ustruct.pack('<HH', on, off)
|
||||||
|
self.i2c.writeto_mem(self.address, 0x06 + 4 * index, data)
|
||||||
|
|
||||||
|
def duty(self, index, value=None, invert=False):
|
||||||
|
if value is None:
|
||||||
|
pwm = self.pwm(index)
|
||||||
|
if pwm == (0, 4096):
|
||||||
|
value = 0
|
||||||
|
elif pwm == (4096, 0):
|
||||||
|
value = 4095
|
||||||
|
value = pwm[1]
|
||||||
|
if invert:
|
||||||
|
value = 4095 - value
|
||||||
|
return value
|
||||||
|
if not 0 <= value <= 4095:
|
||||||
|
raise ValueError("Out of range")
|
||||||
|
if invert:
|
||||||
|
value = 4095 - value
|
||||||
|
if value == 0:
|
||||||
|
self.pwm(index, 0, 4096)
|
||||||
|
elif value == 4095:
|
||||||
|
self.pwm(index, 4096, 0)
|
||||||
|
else:
|
||||||
|
self.pwm(index, 0, value)
|
||||||
|
|
||||||
33
usr/examples/15-Servo-Shield/servo.py
Normal file
33
usr/examples/15-Servo-Shield/servo.py
Normal file
@ -0,0 +1,33 @@
|
|||||||
|
import pca9685
|
||||||
|
import math
|
||||||
|
|
||||||
|
class Servos:
|
||||||
|
def __init__(self, i2c, address=0x40, freq=50, min_us=600, max_us=2400, degrees=180):
|
||||||
|
self.period = 1000000 / freq
|
||||||
|
self.min_duty = self._us2duty(min_us)
|
||||||
|
self.max_duty = self._us2duty(max_us)
|
||||||
|
self.degrees = degrees
|
||||||
|
self.freq = freq
|
||||||
|
self.pca9685 = pca9685.PCA9685(i2c, address)
|
||||||
|
self.pca9685.freq(freq)
|
||||||
|
|
||||||
|
def _us2duty(self, value):
|
||||||
|
return int(4095 * value / self.period)
|
||||||
|
|
||||||
|
def position(self, index, degrees=None, radians=None, us=None, duty=None):
|
||||||
|
span = self.max_duty - self.min_duty
|
||||||
|
if degrees is not None:
|
||||||
|
duty = self.min_duty + span * degrees / self.degrees
|
||||||
|
elif radians is not None:
|
||||||
|
duty = self.min_duty + span * radians / math.radians(self.degrees)
|
||||||
|
elif us is not None:
|
||||||
|
duty = self._us2duty(us)
|
||||||
|
elif duty is not None:
|
||||||
|
pass
|
||||||
|
else:
|
||||||
|
return self.pca9685.duty(index)
|
||||||
|
duty = min(self.max_duty, max(self.min_duty, int(duty)))
|
||||||
|
self.pca9685.duty(index, duty)
|
||||||
|
|
||||||
|
def release(self, index):
|
||||||
|
self.pca9685.duty(index, 0)
|
||||||
Loading…
Reference in New Issue
Block a user