树莓派+PCA9685+舵机控制：wiringPi实现

树莓派+PCA9685+舵机控制
1舵机控制
1.1舵机介绍
淘宝直接搜索舵机，有90° 180° 270° 360°舵机，通过PWM来进行控制，如下所示：

1.2控制原理
如下图所示：一个脉冲周期为20ms，高电平为脉冲宽度，这个脉冲宽度决定舵机旋转角度，假设180°舵机旋转位于中间位置（90°）的脉冲宽度为1.5ms，则0°位置为1ms，180°位置为2ms，以此类推，得出一下公式：
舵机旋转1°= （最大脉冲宽度-最小脉冲宽度）/最大角度（最大位置）

2PCA9685设备
2.1PCA9685设备地址
地址分配是通过模块右上方的短接焊盘来确定的，从A0-A5表示地址的最低位到最高位，也就是最多可级联2^5=32个模块，地址为： 1+A5+A4+A3+A2+A1+A0+rw。如果不用短接的话Ax=0；短接的话Ax=1；rw为写的话rw=0；rw为读的话rw=1；所以写入数据不做短接则地址应该为1000 0000 =0x80
内部寄存器地址如下图所示：程序代码中有define定义，可以进行对照

2.2IIC通信
SCL接树莓派SCL0，SDA接树莓派SDA0，VCC接3.3V，GND接树莓派GND，V+单纯只是供电

2.3.树莓派 开启IIC功能
sudo raspi-config -> 5.Interfacing Options -> P5 I2C　设置enable使能，然后在/boot/config.txt中进行配置，如下所示：

重启树莓派之后进行测试：
执行命令安装i2c-tools：
sudo apt-get install i2c-tools
再运行：
sudo i2cdetect -y 1
显示：

即本地址为0x40，记录下来，后面写程序需要。

3.程序
3.1整体连接图

我这里选择的是最后一个通道，编码为15（第一个通道编码为0）
代码在i2c_pca9685.c中，请自行查看

参考链接：
https://blog.csdn.net/qq_41559171/article/details/87950642
https://blog.csdn.net/asmallwhite/article/details/83048091
https://blog.csdn.net/weixin_42866931/article/details/90676372
https://blog.csdn.net/asmallwhite/article/details/83048091

C版本：

#include <stdbool.h>
#include <stdio.h>
#include "wiringPi.h"
#include <wiringPiI2C.h> 
#include <unistd.h>


bool PCA9685Init();
void ResetPca9685();
void PCA9685SetPwmFreq(unsigned short freq);
void PCA9685SetPwm(unsigned char channel, unsigned short on, unsigned short value);
void SetServoPulse(unsigned char channel, unsigned short pulse);

#define PCA9685_ADDRESS			0x40			//pca9685地址
#define PCA9685_CLOCK_FREQ		25000000		//PWM频率25MHz
#define PCA9685_MODE1			0x00
#define PCA9685_MODE2			0x01
#define PCA9685_PRE_SCALE		0xFE
#define PCA9685_LED0_ON_L		0x06
#define PCA9685_LED0_ON_H		0x07
#define PCA9685_LED0_OFF_L		0x08
#define PCA9685_LED0_OFF_H		0x09
#define PCA9685_LED_SHIFT		4



bool WriteByte(int fd, unsigned char regAddr, unsigned char data);
unsigned char ReadByte(int fd, unsigned char regAddr);


bool PCA9685_initSuccess = false;
int PCA9685_fd = 0;


int main()
{
	int PCA9865_channel = 15;
	PCA9685Init();
	PCA9685SetPwmFreq(50);
	while (1)
	{
		for (int i = 500; i < 2500; i += 10)
		{
			SetServoPulse(PCA9865_channel, i);
			usleep(20000);
		}
	}

	return 0;
}



bool PCA9685Init() 
{
	//初始化
	PCA9685_fd = wiringPiI2CSetup(PCA9685_ADDRESS);
	if (PCA9685_fd <= 0) 
		return false;
	PCA9685_initSuccess = true;
	ResetPca9685();
	return true;
}

