162 lines
5.6 KiB
C
162 lines
5.6 KiB
C
#include "pca9685.h"
|
||
#include "esp_log.h"
|
||
#include "esp_err.h"
|
||
#include <math.h>
|
||
|
||
static const char *TAG = "PCA9685";
|
||
|
||
static esp_err_t write8(i2c_port_t i2c_num, uint8_t addr, uint8_t reg, uint8_t val) {
|
||
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
|
||
i2c_master_start(cmd);
|
||
i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
|
||
i2c_master_write_byte(cmd, reg, true);
|
||
i2c_master_write_byte(cmd, val, true);
|
||
i2c_master_stop(cmd);
|
||
esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
|
||
i2c_cmd_link_delete(cmd);
|
||
if (ret != ESP_OK) {
|
||
ESP_LOGE(TAG, "write8 failed: reg=0x%02X val=0x%02X, err=0x%X", reg, val, ret);
|
||
}
|
||
return ret;
|
||
}
|
||
|
||
static uint8_t read8(i2c_port_t i2c_num, uint8_t addr, uint8_t reg) {
|
||
uint8_t val = 0;
|
||
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
|
||
|
||
// 写入寄存器地址
|
||
i2c_master_start(cmd);
|
||
i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
|
||
i2c_master_write_byte(cmd, reg, true);
|
||
|
||
// 重复START条件
|
||
i2c_master_start(cmd);
|
||
i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_READ, true);
|
||
|
||
// 读取数据 (NACK最后一个字节)
|
||
i2c_master_read_byte(cmd, &val, I2C_MASTER_LAST_NACK);
|
||
i2c_master_stop(cmd);
|
||
|
||
esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
|
||
i2c_cmd_link_delete(cmd);
|
||
|
||
if (ret != ESP_OK) {
|
||
ESP_LOGE(TAG, "read8 failed: reg=0x%02X, err=0x%X", reg, ret);
|
||
}
|
||
return val;
|
||
}
|
||
|
||
static esp_err_t set_pwm(i2c_port_t i2c_num, uint8_t addr, uint8_t channel, uint16_t on, uint16_t off) {
|
||
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
|
||
i2c_master_start(cmd);
|
||
i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
|
||
i2c_master_write_byte(cmd, LED0_ON_L + 4 * channel, true);
|
||
i2c_master_write_byte(cmd, on & 0xFF, true);
|
||
i2c_master_write_byte(cmd, on >> 8, true);
|
||
i2c_master_write_byte(cmd, off & 0xFF, true);
|
||
|
||
i2c_master_write_byte(cmd, off >> 8, true);
|
||
i2c_master_stop(cmd);
|
||
esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
|
||
i2c_cmd_link_delete(cmd);
|
||
|
||
if (ret != ESP_OK) {
|
||
ESP_LOGE(TAG, "set_pwm failed: ch=%d, err=0x%X", channel, ret);
|
||
}
|
||
return ret;
|
||
}
|
||
|
||
void pca9685_init(i2c_port_t i2c_num, uint8_t addr) {
|
||
// 完整初始化序列
|
||
write8(i2c_num, addr, MODE1, MODE1_SLEEP | MODE1_ALLCALL);
|
||
vTaskDelay(50 / portTICK_PERIOD_MS); // 延长延迟
|
||
|
||
// 唤醒设备并启用自动增量
|
||
write8(i2c_num, addr, MODE1, MODE1_AI | MODE1_ALLCALL);
|
||
vTaskDelay(10 / portTICK_PERIOD_MS);
|
||
|
||
// 关闭所有LED输出
|
||
write8(i2c_num, addr, ALL_LED_OFF_H, 0x10); // 设置所有LED_OFF_H为0x10
|
||
|
||
ESP_LOGI(TAG, "PCA9685 at 0x%02X initialized", addr);
|
||
}
|
||
|
||
void pca9685_set_pwm_freq(i2c_port_t i2c_num, uint8_t addr, float freq) {
|
||
//待优化
|
||
// 计算预分频值 (25MHz时钟)
|
||
float prescaleval = 25000000.0;
|
||
prescaleval /= 4096.0; // 12位分辨率
|
||
prescaleval /= freq;
|
||
prescaleval -= 1.0;
|
||
uint8_t prescale = (uint8_t)(prescaleval + 0.5);
|
||
|
||
ESP_LOGI(TAG, "Setting PWM freq: %.1f Hz, prescale: %d", freq, prescale);
|
||
|
||
// 进入睡眠模式设置预分频
|
||
uint8_t oldmode = read8(i2c_num, addr, MODE1);
|
||
uint8_t sleepmode = (oldmode & ~MODE1_RESTART) | MODE1_SLEEP;
|
||
write8(i2c_num, addr, MODE1, sleepmode);
|
||
vTaskDelay(5 / portTICK_PERIOD_MS);
|
||
|
||
// 设置预分频寄存器
|
||
write8(i2c_num, addr, PRESCALE, prescale);
|
||
|
||
// 恢复模式并唤醒
|
||
write8(i2c_num, addr, MODE1, oldmode);
|
||
vTaskDelay(5 / portTICK_PERIOD_MS);
|
||
|
||
// 触发重启
|
||
write8(i2c_num, addr, MODE1, oldmode | MODE1_RESTART);
|
||
vTaskDelay(10 / portTICK_PERIOD_MS);
|
||
}
|
||
|
||
void pca9685_set_angle(i2c_port_t i2c_num, uint8_t addr, uint8_t channel, float angle) {
|
||
// 角度限幅
|
||
angle = (angle < 0) ? 0 : (angle > 180) ? 180 : angle;
|
||
|
||
// 计算脉冲宽度 (0.5ms - 2.5ms)
|
||
float pulse_width_ms = 0.5f + (angle / 180.0f) * 2.0f;
|
||
|
||
// 计算PWM值 (20ms周期)
|
||
uint16_t pwm_value = (uint16_t)((pulse_width_ms / 20.0f) * 4096.0f);
|
||
|
||
// 设置PWM (ON=0, OFF=pwm_value)
|
||
esp_err_t ret = set_pwm(i2c_num, addr, channel, 0, pwm_value);
|
||
|
||
if (ret == ESP_OK) {
|
||
ESP_LOGI(TAG, "Set addr=0x%02X ch=%d: %d° -> PWM=%d",
|
||
addr, channel, (int)angle, pwm_value);
|
||
} else {
|
||
ESP_LOGE(TAG, "Failed to set angle on addr=0x%02X ch=%d", addr, channel);
|
||
}
|
||
}
|
||
|
||
// 一次性更新多个通道的示例（在 pca9685.c 新增）
|
||
void pca9685_set_angles(i2c_port_t i2c_num, uint8_t addr,
|
||
uint8_t start_ch, uint8_t count,
|
||
const float *angles) {
|
||
// 准备一大块 data：每通道 4 字节 ON_L,ON_H,OFF_L,OFF_H
|
||
uint8_t data[4 * count];
|
||
for (int i = 0; i < count; i++) {
|
||
float a = angles[i];
|
||
a = a < 0 ? 0 : a > 180 ? 180 : a;
|
||
float pw = 0.5f + (a/180.0f)*2.0f; // ms
|
||
uint16_t pwm = (uint16_t)((pw/20.0f)*4096);
|
||
data[4*i + 0] = 0; // ON_L
|
||
data[4*i + 1] = 0; // ON_H
|
||
data[4*i + 2] = pwm & 0xFF;// OFF_L
|
||
data[4*i + 3] = pwm >> 8; // OFF_H
|
||
}
|
||
|
||
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
|
||
i2c_master_start(cmd);
|
||
// 写入寄存器地址，LED0_ON_L + 4*start_ch，自动递增模式
|
||
i2c_master_write_byte(cmd, (addr<<1)|I2C_MASTER_WRITE, true);
|
||
i2c_master_write_byte(cmd, LED0_ON_L + 4*start_ch, true);
|
||
// 一次性写出所有 data
|
||
i2c_master_write(cmd, data, sizeof(data), true);
|
||
i2c_master_stop(cmd);
|
||
i2c_master_cmd_begin(i2c_num, cmd, 1000/portTICK_PERIOD_MS);
|
||
i2c_cmd_link_delete(cmd);
|
||
}
|