HWI working, updated xacro files with extra attributes for PWM calibration

CMakeLists.txt

@@ -32,6 +32,8 @@ ament_target_dependencies(
     rclcpp_lifecycle
 )
 
+target_link_libraries(hexapod_robot PUBLIC i2c)
+
 pluginlib_export_plugin_description_file(hardware_interface hexapod_robot.xml)
 
 
@@ -75,7 +77,9 @@ ament_export_dependencies(
     hardware_interface 
     pluginlib
     rclcpp
-    rclcpp_lifecycle
+    rcl
+    cpp_lifecycle
+    i2c
 )
 
 

bringup/config/controllers.yaml

@@ -1,32 +1,58 @@
 controller_manager:
     ros__parameters:
-        update_rate: 10
+        update_rate: 50
         
         joint_state_broadcaster:
             type: joint_state_broadcaster/JointStateBroadcaster
-            
+
+        joint_trajectory_controller:
+            type: joint_trajectory_controller/JointTrajectoryController
+
         forward_position_controller:
             type: forward_command_controller/ForwardCommandController
+
+
             
 forward_position_controller:
+    ros__parameters:
+        joints:
+            - t3_joint
+        interface_name: position
+
+joint_trajectory_controller:
     ros__parameters:
         joints:
             - c1_joint
-            - c2_joint
-            - c3_joint
-            - c4_joint
-            - c5_joint
-            - c6_joint
             - f1_joint
-            - f2_joint
-            - f3_joint
-            - f4_joint
-            - f5_joint
-            - f6_joint
             - t1_joint
+
+            - c2_joint
+            - f2_joint
             - t2_joint
+
+            - c3_joint
+            - f3_joint
             - t3_joint
+
+            - c4_joint
+            - f4_joint
             - t4_joint
+
+            - c5_joint
+            - f5_joint
             - t5_joint
+
+            - c6_joint
+            - f6_joint
             - t6_joint
-        interface_name: position
+
+        command_interfaces:
+            - position
+
+        state_interfaces:
+            - position
+
+        action_monitor_rate: 20.0
+
+        allow_partial_joints_goal: false
+

bringup/launch/hexapod_robot.launch.py

@@ -25,13 +25,6 @@ def generate_launch_description():
     robot_description = {"robot_description": robot_description_content}
 
     robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "controllers.yaml"])
-    
-    control_node = Node(
-        package = "controller_manager",
-        executable = "ros2_control_node",
-        parameters = [robot_description, robot_controllers],
-        output="both"
-    )
 
     robot_state_publisher_node = Node(
         package = "robot_state_publisher",
@@ -40,51 +33,69 @@ def generate_launch_description():
         output = "both"
     )            
 
-    joint_state_publisher_gui = Node(
-        package="joint_state_publisher_gui",
-        executable="joint_state_publisher_gui"
-    )
-            
-    rviz_node = Node(
-        package = "rviz2",
-        executable = "rviz2",
-        arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])],
-        name = "rviz2",
-        output = "screen",
+    control_node = Node(
+        package = "controller_manager",
+        executable = "ros2_control_node",
+        parameters = [robot_description, robot_controllers],
+        output="both"
     )
 
+    # rviz_node = Node(
+    #     package = "rviz2",
+    #     executable = "rviz2",
+    #     arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])],
+    #     name = "rviz2",
+    #     output = "screen",
+    # )
+
     joint_state_broadcaster_spawner = Node (
         package = "controller_manager",
         executable = "spawner",
         arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"]
     )
     
-    robot_controller_spawner = Node(
+    # forward_position_controller_spawner = Node(
+    #     package = "controller_manager",
+    #     executable = "spawner",
+    #     arguments = ["forward_position_controller", "--controller-manager", "/controller_manager"]
+    # )
+
+    joint_trajectory_controller_spawner = Node(
         package = "controller_manager",
         executable = "spawner",
-        arguments = ["forward_position_controller", "--controller-manager", "/controller_manager"]
-    )
-    
-    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
-        event_handler = OnProcessExit(
-            target_action = joint_state_broadcaster_spawner,
-            on_exit = [rviz_node]
-        )
+        arguments = ["joint_trajectory_controller", "--controller-manager", "/controller_manager"]
     )
+
+
+    # delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+    #     event_handler = OnProcessExit(
+    #         target_action = joint_state_broadcaster_spawner,
+    #         on_exit = [rviz_node]
+    #     )
+    # )
     
