OpenMV：21控制多个舵机(需要模块PCA9685)

今天我们来学习下 OpenMV的舵机拓展板来控制多个舵机同时使用

如果我们想控制三个以上的舵机，就需要用到舵机拓展板PCA9685

PCA9685使用的是I2C协议，我们可以直接使用PCA9685舵机拓展板来控制八个舵机

舵机的供电采用的是4.8V-6V的电源，我们可以用四节~五节干电池来进行供电(一节干电池就是1.2V的电压) ——不能使用3.7V的锂离子电池供电！

对于电源的供电，我们直接将电源连接到OpenMV的VCC和GND两个引脚即可

连接

将电机拓展板PCA9685插到OpenMV上，电源（4.8V~6V）连接到右侧的VCC和GND上

一个有八个引脚，可以控制八个舵机，每排引脚从上往下是：PWM(信号线)、VCC(电源线)、GND(地线)

---

舵机的编号从左到右编号为0-7

---

代码

控制单个舵机的旋转

核心代码：servo.position(i, x) # 控制第i号舵机旋转x°

pc8596.py

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)

servo.py

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)

main.py

# 舵机控制示例。
#
# 这个例子演示了舵机扩展板。请按照以下步骤操作：
#
#   1. 连接舵机到任何PWM输出。
#   2. 将3.7v电池（或5V电源）连接到VIN和GND。
#   3. 将pca9685.py和servo.py复制到OpenMV并重置。
#   4. 在IDE中连接并运行此脚本。

import time # 导入时钟
from servo import Servos    # 导入舵机类
from machine import I2C, Pin    # 导入I2C协议和Pin引脚定义

# 创建一个I2C对象（I2C是一个类）
i2c = I2C(sda=Pin('P5'), scl=Pin('P4')) # 设置I2C的sda地址为P5引脚，scl地址为P4引脚   这是不需要更改的

# 创建一个舵机对象
servo = Servos(i2c, address=0x40, freq=50, min_us=500, max_us=2500, degrees=180)
    # 参数1："i2c"表示我们采用I2C协议  参数2："address=0x40"表示I2C的地址是0x40(0x40是由拓展板的参数决定的，如果要焊接背面的焊点要改成0x60，具体见官网)
    # 参数3："freq"表示舵机的频率，与我们使用的舵机有关  参数4、5："min_us和max_us"分别表示最小脉宽和最大脉宽，需要更改为和舵机一样的参数（具体见官网）
    # 参数6："degress"表示角度
    
# 循环八个舵机
while True:# i指舵机的编号
    for i in range(0, 8):
        servo.position(i, 0)    # 控制每个舵机先转到0°
        
    time.sleep_ms(500)  # 延迟500ms
    
    for i in range(0, 8):
        servo.position(i, 180)  # 控制每个舵机转到180°
        
    time.sleep_ms(500)  # 延迟500ms

利用两个舵机拓展板控制16个舵机

# 舵机控制示例。
#
# 这个例子演示了舵机扩展板。请按照以下步骤操作：
#
#   1. 连接舵机到任何PWM输出。
#   2. 将3.7v电池（或5V电源）连接到VIN和GND。
#   3. 将pca9685.py和servo.py复制到OpenMV并重置。
#   4. 在IDE中连接并运行此脚本。

import time # 导入时钟
from servo import Servos    # 导入舵机类
from machine import I2C, Pin    # 导入I2C协议和Pin引脚定义

# 创建一个I2C对象（I2C是一个类）
i2c = I2C(sda=Pin('P5'), scl=Pin('P4')) # 设置I2C的sda地址为P5引脚，scl地址为P4引脚   这是不需要更改的

# 创建两个舵机对象
servo1 = Servos(i2c, address=0x40, freq=50, min_us=500, max_us=2500, degrees=180)
    # 参数1："i2c"表示我们采用I2C协议  参数2："address=0x40"表示I2C的地址是0x40(0x40是由拓展板的参数决定的，如果要焊接背面的焊点要改成0x60，具体见官网)
    # 参数3："freq"表示舵机的频率，与我们使用的舵机有关  参数4、5："min_us和max_us"分别表示最小脉宽和最大脉宽，需要更改为和舵机一样的参数（具体见官网）
    # 参数6："degress"表示角度
    
    # 二号舵机需要更改地址为0x60   由于使用的是同一款舵机，因此其他参数不需要更改
servo2 = Servos(i2c, address=0x60, freq=50, min_us=500, max_us=2500, degrees=180)  
  
# 循环十六个舵机
while True:# i指舵机的编号    可分别指定是servo1的舵机运动还是servo2的舵机运动
    for i in range(0, 8):
        servo1.position(i, 0)    # 控制每个舵机先转到0°        
    time.sleep_ms(500)  # 延迟500ms
    
    for i in range(0, 8):
        servo1.position(i, 180)  # 控制每个舵机转到180°        
    time.sleep_ms(500)  # 延迟500ms


    for i in range(0, 8):
        servo2.position(i, 0)    # 控制每个舵机先转到0°        
    time.sleep_ms(500)  # 延迟500ms
    
    for i in range(0, 8):
        servo2.position(i, 180)  # 控制每个舵机转到180°        
    time.sleep_ms(500)  # 延迟500ms