-
Notifications
You must be signed in to change notification settings - Fork 25
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
6 changed files
with
226 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| #include "pca9685.h" | ||
|
|
||
| #include <unistd.h> | ||
|
|
||
| #include "extern-plugininfo.h" | ||
|
|
||
| #include <QThread> | ||
| #include <qmath.h> | ||
|
|
||
| Pca9685::Pca9685(const QString &portName, int address, QObject *parent) : I2CDevice(portName, address, parent) | ||
| { | ||
|
|
||
| } | ||
|
|
||
| bool Pca9685::writeData(int fd, const QByteArray &command) | ||
| { | ||
| if (command == "init") { | ||
| writeByte8(fd, MODE2, OUTDRV); | ||
| writeByte8(fd, MODE1, ALLCALL); | ||
| QThread::msleep(5); | ||
| quint8 mode1 = readByte8(fd, MODE1); | ||
| mode1 = mode1 & ~SLEEP; | ||
| writeByte8(fd, MODE1, mode1); | ||
| QThread::msleep(5); | ||
| qCInfo(dcI2cDevices())<<"PCA9685 Initialization: SUCCESS"; | ||
| return true; | ||
| } | ||
|
|
||
| if (command.startsWith("PWM")) { | ||
| QList<QByteArray> parts = command.split(':'); | ||
| quint8 channel = parts.at(1).toUInt(); | ||
| quint16 on = parts.at(2).toUInt(); | ||
| quint16 off = parts.at(3).toUInt(); | ||
| writeByte8(fd, LED0_ON_L+4*channel, on & 0xFF); | ||
| writeByte8(fd, LED0_ON_H+4*channel, on >> 8); | ||
| writeByte8(fd, LED0_OFF_L+4*channel, off & 0xFF); | ||
| writeByte8(fd, LED0_OFF_H+4*channel, off >> 8); | ||
| return true; | ||
| } | ||
|
|
||
| if (command.startsWith("FREQ")) { | ||
| QList<QByteArray> parts = command.split(':'); | ||
| quint16 frequency = parts.at(1).toUInt(); | ||
| float prescaleval = 25000000.0; //25MHz | ||
| prescaleval /= 4096.0; //12-bit | ||
| prescaleval /= float(frequency); | ||
| prescaleval -= 1.0; | ||
| qInfo()<<"Setting PWM frequency to "<<frequency<<" hz"; | ||
| qInfo()<<"Estimated pre-scale: "<<prescaleval; | ||
| int prescale = (int)qFloor(prescaleval + 0.5); | ||
| qInfo()<<"Final pre-scale: "<<prescale; | ||
| uint8_t oldmode = readByte8(fd, MODE1); | ||
| uint8_t newmode = (oldmode & 0x7F) | 0x10; // sleep | ||
| writeByte8(fd, MODE1, newmode); //go to sleep | ||
| writeByte8(fd, PRESCALE, prescale); | ||
| writeByte8(fd, MODE1, oldmode); | ||
| //time.sleep(0.005) # wait | ||
| QThread::msleep(5); | ||
| writeByte8(fd, MODE1, oldmode | 0x80); | ||
| return true; | ||
| } | ||
|
|
||
| return false; | ||
| } | ||
|
|
||
| bool Pca9685::writeByte8(int fd, uint8_t registerAddress, uint8_t data) | ||
| { | ||
| uint8_t buffer[2] = {0}; | ||
| buffer[0] = registerAddress & 0x00FF; | ||
| buffer[1] = data; | ||
|
|
||
| if (write(fd, buffer, 2) != 2){ | ||
| qCDebug(dcI2cDevices())<<"I2C Transaction failed!"; | ||
| return false; | ||
| } | ||
| return true; | ||
| } | ||
|
|
||
| uint8_t Pca9685::readByte8(int fd, uint8_t registerAddress) | ||
| { | ||
| uint8_t buffer[1] = {0}; | ||
| buffer[0] = registerAddress & 0x00FF; | ||
| write(fd, buffer, 1); | ||
|
|
||
| uint8_t result_buffer[1] = {0}; | ||
| if (read(fd, result_buffer, 1) != 1){ | ||
| qDebug()<<"I2C Transaction failed!"; | ||
| return 0; | ||
| } | ||
|
|
||
| return result_buffer[0]; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,57 @@ | ||
| #ifndef PCA9685_H | ||
| #define PCA9685_H | ||
|
|
||
| #include <QObject> | ||
|
|
||
| #include <hardware/i2c/i2cdevice.h> | ||
|
|
||
| #define MODE1 0x00 | ||
| #define MODE2 0x01 | ||
| #define SUBADR1 0x02 | ||
| #define SUBADR2 0x03 | ||
| #define SUBADR3 0x04 | ||
| #define PRESCALE 0xFE | ||
| #define LED0_ON_L 0x06 | ||
| #define LED0_ON_H 0x07 | ||
| #define LED0_OFF_L 0x08 | ||
| #define LED0_OFF_H 0x09 | ||
| #define ALL_LED_ON_L 0xFA | ||
| #define ALL_LED_ON_H 0xFB | ||
| #define ALL_LED_OFF_L 0xFC | ||
| #define ALL_LED_OFF_H 0xFD | ||
| // Bits | ||
| #define RESTART 0x80 | ||
| #define SLEEP 0x10 | ||
| #define ALLCALL 0x01 | ||
| #define INVRT 0x10 | ||
| #define OUTDRV 0x04 | ||
|
|
||
| #define PAN 0 | ||
| #define TILT 1 | ||
| #define FREQUENCY 50 | ||
| #define CLOCKFREQ 25000000 | ||
| #define PANOFFSET 1 | ||
| #define PANSCALE 1.4 | ||
| #define TILTOFFSET 30 | ||
| #define TILTSCALE 1.43 | ||
| #define PANMAX 270 | ||
| #define PANMIN 90 | ||
| #define TILTMAX 90 | ||
| #define TILTMIN -45 | ||
|
|
||
| class Pca9685 : public I2CDevice | ||
| { | ||
| Q_OBJECT | ||
| public: | ||
| explicit Pca9685(const QString &portName, int address, QObject *parent = nullptr); | ||
|
|
||
|
|
||
| bool writeData(int fd, const QByteArray &data) override; | ||
|
|
||
| private: | ||
| bool writeByte8(int fd, uint8_t registerAddress, uint8_t data); | ||
| uint8_t readByte8(int fd, uint8_t registerAddress); | ||
|
|
||
| }; | ||
|
|
||
| #endif // PCA9685_H |