From 9c9184788b2672a8db22269501010adcff61bace Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Sat, 22 Feb 2025 22:28:21 -0700 Subject: [PATCH 1/7] testing of the new pca9685 showed that the driver we got from JH was no longer working. this is the start of a rewrite --- src/rov/rov_pca9685/CMakeLists.txt | 9 +- .../external/JHPWMPCA9685/CMakeLists.txt | 4 - .../include/JHPWMPCA9685/JHPWMPCA9685.h | 180 ------------------ .../external/JHPWMPCA9685/license.txt | 19 -- .../JHPWMPCA9685/src/JHPWMPCA9685.cpp | 108 ----------- .../rov_pca9685/include/pca9685/PCA9685.hpp | 157 +++++++++++++++ .../include/pca9685/ServoDriver.hpp | 2 +- src/rov/rov_pca9685/src/PCA9685.cpp | 146 ++++++++++++++ src/rov/rov_pca9685/src/ServoDriver.cpp | 11 +- 9 files changed, 310 insertions(+), 326 deletions(-) delete mode 100644 src/rov/rov_pca9685/external/JHPWMPCA9685/CMakeLists.txt delete mode 100644 src/rov/rov_pca9685/external/JHPWMPCA9685/include/JHPWMPCA9685/JHPWMPCA9685.h delete mode 100644 src/rov/rov_pca9685/external/JHPWMPCA9685/license.txt delete mode 100644 src/rov/rov_pca9685/external/JHPWMPCA9685/src/JHPWMPCA9685.cpp create mode 100644 src/rov/rov_pca9685/include/pca9685/PCA9685.hpp create mode 100644 src/rov/rov_pca9685/src/PCA9685.cpp diff --git a/src/rov/rov_pca9685/CMakeLists.txt b/src/rov/rov_pca9685/CMakeLists.txt index 5f1d924..6dec674 100644 --- a/src/rov/rov_pca9685/CMakeLists.txt +++ b/src/rov/rov_pca9685/CMakeLists.txt @@ -15,10 +15,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -if(DEBUG_OUTPUT) - add_compile_definitions(DEBUG_OUTPUT=true) -endif() - # find dependencies find_package(ament_cmake REQUIRED) # uncomment the following section in order to fill in @@ -27,9 +23,7 @@ find_package(ament_cmake REQUIRED) find_package(rov_interfaces REQUIRED) find_package(rclcpp REQUIRED) -add_subdirectory(external/JHPWMPCA9685) -add_library(pca9685 src/PCA9685_Node.cpp src/ServoDriver.cpp) -target_include_directories(pca9685 PUBLIC JHPWMPCA9685_INCLUDE_DIRS) +add_library(pca9685 src/PCA9685_Node.cpp src/ServoDriver.cpp src/PCA9685.cpp) add_executable(pca9685_node src/main.cpp) add_executable(pca9685_interactive_test src/main_interactive.cpp) target_include_directories(pca9685 PUBLIC @@ -37,7 +31,6 @@ target_include_directories(pca9685 PUBLIC $ ) -target_link_libraries(pca9685 jhpwmpca9685) target_link_libraries(pca9685_node pca9685) target_link_libraries(pca9685_interactive_test pca9685) diff --git a/src/rov/rov_pca9685/external/JHPWMPCA9685/CMakeLists.txt b/src/rov/rov_pca9685/external/JHPWMPCA9685/CMakeLists.txt deleted file mode 100644 index e34b254..0000000 --- a/src/rov/rov_pca9685/external/JHPWMPCA9685/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -add_library(jhpwmpca9685 ${CMAKE_CURRENT_SOURCE_DIR}/src/JHPWMPCA9685.cpp) -target_include_directories(jhpwmpca9685 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) -target_link_libraries(jhpwmpca9685 i2c) -set(JHPWMPCA9685_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CACHE INTERNAL "JHPWMPCA9685_INCLUDE_DIRS") \ No newline at end of file diff --git a/src/rov/rov_pca9685/external/JHPWMPCA9685/include/JHPWMPCA9685/JHPWMPCA9685.h b/src/rov/rov_pca9685/external/JHPWMPCA9685/include/JHPWMPCA9685/JHPWMPCA9685.h deleted file mode 100644 index b57a55a..0000000 --- a/src/rov/rov_pca9685/external/JHPWMPCA9685/include/JHPWMPCA9685/JHPWMPCA9685.h +++ /dev/null @@ -1,180 +0,0 @@ -/* - * The MIT License (MIT) - -Copyright (c) 2015 Jetsonhacks - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -#ifndef _JHPWMPCA9685_H -#define _JHPWMPCA9685_H - -#include -#include -#include -#include -#include -#include -#include - -extern "C" { - #include - #include -} - -class PCA9685 -{ -public: - unsigned char kI2CBus ; // I2C bus of the PCA9685 - int kI2CFileDescriptor ; // File Descriptor to the PCA9685 - int kI2CAddress ; // Address of PCA9685; defaults to 0x40 - int error ; - PCA9685(int address=0x40); - ~PCA9685() ; - bool openPCA9685() ; - void closePCA9685(); - - void reset() ; - - // Sets the frequency of the PWM signal - // Frequency is ranged between 40 and 1000 Hertz - void setPWMFrequency ( float frequency ); - - // Channels 0-15 - // Channels are in sets of 4 bytes - void setPWM ( int channel, int onValue, int offValue); - - void setAllPWM (int onValue, int offValue); - - // Read the given register - int readByte(int readRegister); - - // Write the the given value to the given register - int writeByte(int writeRegister, int writeValue); - - int getError() ; - -}; - - -// Register definitions from Table 7.3 NXP Semiconductors -// Product Data Sheet, Rev. 4 - 16 April 2015 -#define PCA9685_MODE1 0x00 -#define PCA9685_MODE2 0x01 -#define PCA9685_SUBADR1 0x02 -#define PCA9685_SUBADR2 0x03 -#define PCA9685_SUBADR3 0x04 -#define PCA9685_ALLCALLADR 0x05 -// LED outbut and brightness -#define PCA9685_LED0_ON_L 0x06 -#define PCA9685_LED0_ON_H 0x07 -#define PCA9685_LED0_OFF_L 0x08 -#define PCA9685_LED0_OFF_H 0x09 - -#define PCA9685_LED1_ON_L 0x0A -#define PCA9685_LED1_ON_H 0x0B -#define PCA9685_LED1_OFF_L 0x0C -#define PCA9685_LED1_OFF_H 0x0D - -#define PCA9685_LED2_ON_L 0x0E -#define PCA9685_LED2_ON_H 0x0F -#define PCA9685_LED2_OFF_L 0x10 -#define PCA9685_LED2_OFF_H 0x11 - -#define PCA9685_LED3_ON_L 0x12 -#define PCA9685_LED3_ON_H 0x13 -#define PCA9685_LED3_OFF_L 0x14 -#define PCA9685_LED3_OFF_H 0x15 - -#define PCA9685_LED4_ON_L 0x16 -#define PCA9685_LED4_ON_H 0x17 -#define PCA9685_LED4_OFF_L 0x18 -#define PCA9685_LED4_OFF_H 0x19 - -#define PCA9685_LED5_ON_L 0x1A -#define PCA9685_LED5_ON_H 0x1B -#define PCA9685_LED5_OFF_L 0x1C -#define PCA9685_LED5_OFF_H 0x1D - -#define PCA9685_LED6_ON_L 0x1E -#define PCA9685_LED6_ON_H 0x1F -#define PCA9685_LED6_OFF_L 0x20 -#define PCA9685_LED6_OFF_H 0x21 - -#define PCA9685_LED7_ON_L 0x22 -#define PCA9685_LED7_ON_H 0x23 -#define PCA9685_LED7_OFF_L 0x24 -#define PCA9685_LED7_OFF_H 0x25 - -#define PCA9685_LED8_ON_L 0x26 -#define PCA9685_LED8_ON_H 0x27 -#define PCA9685_LED8_OFF_L 0x28 -#define PCA9685_LED8_OFF_H 0x29 - -#define PCA9685_LED9_ON_L 0x2A -#define PCA9685_LED9_ON_H 0x2B -#define PCA9685_LED9_OFF_L 0x2C -#define PCA9685_LED9_OFF_H 0x2D - -#define PCA9685_LED10_ON_L 0x2E -#define PCA9685_LED10_ON_H 0x2F -#define PCA9685_LED10_OFF_L 0x30 -#define PCA9685_LED10_OFF_H 0x31 - -#define PCA9685_LED11_ON_L 0x32 -#define PCA9685_LED11_ON_H 0x33 -#define PCA9685_LED11_OFF_L 0x34 -#define PCA9685_LED11_OFF_H 0x35 - -#define PCA9685_LED12_ON_L 0x36 -#define PCA9685_LED12_ON_H 0x37 -#define PCA9685_LED12_OFF_L 0x38 -#define PCA9685_LED12_OFF_H 0x39 - -#define PCA9685_LED13_ON_L 0x3A -#define PCA9685_LED13_ON_H 0x3B -#define PCA9685_LED13_OFF_L 0x3C -#define PCA9685_LED13_OFF_H 0x3D - -#define PCA9685_LED14_ON_L 0x3E -#define PCA9685_LED14_ON_H 0x3F -#define PCA9685_LED14_OFF_L 0x40 -#define PCA9685_LED14_OFF_H 0x41 - -#define PCA9685_LED15_ON_L 0x42 -#define PCA9685_LED15_ON_H 0x43 -#define PCA9685_LED15_OFF_L 0x44 -#define PCA9685_LED15_OFF_H 0x45 - -#define PCA9685_ALL_LED_ON_L 0xFA -#define PCA9685_ALL_LED_ON_H 0xFB -#define PCA9685_ALL_LED_OFF_L 0xFC -#define PCA9685_ALL_LED_OFF_H 0xFD -#define PCA9685_PRE_SCALE 0xFE - -// Register Bits -#define PCA9685_ALLCALL 0x01 -#define PCA9685_OUTDRV 0x04 -#define PCA9685_RESTART 0x80 -#define PCA9685_SLEEP 0x10 -#define PCA9685_INVERT 0x10 - - - -#endif \ No newline at end of file diff --git a/src/rov/rov_pca9685/external/JHPWMPCA9685/license.txt b/src/rov/rov_pca9685/external/JHPWMPCA9685/license.txt deleted file mode 100644 index ec0c728..0000000 --- a/src/rov/rov_pca9685/external/JHPWMPCA9685/license.txt +++ /dev/null @@ -1,19 +0,0 @@ -Copyright (c) 2015 Jetsonhacks - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. \ No newline at end of file diff --git a/src/rov/rov_pca9685/external/JHPWMPCA9685/src/JHPWMPCA9685.cpp b/src/rov/rov_pca9685/external/JHPWMPCA9685/src/JHPWMPCA9685.cpp deleted file mode 100644 index 7cb5db6..0000000 --- a/src/rov/rov_pca9685/external/JHPWMPCA9685/src/JHPWMPCA9685.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include "JHPWMPCA9685/JHPWMPCA9685.h" -#include - - PCA9685::PCA9685(int address) { - kI2CBus = 1 ; // Default I2C bus for Jetson TK1 - kI2CAddress = address ; // Defaults to 0x40 for PCA9685 ; jumper settable - error = 0 ; -} - -PCA9685::~PCA9685() { - closePCA9685() ; -} - -bool PCA9685::openPCA9685() -{ - char fileNameBuffer[32]; - sprintf(fileNameBuffer,"/dev/i2c-%d", kI2CBus); - kI2CFileDescriptor = open(fileNameBuffer, O_RDWR); - if (kI2CFileDescriptor < 0) { - // Could not open the file - error = errno ; - return false ; - } - if (ioctl(kI2CFileDescriptor, I2C_SLAVE, kI2CAddress) < 0) { - // Could not open the device on the bus - error = errno ; - return false ; - } - return true ; -} - -void PCA9685::closePCA9685() -{ - if (kI2CFileDescriptor > 0) { - close(kI2CFileDescriptor); - // WARNING - This is not quite right, need to check for error first - kI2CFileDescriptor = -1 ; - } -} - -void PCA9685::reset () { - writeByte(PCA9685_MODE1, PCA9685_ALLCALL ); - writeByte(PCA9685_MODE2, PCA9685_OUTDRV) ; - // Wait for oscillator to stabilize - usleep(5000) ; -} - -// Sets the frequency of the PWM signal -// Frequency is ranged between 40 and 1000 Hertz -void PCA9685::setPWMFrequency ( float frequency ) { - printf("Setting PCA9685 PWM frequency to %f Hz\n",frequency) ; - float rangedFrequency = fmin(fmax(frequency,40),1000) ; - int prescale = (int)(25000000.0f / (4096 * rangedFrequency) - 0.5f) ; - // For debugging - // printf("PCA9685 Prescale: 0x%02X\n",prescale) ; - int oldMode = readByte(PCA9685_MODE1) ; - int newMode = ( oldMode & 0x7F ) | PCA9685_SLEEP ; - writeByte(PCA9685_MODE1, newMode) ; - writeByte(PCA9685_PRE_SCALE, prescale) ; - writeByte(PCA9685_MODE1, oldMode) ; - // Wait for oscillator to stabilize - usleep(5000) ; - writeByte(PCA9685_MODE1, oldMode | PCA9685_RESTART) ; -} - -// Channels 0-15 -// Channels are in sets of 4 bytes -void PCA9685::setPWM ( int channel, int onValue, int offValue) { - writeByte(PCA9685_LED0_ON_L+4*channel, onValue & 0xFF) ; - writeByte(PCA9685_LED0_ON_H+4*channel, onValue >> 8) ; - writeByte(PCA9685_LED0_OFF_L+4*channel, offValue & 0xFF) ; - writeByte(PCA9685_LED0_OFF_H+4*channel, offValue >> 8) ; -} - -void PCA9685::setAllPWM (int onValue, int offValue) { - writeByte(PCA9685_ALL_LED_ON_L, onValue & 0xFF) ; - writeByte(PCA9685_ALL_LED_ON_H, onValue >> 8) ; - writeByte(PCA9685_ALL_LED_OFF_L, offValue & 0xFF) ; - writeByte(PCA9685_ALL_LED_OFF_H, offValue >> 8) ; -} - - -// Read the given register -int PCA9685::readByte(int readRegister) -{ - int toReturn = i2c_smbus_read_byte_data(kI2CFileDescriptor, readRegister); - if (toReturn < 0) { - printf("PCA9685 Read Byte error: %d",errno) ; - error = errno ; - toReturn = -1 ; - } - // For debugging - // printf("Device 0x%02X returned 0x%02X from register 0x%02X\n", kI2CAddress, toReturn, readRegister); - return toReturn ; -} - -// Write the the given value to the given register -int PCA9685::writeByte(int writeRegister, int writeValue) -{ // For debugging: - // printf("Wrote: 0x%02X to register 0x%02X \n",writeValue, writeRegister) ; - int toReturn = i2c_smbus_write_byte_data(kI2CFileDescriptor, writeRegister, writeValue); - if (toReturn < 0) { - printf("PCA9685 Write Byte error: %d",errno) ; - error = errno ; - toReturn = -1 ; - } - return toReturn ; -} \ No newline at end of file diff --git a/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp b/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp new file mode 100644 index 0000000..38ba7d3 --- /dev/null +++ b/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp @@ -0,0 +1,157 @@ +#ifndef PCA9685_HPP +#define PCA9685_HPP + +#include + +namespace { + const uint8_t BIT7 = 0x80; + const uint8_t BIT6 = 0x40; + const uint8_t BIT5 = 0x20; + const uint8_t BIT4 = 0x10; + const uint8_t BIT3 = 0x08; + const uint8_t BIT2 = 0x04; + const uint8_t BIT1 = 0x02; + const uint8_t BIT0 = 0x01; +} + +class PCA9685 { +public: + enum Registers : uint8_t { + MODE1 = 0x00, + MODE2 = 0x01, + SUBADR1 = 0x02, + SUBADR2 = 0x03, + SUBADR3 = 0x04, + ALLCALLADR = 0x05, + LED0_ON_L = 0x06, + LED0_ON_H = 0x07, + LED0_OFF_L = 0x08, + LED0_OFF_H = 0x09, + LED1_ON_L = 0x0a, + LED1_ON_H = 0x0b, + LED1_OFF_L = 0x0c, + LED1_OFF_H = 0x0d, + LED2_ON_L = 0x0e, + LED2_ON_H = 0x0f, + LED2_OFF_L = 0x10, + LED2_OFF_H = 0x11, + LED3_ON_L = 0x12, + LED3_ON_H = 0x13, + LED3_OFF_L = 0x14, + LED3_OFF_H = 0x15, + LED4_ON_L = 0x16, + LED4_ON_H = 0x17, + LED4_OFF_L = 0x18, + LED4_OFF_H = 0x19, + LED5_ON_L = 0x1a, + LED5_ON_H = 0x1b, + LED5_OFF_L = 0x1c, + LED5_OFF_H = 0x1d, + LED6_ON_L = 0x1e, + LED6_ON_H = 0x1f, + LED6_OFF_L = 0x20, + LED6_OFF_H = 0x21, + LED7_ON_L = 0x22, + LED7_ON_H = 0x23, + LED7_OFF_L = 0x24, + LED7_OFF_H = 0x25, + LED8_ON_L = 0x26, + LED8_ON_H = 0x27, + LED8_OFF_L = 0x28, + LED8_OFF_H = 0x29, + LED9_ON_L = 0x2a, + LED9_ON_H = 0x2b, + LED9_OFF_L = 0x2c, + LED9_OFF_H = 0x2d, + LED10_ON_L = 0x2e, + LED10_ON_H = 0x2f, + LED10_OFF_L = 0x30, + LED10_OFF_H = 0x31, + LED11_ON_L = 0x32, + LED11_ON_H = 0x33, + LED11_OFF_L = 0x34, + LED11_OFF_H = 0x35, + LED12_ON_L = 0x36, + LED12_ON_H = 0x37, + LED12_OFF_L = 0x38, + LED12_OFF_H = 0x39, + LED13_ON_L = 0x3a, + LED13_ON_H = 0x3b, + LED13_OFF_L = 0x3c, + LED13_OFF_H = 0x3d, + LED14_ON_L = 0x3e, + LED14_ON_H = 0x3f, + LED14_OFF_L = 0x40, + LED14_OFF_H = 0x41, + LED15_ON_L = 0x42, + LED15_ON_H = 0x43, + LED15_OFF_L = 0x44, + LED15_OFF_H = 0x45, + ALL_LED_ON_L = 0xFA, + ALL_LED_ON_H = 0xFB, + ALL_LED_OFF_L = 0xFC, + ALL_LED_OFF_H = 0xFD, + PRE_SCALE = 0xFE, + TestMode = 0xFF + }; + + enum RegisterMasks : uint8_t { + MODE1_RESTART = BIT7, + MODE1_EXTCLK = BIT6, + MODE1_AI = BIT5, + MODE1_SLEEP = BIT4, + MODE1_SUB1 = BIT3, + MODE1_SUB2 = BIT2, + MODE1_SUB3 = BIT1, + MODE1_ALLCALL = BIT0, + + MODE2_INVRT = BIT4, + MODE2_OCH = BIT3, + MODE2_OUTDRV = BIT2, + MODE2_OUTNE_H = BIT1, + MODE2_OUTNE_L = BIT0 + }; + + PCA9685(uint8_t bus = 7, uint8_t address=0x40); + PCA9685(const PCA9685&) = delete; + PCA9685(PCA9685&&) = default; + ~PCA9685(); + + PCA9685& operator=(const PCA9685&) = delete; + PCA9685& operator=(PCA9685&&) = default; + + // attempt to connect to the pca9685 + bool Open(); + + // invalidate all pwms and close the connection with the pca9685 + void Close(); + + // invalidate all pwms and restart the pca9685 + void Reset(); + + // change the output pwm frequency between 24 and 1526 Hz + void setFrequency(uint16_t frequency); + + void setAllPWM(uint16_t on_counts, uint16_t off_counts); + void setAllOff(); + void setAllOn(); + + // set the duty cycle of a channel, duty in [0,1] + void setDuty(uint8_t channel, float duty, float delay=0.0f); + + // set the output by us + void setUS(uint8_t channel, uint16_t us, uint16_t delay_us=0); + + void setOn(uint8_t channel); + void setOff(uint8_t channel); +private: + int i2c_fd; + uint8_t i2c_address; + uint8_t i2c_bus; + + float clock_frequency; + float clock_us; + uint16_t output_frequency; +}; + +#endif \ No newline at end of file diff --git a/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp b/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp index 446d9a2..f5c9071 100644 --- a/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp +++ b/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp @@ -1,7 +1,7 @@ #ifndef SERVO_DRIVER_HPP #define SERVO_DRIVER_HPP -#include "JHPWMPCA9685/JHPWMPCA9685.h" +#include "pca9685/PCA9685.hpp" #include enum class ServoType{ diff --git a/src/rov/rov_pca9685/src/PCA9685.cpp b/src/rov/rov_pca9685/src/PCA9685.cpp new file mode 100644 index 0000000..100f616 --- /dev/null +++ b/src/rov/rov_pca9685/src/PCA9685.cpp @@ -0,0 +1,146 @@ +#include "pca9685/PCA9685.hpp" + +#include "linux/i2c-dev.h" +#include "i2c/smbus.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#define US_PER_S 1e6 + +PCA9685::PCA9685(uint8_t bus, uint8_t address) : i2c_bus(bus), i2c_address(address) { + clock_frequency = 25e6; + // 1e6 us per s / 25e6 hz = 0.04 us + clock_us = US_PER_S / 25e6; +} + +PCA9685::~PCA9685() { + Close(); +} + +bool PCA9685::Open() { + char filename[32]; + sprintf(filename,"/dev/i2c-%d", i2c_bus); + i2c_fd = open(filename, O_RDWR); + if (i2c_fd < 0) { + // Could not open the bus + return false; + } + if (ioctl(i2c_fd, I2C_SLAVE, i2c_address) < 0) { + // Could not open the device on the bus + return false; + } + + // external clock? + // i2c_smbus_write_byte_data(i2c_fd, MODE1, MODE1_SLEEP); + // usleep(500); + // i2c_smbus_write_byte_data(ic2_fd, MODE1, MODE1_SLEEP | MODE1_EXTCLK); + + setFrequency(200); + + return true; +} + +void PCA9685::Close() { + setAllOff(); + usleep(500); + if (i2c_fd > 0) { + close(i2c_fd); + } +} + +void PCA9685::Reset() { + // stop all pwms + i2c_smbus_write_byte_data(i2c_fd, Registers::ALL_LED_OFF_H, BIT4); + // if restart is a logic 1 + uint8_t mode1 = i2c_smbus_read_byte_data(i2c_fd, Registers::MODE1); + if (mode1 & MODE1_RESTART) { + // clear bit 4 + i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, mode1 & ~MODE1_SLEEP); + // wait 500 us before setting logic 1 to restart + usleep(500); + i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, MODE1_RESTART); + } +} + +void PCA9685::setFrequency(uint16_t frequency) { + // if given frequency is not valid, do nothing + if (frequency < 24 || frequency > 1526) { + return; + } + output_frequency = frequency; + uint8_t prescale = static_cast(std::round(25e6/(4096 * frequency))) - 1; + uint8_t current_mode = i2c_smbus_read_byte_data(i2c_fd, Registers::MODE1); + // set sleep bit + i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, current_mode | MODE1_SLEEP); + usleep(500); + // set the prescalar + i2c_smbus_write_byte_data(i2c_fd, Registers::PRE_SCALE, prescale); + // reset the mode + i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, current_mode); +} + +void PCA9685::setAllPWM(uint16_t on_counts, uint16_t off_counts) { + i2c_smbus_write_byte_data(i2c_fd, Registers::ALL_LED_ON_L, on_counts & 0xFF); + i2c_smbus_write_byte_data(i2c_fd, Registers::ALL_LED_ON_H, on_counts >> 8); + i2c_smbus_write_byte_data(i2c_fd, Registers::ALL_LED_OFF_L, off_counts & 0xFF); + i2c_smbus_write_byte_data(i2c_fd, Registers::ALL_LED_OFF_H, off_counts >> 8); +} + +void PCA9685::setAllOff() { + // clear existing full on bit before setting full off + i2c_smbus_write_byte_data(i2c_fd, ALL_LED_ON_H, i2c_smbus_read_byte_data(i2c_fd, ALL_LED_ON_H) & ~BIT4); + i2c_smbus_write_byte_data(i2c_fd, ALL_LED_OFF_H, BIT4); +} + +void PCA9685::setAllOn() { + // clear existing full off bit before setting full on + i2c_smbus_write_byte_data(i2c_fd, ALL_LED_OFF_H, i2c_smbus_read_byte_data(i2c_fd, ALL_LED_OFF_H) & ~BIT4); + i2c_smbus_write_byte_data(i2c_fd, ALL_LED_ON_H, BIT4); +} + +void PCA9685::setDuty(uint8_t channel, float duty, float _delay) { + uint16_t time_on = static_cast(std::round(duty * 4096)); + uint16_t delay = static_cast(std::round(_delay * 4096)); + uint16_t time_off = delay+time_on - 1; + time_off = time_off > 4096 ? time_off - 4096 : time_off; + + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), time_on >> 8); + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_L + (channel << 2), time_on & 0xFF); + i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), time_off >> 8); + i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_L + (channel << 2), time_off & 0xFF); +} + +void PCA9685::setUS(uint8_t channel, uint16_t us, uint16_t delay_us) { + // calculate the maximum pulse width at the current output frequency + float max_pulse_width_us = US_PER_S / output_frequency; + // calculate the percent duty cycles for on and delay + float duty = us/max_pulse_width_us; + float delay = delay_us/max_pulse_width_us; + // ensure within [0.0, 1.0] + duty = duty > 1.0f ? 1.0f : duty; + duty = duty < 0.0f ? 0.0f : duty; + delay = delay > 1.0f ? 1.0f : delay; + delay = delay < 0.0f ? 0.0f : delay; + // set the duty cycle + setDuty(channel, duty, delay); +} + +void PCA9685::setOn(uint8_t channel) { + // clear existing full off bit before setting full on + i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_OFF_H + channel << 2) & ~BIT4); + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), BIT4); +} + +void PCA9685::setOff(uint8_t channel) { + // clear existing full on bit before setting full off + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_ON_H + channel << 2) & ~BIT4); + i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), BIT4); +} \ No newline at end of file diff --git a/src/rov/rov_pca9685/src/ServoDriver.cpp b/src/rov/rov_pca9685/src/ServoDriver.cpp index 853f790..2f42cbd 100644 --- a/src/rov/rov_pca9685/src/ServoDriver.cpp +++ b/src/rov/rov_pca9685/src/ServoDriver.cpp @@ -2,18 +2,20 @@ #include #define NUM_COUNTS 4177 +#define BUS_DEV 7 +#define ADDRESS 0x40 static int f2imap(float value, float from_min, float from_max, int to_min, int to_max){ return ((value - from_min) * ((to_max - to_min) / (from_max - from_min)) + to_min); } ServoDriver::ServoDriver(){ - this->driver_board = PCA9685(0x41); - if(!this->driver_board.openPCA9685()) { + this->driver_board = PCA9685(BUS_DEV, ADDRESS); + if(!this->driver_board.Open()) { throw std::runtime_error("Failed to open PCA9685"); } this->driver_board.setAllPWM(0,0); - this->driver_board.reset(); + this->driver_board.Reset(); this->setPWMFrequency(this->driver_board_frequency); } @@ -122,9 +124,6 @@ void ServoDriver::setPWMBounds(int channel, int minimum_us, int maximum_us){ void ServoDriver::setPWM(int channel, int value){ if(this->isChannelInUse(channel)){ -#if DEBUG_OUTPUT - printf("Setting pin %i to pwm %i", channel, (int)(value * (1e6 / this->driver_board_frequency / NUM_COUNTS))); -#endif this->driver_board.setPWM(channel, 0, value); } } From 1dee8bfcd71b2ecf1e7ecb458240128639716c7e Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Sun, 23 Feb 2025 01:29:18 -0700 Subject: [PATCH 2/7] compiles and runs but there are probably errors --- src/rov/rov_pca9685/CMakeLists.txt | 23 ++- .../include/pca9685/PCA9685_Node.hpp | 1 - .../include/pca9685/ServoDriver.hpp | 71 ++++--- src/rov/rov_pca9685/src/PCA9685.cpp | 8 +- src/rov/rov_pca9685/src/PCA9685_Node.cpp | 7 +- src/rov/rov_pca9685/src/ServoDriver.cpp | 188 +++++------------- src/rov/rov_pca9685/src/main_interactive.cpp | 45 +---- 7 files changed, 115 insertions(+), 228 deletions(-) diff --git a/src/rov/rov_pca9685/CMakeLists.txt b/src/rov/rov_pca9685/CMakeLists.txt index 6dec674..ad754ad 100644 --- a/src/rov/rov_pca9685/CMakeLists.txt +++ b/src/rov/rov_pca9685/CMakeLists.txt @@ -23,29 +23,36 @@ find_package(ament_cmake REQUIRED) find_package(rov_interfaces REQUIRED) find_package(rclcpp REQUIRED) -add_library(pca9685 src/PCA9685_Node.cpp src/ServoDriver.cpp src/PCA9685.cpp) -add_executable(pca9685_node src/main.cpp) +add_library(pca9685 src/ServoDriver.cpp src/PCA9685.cpp) +add_executable(pca9685_node src/main.cpp src/PCA9685_Node.cpp) add_executable(pca9685_interactive_test src/main_interactive.cpp) target_include_directories(pca9685 PUBLIC $ $ ) - -target_link_libraries(pca9685_node pca9685) -target_link_libraries(pca9685_interactive_test pca9685) +target_include_directories(pca9685_node PUBLIC + $ + $ +) ament_target_dependencies(pca9685 rclcpp rov_interfaces ) +ament_target_dependencies(pca9685_node + rclcpp + rov_interfaces +) -install(TARGETS pca9685_node pca9685 pca9685_interactive_test +target_link_libraries(pca9685 i2c) +target_link_libraries(pca9685_node i2c pca9685) +target_link_libraries(pca9685_interactive_test i2c pca9685) + +install(TARGETS pca9685 pca9685_node pca9685_interactive_test DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/) -install(DIRECTORY external/JHPWMPCA9685/include/ - DESTINATION include/) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/src/rov/rov_pca9685/include/pca9685/PCA9685_Node.hpp b/src/rov/rov_pca9685/include/pca9685/PCA9685_Node.hpp index 37280ae..d5e9f1c 100644 --- a/src/rov/rov_pca9685/include/pca9685/PCA9685_Node.hpp +++ b/src/rov/rov_pca9685/include/pca9685/PCA9685_Node.hpp @@ -8,7 +8,6 @@ #include "rov_interfaces/srv/create_servo.hpp" #include "pca9685/ServoDriver.hpp" -#include "JHPWMPCA9685/JHPWMPCA9685.h" class PCA9685_Node : public rclcpp::Node { public: diff --git a/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp b/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp index f5c9071..665ae9f 100644 --- a/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp +++ b/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp @@ -3,50 +3,47 @@ #include "pca9685/PCA9685.hpp" #include +#include -enum class ServoType{ +enum class ServoType : uint8_t { POSITIONAL, CONTINUOUS }; -struct Servo{ +struct ServoBase { bool initialized = false; - ServoType type; - float setpoint = 0.0f; - float setpoint_minimum = 0.0f; - float setpoint_maximum = 0.0f; - int pwm_minimum; - int pwm_maximum; }; -class ServoDriver{ - public: - ServoDriver(); - ~ServoDriver(); - - void addServo(int channel); - void addContinuousServo(int channel); - void removeServo(int channel); - void removeContinuousServo(int channel); - void setAngle(int channel, float angle); - void setThrottle(int channel, float throttle); - void setOutput(int channel, float value); - void setAngleBounds(int channel, float minimum_angle, float maximum_angle); - void setThrottleBounds(int channel, float minimum_throttle, float maximum_throttle); - void setPWMBounds(int channel, int minimum_us, int maximum_us); - - private: - PCA9685 driver_board; - float driver_board_frequency = 60; - Servo servos[16]; - - void setPWM(int channel, int value); - void setPWMFrequency(float frequency); - bool isChannelInUse(int channel); - bool isChannelNotInUse(int channel); - bool isServoChannelInUse(int channel); - bool isContinuousServoChannelInUse(int channel); - float getCountsPerMicrosecond(); +struct Servo : ServoBase { + uint16_t us_minimum; + uint16_t us_maximum; }; -#endif // SERVO_DRIVER_HPP \ No newline at end of file +struct ContinuousServo : ServoBase { + uint16_t us_minimum; + uint16_t us_maximum; +}; + +class ServoDriver { +public: + ServoDriver(); + ~ServoDriver(); + + void registerServo(uint8_t channel, ServoType type); + + void setDuty(uint8_t channel, float duty); + void setUS(uint8_t channel, uint16_t us); + + void setThrottle(uint8_t channel, float throttle); + void setAngle(uint8_t channel, float angle); + + void setUSBounds(uint8_t channel, uint16_t min_us, uint16_t max_us); + + void setOutput(uint8_t channel, float angle_or_throttle); +private: + PCA9685 driver_board; + std::map servos; + std::map continuous_servos; +}; + +#endif \ No newline at end of file diff --git a/src/rov/rov_pca9685/src/PCA9685.cpp b/src/rov/rov_pca9685/src/PCA9685.cpp index 100f616..13665b2 100644 --- a/src/rov/rov_pca9685/src/PCA9685.cpp +++ b/src/rov/rov_pca9685/src/PCA9685.cpp @@ -1,7 +1,9 @@ #include "pca9685/PCA9685.hpp" +extern "C" { #include "linux/i2c-dev.h" #include "i2c/smbus.h" +} #include #include @@ -15,7 +17,7 @@ #define US_PER_S 1e6 -PCA9685::PCA9685(uint8_t bus, uint8_t address) : i2c_bus(bus), i2c_address(address) { +PCA9685::PCA9685(uint8_t bus, uint8_t address) : i2c_address(address), i2c_bus(bus) { clock_frequency = 25e6; // 1e6 us per s / 25e6 hz = 0.04 us clock_us = US_PER_S / 25e6; @@ -135,12 +137,12 @@ void PCA9685::setUS(uint8_t channel, uint16_t us, uint16_t delay_us) { void PCA9685::setOn(uint8_t channel) { // clear existing full off bit before setting full on - i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_OFF_H + channel << 2) & ~BIT4); + i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_OFF_H + (channel << 2)) & ~BIT4); i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), BIT4); } void PCA9685::setOff(uint8_t channel) { // clear existing full on bit before setting full off - i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_ON_H + channel << 2) & ~BIT4); + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_ON_H + (channel << 2)) & ~BIT4); i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), BIT4); } \ No newline at end of file diff --git a/src/rov/rov_pca9685/src/PCA9685_Node.cpp b/src/rov/rov_pca9685/src/PCA9685_Node.cpp index 5bc3cae..59d4c41 100644 --- a/src/rov/rov_pca9685/src/PCA9685_Node.cpp +++ b/src/rov/rov_pca9685/src/PCA9685_Node.cpp @@ -13,16 +13,13 @@ PCA9685_Node::PCA9685_Node() : Node(std::string("pca9685")) { } void PCA9685_Node::topic_callback(const rov_interfaces::msg::PWM::SharedPtr msg) { -#if DEBUG_OUTPUT - RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "PCA9685 set channel %i to %f", msg->channel, msg->angle_or_throttle); -#endif servoDriver.setOutput(msg->channel, msg->angle_or_throttle); } void PCA9685_Node::create_continuous_servo(const std::shared_ptr request, std::shared_ptr response) { RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "PCA9685 Received Continuous Servo Creation Request on Channel %i", request->channel); - servoDriver.addContinuousServo(request->channel); + servoDriver.registerServo(request->channel, ServoType::CONTINUOUS); response->result = true; response->channel = request->channel; } @@ -30,7 +27,7 @@ void PCA9685_Node::create_continuous_servo(const std::shared_ptr request, std::shared_ptr response) { RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "PCA9685 Received Servo Creation Request on Channel %i", request->channel); - servoDriver.addServo(request->channel); + servoDriver.registerServo(request->channel, ServoType::POSITIONAL); response->result = true; response->channel = request->channel; } \ No newline at end of file diff --git a/src/rov/rov_pca9685/src/ServoDriver.cpp b/src/rov/rov_pca9685/src/ServoDriver.cpp index 2f42cbd..558f6d0 100644 --- a/src/rov/rov_pca9685/src/ServoDriver.cpp +++ b/src/rov/rov_pca9685/src/ServoDriver.cpp @@ -1,5 +1,6 @@ #include "pca9685/ServoDriver.hpp" #include +#include #define NUM_COUNTS 4177 #define BUS_DEV 7 @@ -9,162 +10,73 @@ static int f2imap(float value, float from_min, float from_max, int to_min, int t return ((value - from_min) * ((to_max - to_min) / (from_max - from_min)) + to_min); } -ServoDriver::ServoDriver(){ +ServoDriver::ServoDriver() { this->driver_board = PCA9685(BUS_DEV, ADDRESS); if(!this->driver_board.Open()) { throw std::runtime_error("Failed to open PCA9685"); } - this->driver_board.setAllPWM(0,0); - this->driver_board.Reset(); - this->setPWMFrequency(this->driver_board_frequency); -} - -ServoDriver::~ServoDriver(){} - -void ServoDriver::addServo(int channel){ - if(this->isChannelNotInUse(channel)){ - Servo new_servo; - new_servo.initialized = true; - new_servo.type = ServoType::POSITIONAL; - new_servo.setpoint_minimum = 0.0f; - new_servo.setpoint_maximum = 180.0f; - this->servos[channel] = new_servo; - this->setPWMBounds(channel, 500, 2500); + this->driver_board.setAllOff(); +} + +ServoDriver::~ServoDriver() {} + +void ServoDriver::registerServo(uint8_t channel, ServoType type) { + switch (type) + { + case ServoType::POSITIONAL: + servos[channel] = Servo(); + setAngle(channel, 90.0f); + break; + + case ServoType::CONTINUOUS: + continuous_servos[channel] = ContinuousServo(); + setThrottle(channel, 0.0f); + break; } } -void ServoDriver::addContinuousServo(int channel){ - if(this->isChannelNotInUse(channel)){ - Servo new_servo; - new_servo.initialized = true; - new_servo.type = ServoType::CONTINUOUS; - new_servo.setpoint_minimum = -1.0f; - new_servo.setpoint_maximum = 1.0f; - this->servos[channel] = new_servo; - this->setPWMBounds(channel, 1100, 1900); - } +void ServoDriver::setDuty(uint8_t channel, float duty) { + driver_board.setDuty(channel, duty); } -void ServoDriver::removeServo(int channel){ - if(this->isServoChannelInUse(channel)){ - this->servos[channel].initialized = false; - } -} - -void ServoDriver::removeContinuousServo(int channel){ - if(this->isContinuousServoChannelInUse(channel)){ - this->servos[channel].initialized = false; - } -} - -void ServoDriver::setAngle(int channel, float angle){ - if(this->isServoChannelInUse(channel)){ - Servo* working_servo = &this->servos[channel]; - if(!(angle >= working_servo->setpoint_minimum && angle <= working_servo->setpoint_maximum)) { - throw std::domain_error("Servo angle" + std::to_string(angle) + " is not in range [" + std::to_string(working_servo->setpoint_minimum) + ", " + std::to_string(working_servo->setpoint_maximum) + "]"); - } - working_servo->setpoint = angle; - this->setPWM(channel, f2imap(working_servo->setpoint, working_servo->setpoint_minimum, working_servo->setpoint_maximum, working_servo->pwm_minimum, working_servo->pwm_maximum)); - } else { - std::cerr << "[ERROR] Attempted to set an angle for a continuous servo" << std::endl; - } -} - -void ServoDriver::setThrottle(int channel, float throttle){ - if(this->isContinuousServoChannelInUse(channel)){ - Servo* working_servo = &this->servos[channel]; - if(!(throttle >= working_servo->setpoint_minimum && throttle <= working_servo->setpoint_maximum)) { - throw std::domain_error("Servo angle " + std::to_string(throttle) + "is not in range [" + std::to_string(working_servo->setpoint_minimum) + ", " + std::to_string(working_servo->setpoint_maximum) + "]"); - } - working_servo->setpoint = throttle; - this->setPWM(channel, f2imap(working_servo->setpoint, working_servo->setpoint_minimum, working_servo->setpoint_maximum, working_servo->pwm_minimum, working_servo->pwm_maximum)); - - } else { - std::cerr << "[ERROR] Attempted to set a throttle for a positional servo" << std::endl; - } -} - -void ServoDriver::setOutput(int channel, float value) { - if(isChannelInUse(channel)) { - switch(servos[channel].type) { - case ServoType::CONTINUOUS: - setThrottle(channel, value); - break; - case ServoType::POSITIONAL: - setAngle(channel, value); - break; - } - } +void ServoDriver::setUS(uint8_t channel, uint16_t us) { + driver_board.setUS(channel, us); } -void ServoDriver::setAngleBounds(int channel, float minimum_angle, float maximum_angle){ - if(this->isServoChannelInUse(channel)){ - Servo* working_servo = &this->servos[channel]; - working_servo->setpoint_minimum = minimum_angle; - working_servo->setpoint_maximum = maximum_angle; +void ServoDriver::setThrottle(uint8_t channel, float throttle) { + if (!continuous_servos.count(channel)) { + RCLCPP_WARN(rclcpp::get_logger("ServoDriver"), "Attempted to set throttle on non-continuous servo"); + return; } + ContinuousServo* s = &continuous_servos[channel]; + driver_board.setUS(channel, f2imap(throttle, -1.0, 1.0, s->us_minimum, s->us_maximum)); } -void ServoDriver::setThrottleBounds(int channel, float minimum_throttle, float maximum_throttle){ - if(this->isContinuousServoChannelInUse(channel)){ - Servo* working_servo = &this->servos[channel]; - working_servo->setpoint_minimum = minimum_throttle; - working_servo->setpoint_maximum = maximum_throttle; +void ServoDriver::setAngle(uint8_t channel, float angle) { + if (!servos.count(channel)) { + RCLCPP_WARN(rclcpp::get_logger("ServoDriver"), "Attempted to set angle on continuous servo"); + return; } + Servo* s = &servos[channel]; + driver_board.setUS(channel, f2imap(angle, 0.0, 180.0, s->us_minimum, s->us_maximum)); } -void ServoDriver::setPWMBounds(int channel, int minimum_us, int maximum_us){ - if(this->isChannelInUse(channel)){ - Servo* working_servo = &this->servos[channel]; - float counts_per_microsecond = this->getCountsPerMicrosecond(); - working_servo->pwm_minimum = minimum_us * counts_per_microsecond; - working_servo->pwm_maximum = maximum_us * counts_per_microsecond; +void ServoDriver::setUSBounds(uint8_t channel, uint16_t min_us, uint16_t max_us) { + if (servos.count(channel)) { + servos[channel].us_minimum = min_us; + servos[channel].us_maximum = max_us; + } else if (continuous_servos.count(channel)) { + continuous_servos[channel].us_minimum = min_us; + continuous_servos[channel].us_maximum = max_us; } + RCLCPP_WARN(rclcpp::get_logger("ServoDriver"), "Attempted to set the bounds of an unregistered servo"); } -void ServoDriver::setPWM(int channel, int value){ - if(this->isChannelInUse(channel)){ - this->driver_board.setPWM(channel, 0, value); +void ServoDriver::setOutput(uint8_t channel, float angle_or_throttle) { + if (servos.count(channel)) { + setAngle(channel, angle_or_throttle); + } else if (continuous_servos.count(channel)) { + setThrottle(channel, angle_or_throttle); } -} - -void ServoDriver::setPWMFrequency(float frequency){ - this->driver_board.setPWMFrequency(frequency); -} - -bool ServoDriver::isChannelInUse(int channel){ - if(!(channel >= 0 && channel <= 15)) { - throw std::domain_error("Channel number " + std::to_string(channel) + " is not in range [0, 15]"); - } - return this->servos[channel].initialized; -} - -bool ServoDriver::isChannelNotInUse(int channel){ - return !isChannelInUse(channel); -} - -bool ServoDriver::isServoChannelInUse(int channel){ - if(this->isChannelInUse(channel)) { - if(servos[channel].type != ServoType::POSITIONAL) { - throw std::logic_error("Requested servo is already initialized as continuous"); - } - return true; - }else{ - return false; - } -} - -bool ServoDriver::isContinuousServoChannelInUse(int channel){ - if(this->isChannelInUse(channel)) { - if(servos[channel].type != ServoType::CONTINUOUS) { - throw std::logic_error("Requested servo is already initialized as positional"); - } - return true; - } else { - return false; - } -} - -float ServoDriver::getCountsPerMicrosecond(){ - return (this->driver_board_frequency * NUM_COUNTS) / 1e6; -} + RCLCPP_WARN(rclcpp::get_logger("ServoDriver"), "Attempted to set the output of an unregistered servo"); +} \ No newline at end of file diff --git a/src/rov/rov_pca9685/src/main_interactive.cpp b/src/rov/rov_pca9685/src/main_interactive.cpp index 5e2333b..cd3693e 100644 --- a/src/rov/rov_pca9685/src/main_interactive.cpp +++ b/src/rov/rov_pca9685/src/main_interactive.cpp @@ -1,8 +1,6 @@ #include #include -#include - #include #include @@ -24,18 +22,16 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { if(userInput.at(0) == 'c') { // load continuous servo defaults for(int i=0; i<16; i++) { - servoDriver.addContinuousServo(i); + servoDriver.registerServo(i, ServoType::CONTINUOUS); isContinuous[i]=true; - servoDriver.setPWMBounds(i, 1100, 1900); - servoDriver.setThrottleBounds(i, -1, 1); + servoDriver.setUSBounds(i, 1100, 1900); } } else { // load non continuous servo defaults for(int i=0; i<16; i++) { - servoDriver.addServo(i); + servoDriver.registerServo(i, ServoType::POSITIONAL); isContinuous[i]=false; - servoDriver.setPWMBounds(i, 500, 2500); - servoDriver.setAngleBounds(i, 0, 180); + servoDriver.setUSBounds(i, 500, 2500); } } } else { @@ -47,20 +43,20 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { boost::algorithm::to_lower(userInput); try { if(userInput.at(0) == 'c') { - servoDriver.addContinuousServo(i); + servoDriver.registerServo(i, ServoType::CONTINUOUS); isContinuous[i] = true; } else { // default to non continuous - servoDriver.addServo(i); + servoDriver.registerServo(i, ServoType::POSITIONAL); isContinuous[i] = false; } } catch(std::out_of_range &) { - servoDriver.addServo(i); + servoDriver.registerServo(i, ServoType::POSITIONAL); isContinuous[i] = false; } } - std::cout << "\nFor all 16 pins, enter PWM range separated by spaces (newline for default 500-2500)\n"; + std::cout << "\nFor all 16 pins, enter US range separated by spaces (newline for default 500-2500)\n"; for(int i = 0; i < 16; i++) { std::cout << "Pin " << i << ": "; std::getline(std::cin, userInput); @@ -72,30 +68,7 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { lower_bound = boost::lexical_cast(userInput.substr(0, idx)); upper_bound = boost::lexical_cast(userInput.substr(idx+1)); } - servoDriver.setPWMBounds(i, lower_bound, upper_bound); - } - - std::cout << "\nFor all 16 pins, set boundaries for Angle/Throttle\n"; - for(int i = 0; i < 16; i++) { - std::cout << "Pin " << i << ": "; - std::getline(std::cin, userInput); - std::cout << '\n'; - boost::algorithm::to_lower(userInput); - int lower_bound = 0; - int upper_bound = 180; - size_t idx = userInput.find(' '); - if(idx != 0 && idx != std::string::npos) { - lower_bound = boost::lexical_cast(userInput.substr(0, idx)); - upper_bound = boost::lexical_cast(userInput.substr(idx+1)); - } else { - servoDriver.setAngleBounds(i, lower_bound, upper_bound); - continue; - } - if(isContinuous.at(i)) { - servoDriver.setThrottleBounds(i, lower_bound, upper_bound); - } else { - servoDriver.setAngleBounds(i, lower_bound, upper_bound); - } + servoDriver.setUSBounds(i, lower_bound, upper_bound); } } From e30434568ae348085f294b2c333249e4d15bf672 Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Tue, 25 Mar 2025 21:04:17 -0600 Subject: [PATCH 3/7] updated API, working now --- .../rov_pca9685/include/pca9685/PCA9685.hpp | 2 + src/rov/rov_pca9685/src/PCA9685.cpp | 90 ++++++++++++++----- src/rov/rov_pca9685/src/ServoDriver.cpp | 17 +++- src/rov/rov_pca9685/src/main_interactive.cpp | 1 + 4 files changed, 82 insertions(+), 28 deletions(-) diff --git a/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp b/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp index 38ba7d3..8c0b73a 100644 --- a/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp +++ b/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp @@ -145,6 +145,8 @@ class PCA9685 { void setOn(uint8_t channel); void setOff(uint8_t channel); private: + void helloworld(); + int i2c_fd; uint8_t i2c_address; uint8_t i2c_bus; diff --git a/src/rov/rov_pca9685/src/PCA9685.cpp b/src/rov/rov_pca9685/src/PCA9685.cpp index 13665b2..7cbcd46 100644 --- a/src/rov/rov_pca9685/src/PCA9685.cpp +++ b/src/rov/rov_pca9685/src/PCA9685.cpp @@ -21,6 +21,7 @@ PCA9685::PCA9685(uint8_t bus, uint8_t address) : i2c_address(address), i2c_bus(b clock_frequency = 25e6; // 1e6 us per s / 25e6 hz = 0.04 us clock_us = US_PER_S / 25e6; + output_frequency = 200; } PCA9685::~PCA9685() { @@ -30,10 +31,12 @@ PCA9685::~PCA9685() { bool PCA9685::Open() { char filename[32]; sprintf(filename,"/dev/i2c-%d", i2c_bus); + printf("%s\n", filename); i2c_fd = open(filename, O_RDWR); if (i2c_fd < 0) { // Could not open the bus - return false; + + return false; } if (ioctl(i2c_fd, I2C_SLAVE, i2c_address) < 0) { // Could not open the device on the bus @@ -45,7 +48,23 @@ bool PCA9685::Open() { // usleep(500); // i2c_smbus_write_byte_data(ic2_fd, MODE1, MODE1_SLEEP | MODE1_EXTCLK); - setFrequency(200); + usleep(500); + setFrequency(60); + + // startup pattern + // for (uint8_t idx = 0; idx < 16; idx++) { + // setOn(idx); + // usleep(5000); + // setAllOff(); + // } + // for (uint8_t idx = 14; idx < 16; idx--) { + // setOn(idx); + // usleep(5000); + // setAllOff(); + // } + + // clear all registers + Reset(); return true; } @@ -61,24 +80,29 @@ void PCA9685::Close() { void PCA9685::Reset() { // stop all pwms i2c_smbus_write_byte_data(i2c_fd, Registers::ALL_LED_OFF_H, BIT4); - // if restart is a logic 1 - uint8_t mode1 = i2c_smbus_read_byte_data(i2c_fd, Registers::MODE1); - if (mode1 & MODE1_RESTART) { - // clear bit 4 - i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, mode1 & ~MODE1_SLEEP); - // wait 500 us before setting logic 1 to restart + usleep(500); + + uint8_t mode1_current = i2c_smbus_read_byte_data(i2c_fd, Registers::MODE1); + if (mode1_current & MODE1_RESTART) { + i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, mode1_current & ~MODE1_SLEEP); usleep(500); - i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, MODE1_RESTART); } + i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, (mode1_current & ~MODE1_SLEEP) | MODE1_RESTART); +} + +void PCA9685::helloworld() { + } void PCA9685::setFrequency(uint16_t frequency) { + static const uint16_t NUM_COUNTS = 4096; // scaling factor since clock sucks balls lol // if given frequency is not valid, do nothing if (frequency < 24 || frequency > 1526) { return; } output_frequency = frequency; - uint8_t prescale = static_cast(std::round(25e6/(4096 * frequency))) - 1; + uint8_t prescale = static_cast(std::round(25e6/(NUM_COUNTS * frequency))) - 1; + // printf("prescale: 0x%02x", prescale); uint8_t current_mode = i2c_smbus_read_byte_data(i2c_fd, Registers::MODE1); // set sleep bit i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, current_mode | MODE1_SLEEP); @@ -87,6 +111,10 @@ void PCA9685::setFrequency(uint16_t frequency) { i2c_smbus_write_byte_data(i2c_fd, Registers::PRE_SCALE, prescale); // reset the mode i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, current_mode); + + // restart + usleep(500); + i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, current_mode | MODE1_RESTART); } void PCA9685::setAllPWM(uint16_t on_counts, uint16_t off_counts) { @@ -97,27 +125,38 @@ void PCA9685::setAllPWM(uint16_t on_counts, uint16_t off_counts) { } void PCA9685::setAllOff() { - // clear existing full on bit before setting full off - i2c_smbus_write_byte_data(i2c_fd, ALL_LED_ON_H, i2c_smbus_read_byte_data(i2c_fd, ALL_LED_ON_H) & ~BIT4); + // clear existing full on bit after setting full off (off takes precedence) i2c_smbus_write_byte_data(i2c_fd, ALL_LED_OFF_H, BIT4); + i2c_smbus_write_byte_data(i2c_fd, ALL_LED_ON_H, i2c_smbus_read_byte_data(i2c_fd, ALL_LED_ON_H) & ~BIT4); } void PCA9685::setAllOn() { - // clear existing full off bit before setting full on - i2c_smbus_write_byte_data(i2c_fd, ALL_LED_OFF_H, i2c_smbus_read_byte_data(i2c_fd, ALL_LED_OFF_H) & ~BIT4); + // clear existing full off bit after setting full on (off takes precedence) i2c_smbus_write_byte_data(i2c_fd, ALL_LED_ON_H, BIT4); + i2c_smbus_write_byte_data(i2c_fd, ALL_LED_OFF_H, i2c_smbus_read_byte_data(i2c_fd, ALL_LED_OFF_H) & ~BIT4); } void PCA9685::setDuty(uint8_t channel, float duty, float _delay) { - uint16_t time_on = static_cast(std::round(duty * 4096)); - uint16_t delay = static_cast(std::round(_delay * 4096)); - uint16_t time_off = delay+time_on - 1; - time_off = time_off > 4096 ? time_off - 4096 : time_off; + // uint16_t time_on = static_cast(std::round(duty * 4096)); + // uint16_t delay = static_cast(std::round(_delay * 4096)); + // uint16_t time_negate = (delay+time_on - 1) & 0xe000 ? 0 : delay+time_on - 1; + // time_negate = time_negate > 4096 ? time_negate - 4096 : time_negate; + + // counts to leading edge + uint16_t time_on = 0; + + // counts to trailing edge + uint16_t time_off = static_cast(std::round(duty * 4095)); + + // printf("%f\n%f\n", duty, _delay); + // printf("%0.2f\n%0.2f\n%04x\n%04x\n%04x\n", duty, _delay, time_on, delay, time_negate); - i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), time_on >> 8); + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), (time_on >> 8) & 0xF); i2c_smbus_write_byte_data(i2c_fd, LED0_ON_L + (channel << 2), time_on & 0xFF); - i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), time_off >> 8); + i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), (time_off >> 8) & 0xF); i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_L + (channel << 2), time_off & 0xFF); + + // printf("%02x\n%02x\n%02x\n%02x\n", i2c_smbus_read_byte_data(i2c_fd, LED0_ON_H + (channel << 2)), i2c_smbus_read_byte_data(i2c_fd, LED0_ON_L + (channel << 2)), i2c_smbus_read_byte_data(i2c_fd, LED0_OFF_H + (channel << 2)), i2c_smbus_read_byte_data(i2c_fd, LED0_OFF_L + (channel << 2))); } void PCA9685::setUS(uint8_t channel, uint16_t us, uint16_t delay_us) { @@ -126,6 +165,9 @@ void PCA9685::setUS(uint8_t channel, uint16_t us, uint16_t delay_us) { // calculate the percent duty cycles for on and delay float duty = us/max_pulse_width_us; float delay = delay_us/max_pulse_width_us; + + printf("%0.4f\n", duty); + // ensure within [0.0, 1.0] duty = duty > 1.0f ? 1.0f : duty; duty = duty < 0.0f ? 0.0f : duty; @@ -136,13 +178,13 @@ void PCA9685::setUS(uint8_t channel, uint16_t us, uint16_t delay_us) { } void PCA9685::setOn(uint8_t channel) { - // clear existing full off bit before setting full on - i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_OFF_H + (channel << 2)) & ~BIT4); + // clear existing full off bit after setting full on i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), BIT4); + i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_OFF_H + (channel << 2)) & ~BIT4); } void PCA9685::setOff(uint8_t channel) { - // clear existing full on bit before setting full off - i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_ON_H + (channel << 2)) & ~BIT4); + // clear existing full on bit after setting full off i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), BIT4); + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_ON_H + (channel << 2)) & ~BIT4); } \ No newline at end of file diff --git a/src/rov/rov_pca9685/src/ServoDriver.cpp b/src/rov/rov_pca9685/src/ServoDriver.cpp index 558f6d0..d51111a 100644 --- a/src/rov/rov_pca9685/src/ServoDriver.cpp +++ b/src/rov/rov_pca9685/src/ServoDriver.cpp @@ -3,11 +3,14 @@ #include #define NUM_COUNTS 4177 -#define BUS_DEV 7 -#define ADDRESS 0x40 +#define BUS_DEV 1 +#define ADDRESS 0x41 static int f2imap(float value, float from_min, float from_max, int to_min, int to_max){ - return ((value - from_min) * ((to_max - to_min) / (from_max - from_min)) + to_min); + int val = ((value - from_min) * ((to_max - to_min) / (from_max - from_min)) + to_min); + val = val < to_min ? to_min : val; + val = val > to_max ? to_max : val; + return val; } ServoDriver::ServoDriver() { @@ -25,12 +28,14 @@ void ServoDriver::registerServo(uint8_t channel, ServoType type) { { case ServoType::POSITIONAL: servos[channel] = Servo(); + RCLCPP_INFO(rclcpp::get_logger("ServoDriver"), "Registered positional servo on channel %d", channel); setAngle(channel, 90.0f); break; case ServoType::CONTINUOUS: continuous_servos[channel] = ContinuousServo(); - setThrottle(channel, 0.0f); + RCLCPP_INFO(rclcpp::get_logger("ServoDriver"), "Registered continuous servo on channel %d", channel); + // setThrottle(channel, 0.0f); break; } } @@ -65,9 +70,11 @@ void ServoDriver::setUSBounds(uint8_t channel, uint16_t min_us, uint16_t max_us) if (servos.count(channel)) { servos[channel].us_minimum = min_us; servos[channel].us_maximum = max_us; + return; } else if (continuous_servos.count(channel)) { continuous_servos[channel].us_minimum = min_us; continuous_servos[channel].us_maximum = max_us; + return; } RCLCPP_WARN(rclcpp::get_logger("ServoDriver"), "Attempted to set the bounds of an unregistered servo"); } @@ -75,8 +82,10 @@ void ServoDriver::setUSBounds(uint8_t channel, uint16_t min_us, uint16_t max_us) void ServoDriver::setOutput(uint8_t channel, float angle_or_throttle) { if (servos.count(channel)) { setAngle(channel, angle_or_throttle); + return; } else if (continuous_servos.count(channel)) { setThrottle(channel, angle_or_throttle); + return; } RCLCPP_WARN(rclcpp::get_logger("ServoDriver"), "Attempted to set the output of an unregistered servo"); } \ No newline at end of file diff --git a/src/rov/rov_pca9685/src/main_interactive.cpp b/src/rov/rov_pca9685/src/main_interactive.cpp index cd3693e..b43554c 100644 --- a/src/rov/rov_pca9685/src/main_interactive.cpp +++ b/src/rov/rov_pca9685/src/main_interactive.cpp @@ -104,6 +104,7 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { if(is_pwm) { std::cout << "Setting pin " << pin << " to pwm: " << pwm_or_angle << '\n'; + servoDriver.setDuty(pin, pwm_or_angle); } else { if(isContinuous[pin]) { servoDriver.setThrottle(pin, pwm_or_angle); From 23687dc2919604c29730de354659a418b71a09ac Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Tue, 25 Mar 2025 21:18:51 -0600 Subject: [PATCH 4/7] remove old debug print --- src/rov/rov_pca9685/src/PCA9685.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rov/rov_pca9685/src/PCA9685.cpp b/src/rov/rov_pca9685/src/PCA9685.cpp index 7cbcd46..a464577 100644 --- a/src/rov/rov_pca9685/src/PCA9685.cpp +++ b/src/rov/rov_pca9685/src/PCA9685.cpp @@ -31,7 +31,7 @@ PCA9685::~PCA9685() { bool PCA9685::Open() { char filename[32]; sprintf(filename,"/dev/i2c-%d", i2c_bus); - printf("%s\n", filename); + // printf("%s\n", filename); i2c_fd = open(filename, O_RDWR); if (i2c_fd < 0) { // Could not open the bus From ebebc3d27027be6003730abefef218dfca17e73a Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Wed, 26 Mar 2025 22:27:07 -0600 Subject: [PATCH 5/7] implement lucas feedback, add more detailed comments to explain why some commented code is left behind and why certain bitmasks are the way they are --- .../rov_pca9685/include/pca9685/PCA9685.hpp | 9 ++-- src/rov/rov_pca9685/src/PCA9685.cpp | 54 ++++++++----------- src/rov/rov_pca9685/src/ServoDriver.cpp | 4 +- 3 files changed, 28 insertions(+), 39 deletions(-) diff --git a/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp b/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp index 8c0b73a..78166ef 100644 --- a/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp +++ b/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp @@ -92,7 +92,7 @@ class PCA9685 { ALL_LED_OFF_L = 0xFC, ALL_LED_OFF_H = 0xFD, PRE_SCALE = 0xFE, - TestMode = 0xFF + TestMode = 0xFF // NOTE PER DATASHEET: DO NOT USE, UNDOCUMENTED BEHAVIOR }; enum RegisterMasks : uint8_t { @@ -121,13 +121,13 @@ class PCA9685 { PCA9685& operator=(PCA9685&&) = default; // attempt to connect to the pca9685 - bool Open(); + bool open(); // invalidate all pwms and close the connection with the pca9685 - void Close(); + void close(); // invalidate all pwms and restart the pca9685 - void Reset(); + void reset(); // change the output pwm frequency between 24 and 1526 Hz void setFrequency(uint16_t frequency); @@ -145,7 +145,6 @@ class PCA9685 { void setOn(uint8_t channel); void setOff(uint8_t channel); private: - void helloworld(); int i2c_fd; uint8_t i2c_address; diff --git a/src/rov/rov_pca9685/src/PCA9685.cpp b/src/rov/rov_pca9685/src/PCA9685.cpp index a464577..9ee14d4 100644 --- a/src/rov/rov_pca9685/src/PCA9685.cpp +++ b/src/rov/rov_pca9685/src/PCA9685.cpp @@ -25,14 +25,14 @@ PCA9685::PCA9685(uint8_t bus, uint8_t address) : i2c_address(address), i2c_bus(b } PCA9685::~PCA9685() { - Close(); + close(); } -bool PCA9685::Open() { +bool PCA9685::open() { char filename[32]; sprintf(filename,"/dev/i2c-%d", i2c_bus); // printf("%s\n", filename); - i2c_fd = open(filename, O_RDWR); + i2c_fd = ::open(filename, O_RDWR); if (i2c_fd < 0) { // Could not open the bus @@ -43,65 +43,54 @@ bool PCA9685::Open() { return false; } - // external clock? + // TODO: enable and test when external clock is available // i2c_smbus_write_byte_data(i2c_fd, MODE1, MODE1_SLEEP); // usleep(500); // i2c_smbus_write_byte_data(ic2_fd, MODE1, MODE1_SLEEP | MODE1_EXTCLK); usleep(500); setFrequency(60); - - // startup pattern - // for (uint8_t idx = 0; idx < 16; idx++) { - // setOn(idx); - // usleep(5000); - // setAllOff(); - // } - // for (uint8_t idx = 14; idx < 16; idx--) { - // setOn(idx); - // usleep(5000); - // setAllOff(); - // } // clear all registers - Reset(); + reset(); return true; } -void PCA9685::Close() { +void PCA9685::close() { setAllOff(); usleep(500); if (i2c_fd > 0) { - close(i2c_fd); + ::close(i2c_fd); } } -void PCA9685::Reset() { - // stop all pwms +void PCA9685::reset() { + // data sheet restart procedure per 7.3.1.1 + // stop all pwms (orderly shutdown invalidates all PWM registers) i2c_smbus_write_byte_data(i2c_fd, Registers::ALL_LED_OFF_H, BIT4); usleep(500); + // if bit 7 is 1, clear bit 4, wait for clock to stabilize uint8_t mode1_current = i2c_smbus_read_byte_data(i2c_fd, Registers::MODE1); if (mode1_current & MODE1_RESTART) { i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, mode1_current & ~MODE1_SLEEP); usleep(500); } + // write 1 to bit 7, pwm channels will restart and restart bit will clear i2c_smbus_write_byte_data(i2c_fd, Registers::MODE1, (mode1_current & ~MODE1_SLEEP) | MODE1_RESTART); } -void PCA9685::helloworld() { - -} - void PCA9685::setFrequency(uint16_t frequency) { static const uint16_t NUM_COUNTS = 4096; // scaling factor since clock sucks balls lol + static const uint16_t MIN_FREQUENCY = 24; // Hz + static const uint16_t MAX_FREQUENCY = 1526; // Hz // if given frequency is not valid, do nothing - if (frequency < 24 || frequency > 1526) { + if (frequency < MIN_FREQUENCY || MAX_FREQUENCY > 1526) { return; } output_frequency = frequency; - uint8_t prescale = static_cast(std::round(25e6/(NUM_COUNTS * frequency))) - 1; + uint8_t prescale = static_cast(std::round(clock_frequency/(NUM_COUNTS * frequency))) - 1; // printf("prescale: 0x%02x", prescale); uint8_t current_mode = i2c_smbus_read_byte_data(i2c_fd, Registers::MODE1); // set sleep bit @@ -137,6 +126,12 @@ void PCA9685::setAllOn() { } void PCA9685::setDuty(uint8_t channel, float duty, float _delay) { + /* asdf asdf + * this is the datasheet recommended way to use the 9685 + * but it breaks when the wanted duty cycle is 0, due + * due to the underflow on time_negate. haven't figured + * out a good way to handle that condition... + */ // uint16_t time_on = static_cast(std::round(duty * 4096)); // uint16_t delay = static_cast(std::round(_delay * 4096)); // uint16_t time_negate = (delay+time_on - 1) & 0xe000 ? 0 : delay+time_on - 1; @@ -147,16 +142,11 @@ void PCA9685::setDuty(uint8_t channel, float duty, float _delay) { // counts to trailing edge uint16_t time_off = static_cast(std::round(duty * 4095)); - - // printf("%f\n%f\n", duty, _delay); - // printf("%0.2f\n%0.2f\n%04x\n%04x\n%04x\n", duty, _delay, time_on, delay, time_negate); i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), (time_on >> 8) & 0xF); i2c_smbus_write_byte_data(i2c_fd, LED0_ON_L + (channel << 2), time_on & 0xFF); i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), (time_off >> 8) & 0xF); i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_L + (channel << 2), time_off & 0xFF); - - // printf("%02x\n%02x\n%02x\n%02x\n", i2c_smbus_read_byte_data(i2c_fd, LED0_ON_H + (channel << 2)), i2c_smbus_read_byte_data(i2c_fd, LED0_ON_L + (channel << 2)), i2c_smbus_read_byte_data(i2c_fd, LED0_OFF_H + (channel << 2)), i2c_smbus_read_byte_data(i2c_fd, LED0_OFF_L + (channel << 2))); } void PCA9685::setUS(uint8_t channel, uint16_t us, uint16_t delay_us) { diff --git a/src/rov/rov_pca9685/src/ServoDriver.cpp b/src/rov/rov_pca9685/src/ServoDriver.cpp index d51111a..dfa5bbe 100644 --- a/src/rov/rov_pca9685/src/ServoDriver.cpp +++ b/src/rov/rov_pca9685/src/ServoDriver.cpp @@ -15,7 +15,7 @@ static int f2imap(float value, float from_min, float from_max, int to_min, int t ServoDriver::ServoDriver() { this->driver_board = PCA9685(BUS_DEV, ADDRESS); - if(!this->driver_board.Open()) { + if(!this->driver_board.open()) { throw std::runtime_error("Failed to open PCA9685"); } this->driver_board.setAllOff(); @@ -35,7 +35,7 @@ void ServoDriver::registerServo(uint8_t channel, ServoType type) { case ServoType::CONTINUOUS: continuous_servos[channel] = ContinuousServo(); RCLCPP_INFO(rclcpp::get_logger("ServoDriver"), "Registered continuous servo on channel %d", channel); - // setThrottle(channel, 0.0f); + setThrottle(channel, 0.0f); break; } } From 9a76045776348d416054bbb549bb131b0ae65d8e Mon Sep 17 00:00:00 2001 From: Lucas Niewohner Date: Thu, 27 Mar 2025 16:20:08 -0600 Subject: [PATCH 6/7] Update the setDuty implementation to handle edge cases nicer and use the special ON/OFF cases --- src/rov/rov_pca9685/src/PCA9685.cpp | 45 ++++++++++++++++------------- 1 file changed, 25 insertions(+), 20 deletions(-) diff --git a/src/rov/rov_pca9685/src/PCA9685.cpp b/src/rov/rov_pca9685/src/PCA9685.cpp index 9ee14d4..2d08f05 100644 --- a/src/rov/rov_pca9685/src/PCA9685.cpp +++ b/src/rov/rov_pca9685/src/PCA9685.cpp @@ -126,26 +126,31 @@ void PCA9685::setAllOn() { } void PCA9685::setDuty(uint8_t channel, float duty, float _delay) { - /* asdf asdf - * this is the datasheet recommended way to use the 9685 - * but it breaks when the wanted duty cycle is 0, due - * due to the underflow on time_negate. haven't figured - * out a good way to handle that condition... - */ - // uint16_t time_on = static_cast(std::round(duty * 4096)); - // uint16_t delay = static_cast(std::round(_delay * 4096)); - // uint16_t time_negate = (delay+time_on - 1) & 0xe000 ? 0 : delay+time_on - 1; - // time_negate = time_negate > 4096 ? time_negate - 4096 : time_negate; - - // counts to leading edge - uint16_t time_on = 0; - - // counts to trailing edge - uint16_t time_off = static_cast(std::round(duty * 4095)); - - i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), (time_on >> 8) & 0xF); - i2c_smbus_write_byte_data(i2c_fd, LED0_ON_L + (channel << 2), time_on & 0xFF); - i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), (time_off >> 8) & 0xF); + // Hehe duty + + uint16_t MAX_COUNT = 4096; + uint16_t dur_on = static_cast(std::round(duty * MAX_COUNT)); + uint16_t time_on = static_cast(std::round(_delay * MAX_COUNT)); + uint16_t time_off; + if(dur_on >= MAX_COUNT){ + // Special case for fully on + time_on = (1 << 12); // Bit 12 ON means locked-on + time_off = 0; // The count doesn't matter but A) we're already sending all four on/off bytes and B) if bit 12 is on it will override the ON above + }else if (dur_on <= 0){ + // Special case for fully off (same arguments as above) + time_on = 0; + time_off = (1 << 12); // Bit 12 OFF means locked-off + }else{ + // General case (clamp delay to 12 bits to cover case of duty/delay time very close to but not quite 1, or when the user sets an invalid delay) + time_on &= 0xFFF; + time_off = time_on + dur_on; + time_off &= 0xFFF; // Modulo Again (in case the off time is in the next frame) + } + + // TODO: send the 13th bit for special cases + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), (dur_on >> 8) & 0x1F); + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_L + (channel << 2), dur_on & 0xFF); + i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), (time_off >> 8) & 0x1F); i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_L + (channel << 2), time_off & 0xFF); } From daa69b232c41efcffd37efa26e61154dea0bb42d Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Wed, 2 Apr 2025 19:20:34 -0600 Subject: [PATCH 7/7] fix lucas not knowing what the variables HE made represent --- src/rov/rov_bno085/jetgpio | 1 + src/rov/rov_bno085/sh2 | 1 + src/rov/rov_pca9685/src/PCA9685.cpp | 15 ++++++++------- 3 files changed, 10 insertions(+), 7 deletions(-) create mode 160000 src/rov/rov_bno085/jetgpio create mode 160000 src/rov/rov_bno085/sh2 diff --git a/src/rov/rov_bno085/jetgpio b/src/rov/rov_bno085/jetgpio new file mode 160000 index 0000000..51d7aca --- /dev/null +++ b/src/rov/rov_bno085/jetgpio @@ -0,0 +1 @@ +Subproject commit 51d7aca988c5d8cfbe1a66146ba1491cdd1a571d diff --git a/src/rov/rov_bno085/sh2 b/src/rov/rov_bno085/sh2 new file mode 160000 index 0000000..b514b1e --- /dev/null +++ b/src/rov/rov_bno085/sh2 @@ -0,0 +1 @@ +Subproject commit b514b1e2586ddc195e553dac89fc94c637b25298 diff --git a/src/rov/rov_pca9685/src/PCA9685.cpp b/src/rov/rov_pca9685/src/PCA9685.cpp index 2d08f05..56c03c4 100644 --- a/src/rov/rov_pca9685/src/PCA9685.cpp +++ b/src/rov/rov_pca9685/src/PCA9685.cpp @@ -127,10 +127,12 @@ void PCA9685::setAllOn() { void PCA9685::setDuty(uint8_t channel, float duty, float _delay) { // Hehe duty - - uint16_t MAX_COUNT = 4096; + static const uint16_t MAX_COUNT = 4096; uint16_t dur_on = static_cast(std::round(duty * MAX_COUNT)); - uint16_t time_on = static_cast(std::round(_delay * MAX_COUNT)); + uint16_t time_on = static_cast(std::round(_delay * MAX_COUNT)) - 1; + if (time_on > MAX_COUNT) { + time_on = 0; + } uint16_t time_off; if(dur_on >= MAX_COUNT){ // Special case for fully on @@ -147,9 +149,8 @@ void PCA9685::setDuty(uint8_t channel, float duty, float _delay) { time_off &= 0xFFF; // Modulo Again (in case the off time is in the next frame) } - // TODO: send the 13th bit for special cases - i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), (dur_on >> 8) & 0x1F); - i2c_smbus_write_byte_data(i2c_fd, LED0_ON_L + (channel << 2), dur_on & 0xFF); + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), (time_on >> 8) & 0x1F); + i2c_smbus_write_byte_data(i2c_fd, LED0_ON_L + (channel << 2), time_on & 0xFF); i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), (time_off >> 8) & 0x1F); i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_L + (channel << 2), time_off & 0xFF); } @@ -182,4 +183,4 @@ void PCA9685::setOff(uint8_t channel) { // clear existing full on bit after setting full off i2c_smbus_write_byte_data(i2c_fd, LED0_OFF_H + (channel << 2), BIT4); i2c_smbus_write_byte_data(i2c_fd, LED0_ON_H + (channel << 2), i2c_smbus_read_byte_data(i2c_fd, LED0_ON_H + (channel << 2)) & ~BIT4); -} \ No newline at end of file +}