-    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+    # delay_forward_position_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+    #     event_handler = OnProcessExit(
+    #         target_action = joint_state_broadcaster_spawner,
+    #         on_exit = [forward_position_controller_spawner]
+    #     )
+    # )
+
+    delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
         event_handler = OnProcessExit(
             target_action = joint_state_broadcaster_spawner,
-            on_exit = [robot_controller_spawner]
+            on_exit = [joint_trajectory_controller_spawner]
         )
     )
-    
+
+
     nodes = [
         control_node,
         robot_state_publisher_node,
         joint_state_broadcaster_spawner,
-        delay_rviz_after_joint_state_broadcaster_spawner,
-        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner        
+        #delay_rviz_after_joint_state_broadcaster_spawner,
+        # delay_forward_position_controller_spawner_after_joint_state_broadcaster_spawner,
+        delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner
     ]
             
     return LaunchDescription(nodes)

description/urdf/hexapod_robot_macros.xacro

@@ -55,11 +55,12 @@
                 <material name="blue"/>
             </visual>                       
         </link>
-        <joint name="${name}_joint" type="continuous">
+        <joint name="${name}_joint" type="revolute">
             <parent link="base_link"/>
             <child  link="${name}_link"/>
             <origin xyz="${xy_offset} 0"/>
             <axis   xyz="0 0 1"/>
+            <limit effort="30" velocity="100" lower="-1" upper="1"/>
         </joint>
     </xacro:macro>
 
@@ -84,12 +85,13 @@
             </visual>
 -->           
         </link>
-        <joint name="${name}_joint" type="continuous">
+        <joint name="${name}_joint" type="revolute">
             <parent link="${parent_name}_link"/>
             <child  link="${name}_link"/>
             <xacro:if value="${side == 'l'}"><origin xyz="-0.020 0.03 -0.0075"/></xacro:if>
             <xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0.03 -0.0075"/></xacro:if>
             <axis xyz="0 1 0"/>
+            <limit effort="30" velocity="100" lower="-1" upper="1"/>
         </joint>
     </xacro:macro>
 
@@ -141,12 +143,13 @@
             </visual>                       
 
         </link>
-        <joint name="${name}_joint" type="continuous">
+        <joint name="${name}_joint" type="revolute">
             <parent link="${parent_name}_link"/>
             <child  link="${name}_link"/>
             <xacro:if value="${side == 'l'}"><origin xyz="-0.080 -0.03 0"/></xacro:if>
             <xacro:if value="${side == 'r'}"><origin xyz=" 0.080 -0.03 0"/></xacro:if>
             <axis xyz="0 1 0"/>
+            <limit effort="30" velocity="100" lower="-1" upper="1"/>
         </joint>
     </xacro:macro>
 

description/urdf/hexapod_robot_ros2_control.xacro

@@ -27,44 +27,49 @@
         </joint>
     </xacro:macro>
 
-    <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel">
+    <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel counts_intersect counts_slope">
         <param name="${name}_joint_i2c_address">${i2c_address}</param>
         <param name="${name}_joint_channel">${channel}</param>
+        <param name="${name}_joint_counts_intersect">${counts_intersect}</param>
+        <param name="${name}_joint_counts_slope">${counts_slope}</param>
     </xacro:macro>
 
     <ros2_control name="Hexapod" type="system">
+        
         <hardware>
             <plugin>hexapod_robot/HexapodRobotHardwareInterface</plugin>
             <param name="i2c_device">/dev/i2c-1</param>
-            <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13"/>
-            <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14"/>
-            <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15"/>
+            <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13" counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14" counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15" counts_intersect="307" counts_slope="129"/>
 
-            <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9"/>
-            <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10"/>
-            <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11"/>
+            <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9"  counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10" counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11" counts_intersect="307" counts_slope="129"/>
 
-            <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2"/>
-            <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1"/>
-            <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0"/>
+            <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2"  counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1"  counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0"  counts_intersect="307" counts_slope="129"/>
 
-            <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13"/>
-            <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14"/>
-            <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15"/>
+            <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13" counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14" counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15" counts_intersect="307" counts_slope="129"/>
 
-            <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6"/>
-            <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5"/>
-            <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4"/>
+            <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6"  counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5"  counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4"  counts_intersect="307" counts_slope="129"/>
 
-            <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2"/>
-            <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1"/>
-            <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0"/>
+            <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2"  counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1"  counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0"  counts_intersect="307" counts_slope="129"/>
         </hardware>
+
         <xacro:create_leg_joints number="1"/>    
         <xacro:create_leg_joints number="2"/>    
         <xacro:create_leg_joints number="3"/>    
         <xacro:create_leg_joints number="4"/>    
         <xacro:create_leg_joints number="5"/>    
         <xacro:create_leg_joints number="6"/>    
+
     </ros2_control>
 </robot>

