Fixes some PCA9685 bugs on pip board

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<uint8_t, ContinuousServo> continuous_servos;
 };
 
-#endif
+#endif

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

src/rov/rov_pca9685/src/ServoDriver.cpp

@@ -2,9 +2,8 @@
 #include <iostream>
 #include <rclcpp/rclcpp.hpp>
 
-#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");
-}
+}

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";
         }
     }
-}
+}