树莓派+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中,请自行查看
参考链接:
/qq_41559171/article/details/87950642
/asmallwhite/article/details/83048091
/weixin_42866931/article/details/90676372
/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_ADDRESS0x40//pca9685地址#define PCA9685_CLOCK_FREQ25000000//PWM频率25MHz#define PCA9685_MODE10x00#define PCA9685_MODE20x01#define PCA9685_PRE_SCALE0xFE#define PCA9685_LED0_ON_L0x06#define PCA9685_LED0_ON_H0x07#define PCA9685_LED0_OFF_L0x08#define PCA9685_LED0_OFF_H0x09#define PCA9685_LED_SHIFT4bool 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 offWriteByte(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 beginWriteByte(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 modeoldMode = 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 freqWriteByte(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){//设置各个通道的PWMif (!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 datadata = wiringPiI2CReadReg8(fd, regAddr);if (data < 0)return -1;return data;}
Python版本:
#!/usr/bin/pythonimport timeimport mathimport 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 = 0xFDdef __init__(self, address=0x40, debug=False):self.bus = smbus.SMBus(1)self.address = addressself.debug = debugif (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 resultdef setPWMFreq(self, freq):"Sets the PWM frequency"prescaleval = 25000000.0 # 25MHzprescaleval /= 4096.0 # 12-bitprescaleval /= float(freq)prescaleval -= 1.0if (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 # sleepself.write(self.__MODE1, newmode) # go to sleepself.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 20000usself.setPWM(channel, 0, pulse)channel = 14if __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)
———————————————————————————————————————————
.10.16更新
舵机无法工作请参考如下解决方案:
1.PCA9685的V+与树莓派的5V引脚连接
2.重装smbus (高版本的树莓派系统存在兼容性问题)
3.检查SDA-SDA SCL-SCL引脚是否连接错误,注意是否接反。