hardware/hexapod_robot_hwi.cpp

@@ -4,6 +4,18 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <stdint.h>
+
+#include <sys/ioctl.h>
+#include <linux/i2c-dev.h>
+
+extern "C" {
+    #include <i2c/smbus.h>
+}
 
 namespace hexapod_robot {
 
@@ -12,7 +24,7 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
     if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
         return hardware_interface::CallbackReturn::ERROR;
     }
-    
+
     RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_init");
     
     for (const hardware_interface::ComponentInfo &joint: info_.joints) {
@@ -21,30 +33,106 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
 
         std::string i2c_address_parameter = joint.name + "_i2c_address";
         std::string channel_parameter = joint.name + "_channel";
+        std::string counts_intersect_parameter = joint.name + "_counts_intersect";
+        std::string counts_slope_parameter = joint.name + "_counts_slope";
+
 
         PWMOutput pwm_output;
-        pwm_output.i2c_address = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);
-        pwm_output.channel     = stoi(info_.hardware_parameters[channel_parameter], nullptr, 0);
+        pwm_output.i2c_address      = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);
+        pwm_output.channel          = stoi(info_.hardware_parameters[channel_parameter], nullptr, 0);
+        pwm_output.counts_intersect = stod(info_.hardware_parameters[counts_intersect_parameter]);
+        pwm_output.counts_slope     = stod(info_.hardware_parameters[counts_slope_parameter]);
 
-        pca9685_controller_available_[pwm_output.i2c_address] = false;
+        pca_available_[pwm_output.i2c_address] = false;
         joint_pwm_mapping_[joint.name] = pwm_output;
 
-        RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "%s mapped to 0x%02x:%d", joint.name.c_str(), pwm_output.i2c_address, pwm_output.channel);
+        RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "%s mapped to 0x%02x:%d. intersect: %.4f slope: %.4f", joint.name.c_str(), pwm_output.i2c_address, pwm_output.channel, pwm_output.counts_intersect, pwm_output.counts_slope);
     }
 
-    for (const auto& [i2c_address, available]: pca9685_controller_available_) {
-        RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "require PCA9685 at i2c 0x%02x.", i2c_address);
-    }
 
-    i2c_available_ = false;
-    i2c_fd_ = open(info_.hardware_parameters["i2c_device"].c_str(), O_RDWR);
+    i2c_available_ = i2c_open_(info_.hardware_parameters["i2c_device"]);
+    pca_initialize_();
+
+    return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+bool
+HexapodRobotHardwareInterface::i2c_open_(const std::string& device) {
+    i2c_fd_ = open(device.c_str(), O_RDWR);
     if (i2c_fd_ < 0) {
         RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Cannot open %s, continuing in dummy mode.", info_.hardware_parameters["i2c_device"].c_str());
-    } else {
-        i2c_available_ = true;
+        return false;
     }
+    return true;
+}
 
-    return hardware_interface::CallbackReturn::SUCCESS;
+void
+HexapodRobotHardwareInterface::pca_initialize_() {
+    for (const auto& [i2c_address, available]: pca_available_) {        
+
+        ioctl(i2c_fd_, I2C_SLAVE, i2c_address);
+
+        if(i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_, 0x00) < 0) {
+            RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Could not open i2c device %02x, marking as unavailable.", i2c_address);
+            pca_available_[i2c_address] = false;
+        } else {
+            i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_sleep_); 
+            i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode2_,    pca_mode2_outdrv_);
+            i2c_smbus_write_byte_data(i2c_fd_, pca_reg_prescale_, (uint8_t)pca_prescaler_value_);
+            i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_restart_);
+
+            RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "PCA9685 at i2c 0x%02x initialized.", i2c_address);
+            pca_available_[i2c_address] = true;
+        }
+    }
+}
+
+bool
+HexapodRobotHardwareInterface::pca_set_pwm_(uint8_t i2c_address, uint8_t channel_number, uint16_t counts) {
+    // if(i2c_address == 0x40 && channel_number == 0) {
+    //    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm(%02x, %02x, %04x)", i2c_address, channel_number, counts);
+    // }
+    if(channel_number>15) {
+        RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: channel %02x >15", channel_number);
+        return false;
+    }
+    if(counts > 0x1000) {
+        RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: counts %04x >15", counts);
+        return false;
+    }
+    if(!pca_available_[i2c_address]) {
+    //    RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: i2c_address %02x unavailable.", i2c_address);
+        return false;
+    }
+
+    if(ioctl(i2c_fd_, I2C_SLAVE, i2c_address) < 0) {
+        RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: ioctl failed.");
+        return false;
+    }
+
+    // adding channel_number * 4 to the base address for LED0 output given by the constants
+    // yields the addresses of all PWM channels
+
+    if(counts == 0) {
+        // output always off
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_  + channel_number * 4, 0x00); // ON_L
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_  + channel_number * 4, 0x00); // ON_H
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x10); // OFF_H == 0x10 => LED full OFF 
+    } else if(counts == 0x1000) {
+        // output always on
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_  + channel_number * 4, 0x00); // ON_L
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_  + channel_number * 4, 0x10); // ON_H  == 0x10 => LED full ON 
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x00); // OFF_H
+    } else {
+        // duty cycle from 0 to counts
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_  + channel_number * 4, 0x00); 
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_  + channel_number * 4, 0x00); 
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, counts & 0xff); // lower byte
+        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, counts >>   8); // shift upper byte into lower byte
+    }
+    return true;
 }
 
 hardware_interface::CallbackReturn
