From d6091a1030f442467565971255909192c37178d8 Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 18:36:15 +0100 Subject: [PATCH] New PWM class --- darkwater/PCA9685.cpp | 72 ++++++++++++++++++++++++++++++------------- darkwater/PCA9685.h | 7 +++++ 2 files changed, 58 insertions(+), 21 deletions(-) diff --git a/darkwater/PCA9685.cpp b/darkwater/PCA9685.cpp index 9533892..a3a11b1 100644 --- a/darkwater/PCA9685.cpp +++ b/darkwater/PCA9685.cpp @@ -4,6 +4,8 @@ Written by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com) Copyright (c) 2014, Emlid Limited, www.emlid.com All rights reserved. +Modified by Dark Water (team@darkwater.io) + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright @@ -72,12 +74,16 @@ void PCA9685::sleep() { * @see PCA9685_MODE1_RESTART_BIT */ void PCA9685::restart() { - I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, (1 << PCA9685_MODE1_SLEEP_BIT)); - I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, ((1 << PCA9685_MODE1_SLEEP_BIT) - | (1 << PCA9685_MODE1_EXTCLK_BIT))); - I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, ((1 << PCA9685_MODE1_RESTART_BIT) - | (1 << PCA9685_MODE1_EXTCLK_BIT) - | (1 << PCA9685_MODE1_AI_BIT))); + + I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, 0x00); + I2Cdev::writeByte(devAddr, PCA9685_RA_MODE2, 0x04); + + // I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, (1 << PCA9685_MODE1_SLEEP_BIT)); + // I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, ((1 << PCA9685_MODE1_SLEEP_BIT) + // | (1 << PCA9685_MODE1_EXTCLK_BIT))); + // I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, ((1 << PCA9685_MODE1_RESTART_BIT) + // | (1 << PCA9685_MODE1_EXTCLK_BIT) + // | (1 << PCA9685_MODE1_AI_BIT))); } /** Calculate prescale value based on the specified frequency and write it to the device. @@ -96,12 +102,23 @@ float PCA9685::getFrequency() { * @see PCA9685_RA_PRE_SCALE */ void PCA9685::setFrequency(float frequency) { + sleep(); usleep(10000); - uint8_t prescale = roundf(24576000.f / 4096.f / frequency) - 1; + uint8_t prescale = roundf(25000000.f / 4096.f / frequency) - 1; + I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, 0x10); I2Cdev::writeByte(devAddr, PCA9685_RA_PRE_SCALE, prescale); + I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, 0x80); + I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, 0x04); this->frequency = getFrequency(); restart(); + + // sleep(); + // usleep(10000); + // uint8_t prescale = roundf(24576000.f / 4096.f / frequency) - 1; + // I2Cdev::writeByte(devAddr, PCA9685_RA_PRE_SCALE, prescale); + // this->frequency = getFrequency(); + // restart(); } /** Set channel start offset of the pulse and it's length @@ -111,18 +128,25 @@ void PCA9685::setFrequency(float frequency) { * @see PCA9685_RA_LED0_ON_L */ void PCA9685::setPWM(uint8_t channel, uint16_t offset, uint16_t length) { - uint8_t data[4] = {0, 0, 0, 0}; - if(length == 0) { - data[3] = 0x10; - } else if(length >= 4096) { - data[1] = 0x10; - } else { - data[0] = offset & 0xFF; - data[1] = offset >> 8; - data[2] = length & 0xFF; - data[3] = length >> 8; - } - I2Cdev::writeBytes(devAddr, PCA9685_RA_LED0_ON_L + 4 * channel, 4, data); + + + I2Cdev::writeByte(devAddr, PCA9685_RA_LED0_ON_L + LED_MULTIPLYER * channel, offset & 0xFF); + I2Cdev::writeByte(devAddr, PCA9685_RA_LED0_ON_H + LED_MULTIPLYER * channel, offset >> 8); + I2Cdev::writeByte(devAddr, PCA9685_RA_LED0_OFF_L + LED_MULTIPLYER * channel, length & 0xFF); + I2Cdev::writeByte(devAddr, PCA9685_RA_LED0_OFF_H + LED_MULTIPLYER * channel, length >> 8); + + // uint8_t data[4] = {0, 0, 0, 0}; + // if(length == 0) { + // data[3] = 0x10; + // } else if(length >= 4096) { + // data[1] = 0x10; + // } else { + // data[0] = offset & 0xFF; + // data[1] = offset >> 8; + // data[2] = length & 0xFF; + // data[3] = length >> 8; + // } + // I2Cdev::writeBytes(devAddr, PCA9685_RA_LED0_ON_L + 4 * channel, 4, data); } /** Set channel's pulse length @@ -158,8 +182,14 @@ void PCA9685::setPWMuS(uint8_t channel, float length_uS) { * @see PCA9685_RA_ALL_LED_ON_L */ void PCA9685::setAllPWM(uint16_t offset, uint16_t length) { - uint8_t data[4] = {offset & 0xFF, offset >> 8, length & 0xFF, length >> 8}; - I2Cdev::writeBytes(devAddr, PCA9685_RA_ALL_LED_ON_L, 4, data); + + I2Cdev::writeByte(devAddr, PCA9685_RA_ALL_LED_ON_L, offset & 0xFF); + I2Cdev::writeByte(devAddr, PCA9685_RA_ALL_LED_ON_H, offset >> 8); + I2Cdev::writeByte(devAddr, PCA9685_RA_ALL_LED_OFF_L, length & 0xFF); + I2Cdev::writeByte(devAddr, PCA9685_RA_ALL_LED_OFF_H, length >> 8); + + // uint8_t data[4] = {offset & 0xFF, offset >> 8, length & 0xFF, length >> 8}; + // I2Cdev::writeBytes(devAddr, PCA9685_RA_ALL_LED_ON_L, 4, data); } /** Set pulse length for all channels diff --git a/darkwater/PCA9685.h b/darkwater/PCA9685.h index 395ec1a..5d58ff2 100644 --- a/darkwater/PCA9685.h +++ b/darkwater/PCA9685.h @@ -4,6 +4,8 @@ Written by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com) Copyright (c) 2014, Emlid Limited All rights reserved. +Modified by Dark Water (team@darkwater.io) + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright @@ -43,10 +45,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define PCA9685_RA_MODE1 0x00 #define PCA9685_RA_MODE2 0x01 +#define PCA9685_RA_SUBADR1 0x02 //I2C-bus subaddress 1 +#define PCA9685_RA_SUBADR2 0x03 //I2C-bus subaddress 2 +#define PCA9685_RA_SUBADR3 0x04 //I2C-bus subaddress 3 +#define PCA9685_RA_ALLCALLADR 0x05 //LED All Call I2C-bus address #define PCA9685_RA_LED0_ON_L 0x06 #define PCA9685_RA_LED0_ON_H 0x07 #define PCA9685_RA_LED0_OFF_L 0x08 #define PCA9685_RA_LED0_OFF_H 0x09 +#define LED_MULTIPLYER 4 // For the other 15 channels #define PCA9685_RA_ALL_LED_ON_L 0xFA #define PCA9685_RA_ALL_LED_ON_H 0xFB #define PCA9685_RA_ALL_LED_OFF_L 0xFC