nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
pca9685.hpp
[詳解]
1// PCA9685 (I2C 16ch PWM) の簡易ドライバ。Linuxの /dev/i2c-N を直接使う (追加ライブラリ不要)。
2// 動作確認はしていません。実機で、I2Cのバス番号とアドレスを確認してください
3// (確認: `sudo i2cdetect -y -r <バス番号>`、i2c-tools が必要)。
4#pragma once
5
6#include <fcntl.h>
7#include <linux/i2c-dev.h>
8#include <sys/ioctl.h>
9#include <unistd.h>
10
11#include <algorithm>
12#include <chrono>
13#include <cmath>
14#include <cstdint>
15#include <stdexcept>
16#include <string>
17#include <thread>
18
19namespace nodding_lidar
20{
21
23{
24public:
25 // bus: /dev/i2c-<bus>, address: 7bitアドレス (通常 0x40)
26 // osc_hz: 内部発振器の周波数 (25MHzが公称。実測でずれる場合は調整する)
27 Pca9685(int bus, int address, double pwm_freq_hz, double osc_hz = 25.0e6)
28 : freq_hz_(pwm_freq_hz)
29 {
30 const std::string dev = "/dev/i2c-" + std::to_string(bus);
31 fd_ = ::open(dev.c_str(), O_RDWR);
32 if (fd_ < 0) {
33 throw std::runtime_error("cannot open " + dev + " (権限とバス番号を確認してください)");
34 }
35 if (::ioctl(fd_, I2C_SLAVE, address) < 0) {
36 ::close(fd_);
37 fd_ = -1;
38 throw std::runtime_error("cannot set I2C slave address");
39 }
40
41 // PWM周波数の設定は、スリープ中にしか書き込めない
42 write_reg(kMode1, 0x10); // SLEEP
43 double prescale = std::round(osc_hz / (4096.0 * pwm_freq_hz)) - 1.0;
44 prescale = std::clamp(prescale, 3.0, 255.0);
45 write_reg(kPrescale, static_cast<uint8_t>(prescale));
46 write_reg(kMode2, 0x04); // OUTDRV (トーテムポール出力)
47 write_reg(kMode1, 0x00); // スリープ解除
48 std::this_thread::sleep_for(std::chrono::milliseconds(5));
49 write_reg(kMode1, 0xA0); // RESTART + AI (レジスタ自動インクリメント)
50 }
51
53 {
54 if (fd_ >= 0) {
55 ::close(fd_);
56 }
57 }
58
59 Pca9685(const Pca9685 &) = delete;
60 Pca9685 & operator=(const Pca9685 &) = delete;
61
62 // channel(0-15) のパルス幅を us で設定する
63 void set_pulse_us(int channel, double pulse_us)
64 {
65 if (channel < 0 || channel > 15) {
66 throw std::runtime_error("channel must be 0-15");
67 }
68 const double period_us = 1.0e6 / freq_hz_;
69 double ticks = std::round(pulse_us / period_us * 4096.0);
70 ticks = std::clamp(ticks, 0.0, 4095.0);
71 const auto off = static_cast<uint16_t>(ticks);
72
73 const uint8_t reg = static_cast<uint8_t>(kLed0OnL + 4 * channel);
74 const uint8_t buf[5] = {
75 reg, 0x00, 0x00, static_cast<uint8_t>(off & 0xFF), static_cast<uint8_t>(off >> 8)};
76 if (::write(fd_, buf, sizeof(buf)) != static_cast<ssize_t>(sizeof(buf))) {
77 throw std::runtime_error("I2C write failed");
78 }
79 }
80
81private:
82 static constexpr uint8_t kMode1 = 0x00;
83 static constexpr uint8_t kMode2 = 0x01;
84 static constexpr uint8_t kLed0OnL = 0x06;
85 static constexpr uint8_t kPrescale = 0xFE;
86
87 void write_reg(uint8_t reg, uint8_t value)
88 {
89 const uint8_t buf[2] = {reg, value};
90 if (::write(fd_, buf, sizeof(buf)) != static_cast<ssize_t>(sizeof(buf))) {
91 throw std::runtime_error("I2C write failed");
92 }
93 }
94
95 int fd_{-1};
96 double freq_hz_;
97};
98
99} // namespace nodding_lidar
Pca9685(int bus, int address, double pwm_freq_hz, double osc_hz=25.0e6)
void set_pulse_us(int channel, double pulse_us)
Pca9685 & operator=(const Pca9685 &)=delete
Pca9685(const Pca9685 &)=delete