diff --git a/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp b/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp index 665ae9f..c78d4dd 100644 --- a/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp +++ b/src/rov/rov_pca9685/include/pca9685/ServoDriver.hpp @@ -15,13 +15,13 @@ struct ServoBase { }; struct Servo : ServoBase { - uint16_t us_minimum; - uint16_t us_maximum; + uint16_t us_minimum = 1000; + uint16_t us_maximum = 2000; }; struct ContinuousServo : ServoBase { - uint16_t us_minimum; - uint16_t us_maximum; + uint16_t us_minimum = 1000; + uint16_t us_maximum = 2000; }; class ServoDriver { @@ -46,4 +46,4 @@ class ServoDriver { std::map continuous_servos; }; -#endif \ No newline at end of file +#endif diff --git a/src/rov/rov_pca9685/src/PCA9685.cpp b/src/rov/rov_pca9685/src/PCA9685.cpp index 56c03c4..c387950 100644 --- a/src/rov/rov_pca9685/src/PCA9685.cpp +++ b/src/rov/rov_pca9685/src/PCA9685.cpp @@ -82,7 +82,7 @@ void PCA9685::reset() { } void PCA9685::setFrequency(uint16_t frequency) { - static const uint16_t NUM_COUNTS = 4096; // scaling factor since clock sucks balls lol + static const uint16_t NUM_COUNTS = 3932; // 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 diff --git a/src/rov/rov_pca9685/src/ServoDriver.cpp b/src/rov/rov_pca9685/src/ServoDriver.cpp index dfa5bbe..241a4dd 100644 --- a/src/rov/rov_pca9685/src/ServoDriver.cpp +++ b/src/rov/rov_pca9685/src/ServoDriver.cpp @@ -2,9 +2,8 @@ #include #include -#define NUM_COUNTS 4177 #define BUS_DEV 1 -#define ADDRESS 0x41 +#define ADDRESS 0x5e static int f2imap(float value, float from_min, float from_max, int to_min, int to_max){ int val = ((value - from_min) * ((to_max - to_min) / (from_max - from_min)) + to_min); @@ -29,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); + setUSBounds(channel, 1000, 2000); 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); + setUSBounds(channel, 1000, 2000); setThrottle(channel, 0.0f); break; } @@ -57,6 +58,23 @@ void ServoDriver::setThrottle(uint8_t channel, float throttle) { driver_board.setUS(channel, f2imap(throttle, -1.0, 1.0, s->us_minimum, s->us_maximum)); } +void ServoDriver::setDuty(uint8_t channel, float duty) { + driver_board.setDuty(channel, duty); +} + +void ServoDriver::setUS(uint8_t channel, uint16_t us) { + driver_board.setUS(channel, us); +} + +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::setAngle(uint8_t channel, float angle) { if (!servos.count(channel)) { RCLCPP_WARN(rclcpp::get_logger("ServoDriver"), "Attempted to set angle on continuous servo"); @@ -88,4 +106,4 @@ void ServoDriver::setOutput(uint8_t channel, float 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 b43554c..d1c8c09 100644 --- a/src/rov/rov_pca9685/src/main_interactive.cpp +++ b/src/rov/rov_pca9685/src/main_interactive.cpp @@ -24,7 +24,7 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { for(int i=0; i<16; i++) { servoDriver.registerServo(i, ServoType::CONTINUOUS); isContinuous[i]=true; - servoDriver.setUSBounds(i, 1100, 1900); + servoDriver.setUSBounds(i, 1000, 2000); } } else { // load non continuous servo defaults @@ -119,4 +119,4 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { std::cout << "substr threw out of range exception\n"; } } -} \ No newline at end of file +} diff --git a/worlds_driverstation_setup.bash b/worlds_driverstation_setup.bash deleted file mode 100755 index 776d639..0000000 --- a/worlds_driverstation_setup.bash +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash -gnome-terminal -e "bash -c 'ssh robotics@10.0.0.2; $SHELL'" -sleep 5 -gnome-terminal --geometry 104x8-0--500 -e "bash -c 'source install/setup.bash && ros2 run joy joy_node; $SHELL'" -gnome-terminal --geometry 20x20--500+0 -e "bash -c 'source install/setup.bash && ros2 topic echo bno055; $SHELL'" -gnome-terminal --geometry 20x20--500+450 -e "bash -c 'source install/setup.bash && ros2 topic echo bar02; $SHELL'" -gnome-terminal -e "bash -c 'source install/setup.bash && ros2 run rqt_image_view rqt_image_view; $SHELL'" -gnome-terminal -e "bash -c 'source install/setup.bash && ros2 run rqt_image_view rqt_image_view; $SHELL'" \ No newline at end of file