@@ -64,7 +152,7 @@ std::vector<hardware_interface::StateInterface>
 HexapodRobotHardwareInterface::export_state_interfaces() {
     std::vector<hardware_interface::StateInterface> state_interfaces;
     for (const hardware_interface::ComponentInfo &joint: info_.joints) {
-        state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_states_[joint.name]));
+        state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name]));
     }
     return state_interfaces;
 }
@@ -81,14 +169,16 @@ HexapodRobotHardwareInterface::export_command_interfaces() {
 
 hardware_interface::return_type
 HexapodRobotHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) {
-    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
-        joint_states_[joint.name]=joint_commands_[joint.name];
-    }
     return hardware_interface::return_type::OK;
 }
 
 hardware_interface::return_type
 HexapodRobotHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) {
+    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
+        const PWMOutput &pwm_output = joint_pwm_mapping_[joint.name];        
+        const double pwm_value = pwm_output.counts_intersect + joint_commands_[joint.name] * pwm_output.counts_slope;
+        pca_set_pwm_(pwm_output.i2c_address, pwm_output.channel, (uint16_t)pwm_value);
+    }    
     return hardware_interface::return_type::OK;
 }
 

hardware/include/hexapod_robot/hexapod_robot_hwi.hpp

@@ -20,6 +20,8 @@ namespace hexapod_robot {
 struct PWMOutput {
     uint8_t i2c_address;
     uint8_t channel;
+    double counts_intersect;
+    double counts_slope;
 };
 
 class HexapodRobotHardwareInterface : public hardware_interface::SystemInterface {
@@ -39,15 +41,44 @@ public:
     hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 private:
-    int i2c_fd_;
+    std::map<std::string, double> joint_commands_;
+
+    std::map<std::string, PWMOutput> joint_pwm_mapping_;
+
     bool i2c_available_;
+    int i2c_fd_;
 
-    std::map<uint8_t, bool> pca9685_controller_available_;
+    std::map<uint8_t, bool> pca_available_;
 
-    std::map<std::string, double> joint_states_;
-    std::map<std::string, double> joint_commands_;
+    bool i2c_open_(const std::string& device);
 
-    std::map<std::string, PWMOutput> joint_pwm_mapping_;
+    const uint8_t pca_reg_mode1_        = 0x00;
+    const uint8_t pca_reg_mode2_        = 0x01;
+    const uint8_t pca_reg_prescale_     = 0xfe;
+
+    // base address for LED0 following, 
+    // other channels can be calculated by adding 
+    //     channel_number * 4
+    // to each base address
+
+    const uint8_t pca_reg_led_on_l_     = 0x06;
+    const uint8_t pca_reg_led_on_h_     = 0x07;
+    const uint8_t pca_reg_led_off_l_    = 0x08;
+    const uint8_t pca_reg_led_off_h_    = 0x09;
+
+    const uint8_t pca_mode1_sleep_      = 0x10;
+    const uint8_t pca_mode1_restart_    = 0x80;
+    const uint8_t pca_mode2_outdrv_     = 0x04;
+
+
+    const double pca_osc_frequency_     = 25000000; // 25 Mhz
+    const double pca_pwm_frequency_     = 50;       // 50 Hz
+    const double pca_prescaler_value_   = (pca_osc_frequency_ / (4096.0 * pca_pwm_frequency_) - 0.5); // according to data sheet
+    const double pca_pwm_cycle_usec_    = 1000000 / pca_pwm_frequency_; // microseconds per PWM counter cycle
+    const double pca_pwm_count_usec_    = pca_pwm_cycle_usec_ / 4096;   // microseconds per PWM single count
+
+    void pca_initialize_();
+    bool pca_set_pwm_(uint8_t i2c_address, uint8_t channel_number, uint16_t counts);
 
 };