void ResetPca9685() 
{
	if (true == PCA9685_initSuccess) 
	{
		//sleep mode, Low power mode. Oscillator off
		WriteByte(PCA9685_fd, PCA9685_MODE1, 0x00);
		WriteByte(PCA9685_fd, PCA9685_MODE2, 0x04);
		usleep(1000);
		//Delay Time is 0, means it always turn into high at the begin
		WriteByte(PCA9685_fd, PCA9685_LED0_ON_L + PCA9685_LED_SHIFT * 0, 0);
		WriteByte(PCA9685_fd, PCA9685_LED0_ON_H + PCA9685_LED_SHIFT * 0, 0);
		WriteByte(PCA9685_fd, PCA9685_LED0_ON_L + PCA9685_LED_SHIFT * 1, 0);
		WriteByte(PCA9685_fd, PCA9685_LED0_ON_H + PCA9685_LED_SHIFT * 1, 0);
		WriteByte(PCA9685_fd, PCA9685_LED0_ON_L + PCA9685_LED_SHIFT * 2, 0);
		WriteByte(PCA9685_fd, PCA9685_LED0_ON_H + PCA9685_LED_SHIFT * 2, 0);
		WriteByte(PCA9685_fd, PCA9685_LED0_ON_L + PCA9685_LED_SHIFT * 3, 0);
		WriteByte(PCA9685_fd, PCA9685_LED0_ON_H + PCA9685_LED_SHIFT * 3, 0);
		usleep(1000);
	}
	else 
	{
		printf("pca9685 doesn't init\n");
	}
}

void PCA9685SetPwmFreq(unsigned short freq) 
{ //设置频率
	unsigned char preScale = (PCA9685_CLOCK_FREQ / 4096 / freq) - 1;
	unsigned char oldMode = 0;
	printf("set PWM frequency to %d HZ\n",freq);
	//read old mode
	oldMode = ReadByte(PCA9685_fd, PCA9685_MODE1);
	//setup sleep mode, Low power mode. Oscillator off (bit4: 1-sleep, 0-normal)
	WriteByte(PCA9685_fd, PCA9685_MODE1, (oldMode & 0x7F) | 0x10);
	//set freq
	WriteByte(PCA9685_fd, PCA9685_PRE_SCALE, preScale);
	//setup normal mode (bit4: 1-sleep, 0-normal)
	WriteByte(PCA9685_fd, PCA9685_MODE1, oldMode);
	usleep(1000); // >500us
	//setup restart (bit7: 1- enable, 0-disable)
	WriteByte(PCA9685_fd, PCA9685_MODE1, oldMode | 0x80);
	usleep(1000); // >500us
}

void PCA9685SetPwm(unsigned char channel, unsigned short on, unsigned short value)
{
	//设置各个通道的PWM
	if (!PCA9685_initSuccess) 
	{
		printf("Set Pwm failure!\n");
		return;
	}

	WriteByte(PCA9685_fd, PCA9685_LED0_ON_L + PCA9685_LED_SHIFT * channel, on & 0xFF);
	WriteByte(PCA9685_fd, PCA9685_LED0_ON_H + PCA9685_LED_SHIFT * channel, on >> 8);
	WriteByte(PCA9685_fd, PCA9685_LED0_OFF_L + PCA9685_LED_SHIFT * channel, value & 0xFF);
	WriteByte(PCA9685_fd, PCA9685_LED0_OFF_H + PCA9685_LED_SHIFT * channel, value >> 8);
}


void SetServoPulse(unsigned char channel, unsigned short pulse)
{
	pulse = pulse * 4096 / 20000;
	PCA9685SetPwm(channel, 0, pulse);
}







bool WriteByte(int fd, unsigned char regAddr, unsigned char data) 
{
	if (wiringPiI2CWriteReg8(fd, regAddr, data) < 0)
		return -1;
	return 0;
}



unsigned char ReadByte(int fd, unsigned char regAddr) 
{
	unsigned char data; // `data` will store the register data
	data = wiringPiI2CReadReg8(fd, regAddr);
	if (data < 0)
		return -1;
	return data;
}

