<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:macro name="create_leg_joints" params="number">

        <joint name="c${number}_joint">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="f${number}_joint">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>
        <joint name="t${number}_joint">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>
    </xacro:macro>

    <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel">
        <param name="${name}_joint_i2c_address">${i2c_address}</param>
        <param name="${name}_joint_channel">${channel}</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="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="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="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="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="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"/>
        </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>
