From d6b048a9af928c0cf3b7310c7f1f7e00817a099e Mon Sep 17 00:00:00 2001 From: iabdalkader Date: Thu, 29 Jun 2017 16:17:21 +0200 Subject: [PATCH] Add servo shield example. --- usr/examples/15-Servo-Shield/main.py | 23 ++++++++++ usr/examples/15-Servo-Shield/pca9685.py | 58 +++++++++++++++++++++++++ usr/examples/15-Servo-Shield/servo.py | 33 ++++++++++++++ 3 files changed, 114 insertions(+) create mode 100644 usr/examples/15-Servo-Shield/main.py create mode 100644 usr/examples/15-Servo-Shield/pca9685.py create mode 100644 usr/examples/15-Servo-Shield/servo.py diff --git a/usr/examples/15-Servo-Shield/main.py b/usr/examples/15-Servo-Shield/main.py new file mode 100644 index 000000000..44e37d0ba --- /dev/null +++ b/usr/examples/15-Servo-Shield/main.py @@ -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) diff --git a/usr/examples/15-Servo-Shield/pca9685.py b/usr/examples/15-Servo-Shield/pca9685.py new file mode 100644 index 000000000..6cdab3d93 --- /dev/null +++ b/usr/examples/15-Servo-Shield/pca9685.py @@ -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('