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/CMakeLists.txt b/src/rov/rov_pca9685/CMakeLists.txt index 5f1d924..ad754ad 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,32 +23,36 @@ 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_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 jhpwmpca9685) -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/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..78166ef --- /dev/null +++ b/src/rov/rov_pca9685/include/pca9685/PCA9685.hpp @@ -0,0 +1,158 @@ +#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 // NOTE PER DATASHEET: DO NOT USE, UNDOCUMENTED BEHAVIOR + }; + + 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/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 446d9a2..665ae9f 100644 --- a/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp +++ b/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp @@ -1,52 +1,49 @@ #ifndef SERVO_DRIVER_HPP #define SERVO_DRIVER_HPP -#include "JHPWMPCA9685/JHPWMPCA9685.h" +#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 new file mode 100644 index 0000000..56c03c4 --- /dev/null +++ b/src/rov/rov_pca9685/src/PCA9685.cpp @@ -0,0 +1,186 @@ +#include "pca9685/PCA9685.hpp" + +extern "C" { +#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_address(address), i2c_bus(bus) { + clock_frequency = 25e6; + // 1e6 us per s / 25e6 hz = 0.04 us + clock_us = US_PER_S / 25e6; + output_frequency = 200; +} + +PCA9685::~PCA9685() { + close(); +} + +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; + } + if (ioctl(i2c_fd, I2C_SLAVE, i2c_address) < 0) { + // Could not open the device on the bus + return false; + } + + // 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); + + // clear all registers + reset(); + + return true; +} + +void PCA9685::close() { + setAllOff(); + usleep(500); + if (i2c_fd > 0) { + ::close(i2c_fd); + } +} + +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::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 < MIN_FREQUENCY || MAX_FREQUENCY > 1526) { + return; + } + output_frequency = frequency; + 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 + 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); + + // 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) { + 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 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 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) { + // Hehe duty + 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)) - 1; + if (time_on > MAX_COUNT) { + time_on = 0; + } + 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) + } + + 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); +} + +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; + + 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; + 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 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 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); +} 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 853f790..dfa5bbe 100644 --- a/src/rov/rov_pca9685/src/ServoDriver.cpp +++ b/src/rov/rov_pca9685/src/ServoDriver.cpp @@ -1,171 +1,91 @@ #include "pca9685/ServoDriver.hpp" #include +#include #define NUM_COUNTS 4177 +#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(){ - this->driver_board = PCA9685(0x41); - if(!this->driver_board.openPCA9685()) { +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); - } -} - -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::removeServo(int channel){ - if(this->isServoChannelInUse(channel)){ - this->servos[channel].initialized = false; + this->driver_board.setAllOff(); +} + +ServoDriver::~ServoDriver() {} + +void ServoDriver::registerServo(uint8_t channel, ServoType type) { + switch (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(); + RCLCPP_INFO(rclcpp::get_logger("ServoDriver"), "Registered continuous servo on channel %d", channel); + setThrottle(channel, 0.0f); + break; } } -void ServoDriver::removeContinuousServo(int channel){ - if(this->isContinuousServoChannelInUse(channel)){ - this->servos[channel].initialized = false; - } +void ServoDriver::setDuty(uint8_t channel, float duty) { + driver_board.setDuty(channel, duty); } -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::setUS(uint8_t channel, uint16_t us) { + driver_board.setUS(channel, us); } -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::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::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::setAngle(uint8_t channel, float angle) { + if (!servos.count(channel)) { + RCLCPP_WARN(rclcpp::get_logger("ServoDriver"), "Attempted to set angle on continuous servo"); + return; } -} - -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; + Servo* s = &servos[channel]; + driver_board.setUS(channel, f2imap(angle, 0.0, 180.0, s->us_minimum, s->us_maximum)); +} + +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"); } -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::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; } -} - -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::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); - } -} - -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..b43554c 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); } } @@ -131,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);