<?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 counts_intersect counts_slope counts_min counts_max">
        <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>
        <param name="${name}_joint_counts_min">${counts_min}</param>
        <param name="${name}_joint_counts_max">${counts_max}</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" counts_intersect="386" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14" counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15" counts_intersect="189" counts_slope="129" counts_min="125" counts_max="550"/>

            <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9"  counts_intersect="327" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10" counts_intersect="317" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11" counts_intersect="227" counts_slope="129" counts_min="125" counts_max="550"/>
            
            <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2"  counts_intersect="237" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1"  counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0"  counts_intersect="267" counts_slope="129" counts_min="125" counts_max="550"/>
            
            <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13" counts_intersect="451" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14" counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15" counts_intersect="487" counts_slope="129" counts_min="125" counts_max="550"/>
            
            <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6"  counts_intersect="357" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5"  counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4"  counts_intersect="437" counts_slope="129" counts_min="125" counts_max="550"/>
            
            <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2"  counts_intersect="255" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1"  counts_intersect="335" counts_slope="129" counts_min="125" counts_max="550"/>
            <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0"  counts_intersect="444" counts_slope="129" counts_min="125" counts_max="550"/>
        </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>