Python版本：

#!/usr/bin/python
 
import time
import math
import smbus
 
# ============================================================================
# Raspi PCA9685 16-Channel PWM Servo Driver
# ============================================================================
 
class PCA9685:
 
  # Registers/etc.
  __SUBADR1            = 0x02
  __SUBADR2            = 0x03
  __SUBADR3            = 0x04
  __MODE1              = 0x00
  __PRESCALE           = 0xFE
  __LED0_ON_L          = 0x06
  __LED0_ON_H          = 0x07
  __LED0_OFF_L         = 0x08
  __LED0_OFF_H         = 0x09
  __ALLLED_ON_L        = 0xFA
  __ALLLED_ON_H        = 0xFB
  __ALLLED_OFF_L       = 0xFC
  __ALLLED_OFF_H       = 0xFD
 
  def __init__(self, address=0x40, debug=False):
    self.bus = smbus.SMBus(1)
    self.address = address
    self.debug = debug
    if (self.debug):
      print("Reseting PCA9685")
    self.write(self.__MODE1, 0x00)
 
  def write(self, reg, value):
    "Writes an 8-bit value to the specified register/address"
    self.bus.write_byte_data(self.address, reg, value)
    if (self.debug):
      print("I2C: Write 0x%02X to register 0x%02X" % (value, reg))
 
  def read(self, reg):
    "Read an unsigned byte from the I2C device"
    result = self.bus.read_byte_data(self.address, reg)
    if (self.debug):
      print("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % (self.address, result & 0xFF, reg))
    return result
 
  def setPWMFreq(self, freq):
    "Sets the PWM frequency"
    prescaleval = 25000000.0    # 25MHz
    prescaleval /= 4096.0       # 12-bit
    prescaleval /= float(freq)
    prescaleval -= 1.0
    if (self.debug):
      print("Setting PWM frequency to %d Hz" % freq)
      print("Estimated pre-scale: %d" % prescaleval)
    prescale = math.floor(prescaleval + 0.5)
    if (self.debug):
      print("Final pre-scale: %d" % prescale)
 
    oldmode = self.read(self.__MODE1);
    newmode = (oldmode & 0x7F) | 0x10        # sleep
    self.write(self.__MODE1, newmode)        # go to sleep
    self.write(self.__PRESCALE, int(math.floor(prescale)))
    self.write(self.__MODE1, oldmode)
    time.sleep(0.005)
    self.write(self.__MODE1, oldmode | 0x80)
 
  def setPWM(self, channel, on, off):
    "Sets a single PWM channel"
    self.write(self.__LED0_ON_L+4*channel, on & 0xFF)
    self.write(self.__LED0_ON_H+4*channel, on >> 8)
    self.write(self.__LED0_OFF_L+4*channel, off & 0xFF)
    self.write(self.__LED0_OFF_H+4*channel, off >> 8)
    if (self.debug):
      print("channel: %d  LED_ON: %d LED_OFF: %d" % (channel,on,off))
 
  def setServoPulse(self, channel, pulse):
    "Sets the Servo Pulse,The PWM frequency must be 50HZ"
    pulse = pulse*4096/20000        #PWM frequency is 50HZ,the period is 20000us
    self.setPWM(channel, 0, pulse)
 
 
channel = 14
if __name__=='__main__':
  pwm = PCA9685(0x40, debug=True)
  pwm.setPWMFreq(50)
  while True:
   # setServoPulse(2,2500)
    for i in range(500,2500,10):  
      pwm.setServoPulse(channel,i)   
      time.sleep(0.02)     
 
    for i in range(2500,500,-10):
      pwm.setServoPulse(channel,i) 
      time.sleep(0.02)

———————————————————————————————————————————
2022.10.16更新
舵机无法工作请参考如下解决方案：
1.PCA9685的V+与树莓派的5V引脚连接
2.重装smbus （高版本的树莓派系统存在兼容性问题）
3.检查SDA-SDA SCL-SCL引脚是否连接错误，注意是否接反。