Skip to content
Snippets Groups Projects
Commit c434fc69 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added thebioloid prefix to all simulated frames. 

Implemented the expansion board GPIO access to the simulator.
parent c528dbc7
No related branches found
No related tags found
No related merge requests found
Pipeline #
Hide whitespace changes
Inline Side-by-side
Showing
with 224 additions and 34 deletions
bioloid_control/config/bioloid_cm510_control.yaml
+ 6
− 0
View file @ c434fc69
......@@ -11,6 +11,12 @@ bioloid:
adc4_frame: gyro_y
exp_adc1_frame: IR2_link
exp_board_id: 192
exp_gpio0_frame: left_front_fwd_ir_link
exp_gpio1_frame: left_front_dwn_ir_link
exp_gpio2_frame: left_lateral_dwn_ir_link
exp_gpio3_frame: right_front_fwd_ir_link
exp_gpio4_frame: right_front_dwn_ir_link
exp_gpio5_frame: right_lateral_dwn_ir_link
joints:
- j_shoulder_r
- j_shoulder_l
......
bioloid_controller_cm510/include/bioloid_controller_cm510.h
+ 21
− 0
View file @ c434fc69
......@@ -82,6 +82,7 @@ namespace bioloid_controller_cm510
void convert_page_info(void);
void init_controller_analog_ports(ros::NodeHandle &nh);
void init_exp_board_analog_ports(ros::NodeHandle &nh);
void init_exp_board_gpio_ports(ros::NodeHandle &nh);
private:
typedef typename HardwareInterface::ResourceHandleType JointHandle;
......@@ -110,6 +111,26 @@ namespace bioloid_controller_cm510
std::string exp_adc6_frame;
std::string exp_adc7_frame;
std::string exp_adc8_frame;
/* frame associated to each of the GPIO of the expansion board */
std::string exp_gpio0_frame;
std::string exp_gpio1_frame;
std::string exp_gpio2_frame;
std::string exp_gpio3_frame;
std::string exp_gpio4_frame;
std::string exp_gpio5_frame;
std::string exp_gpio6_frame;
std::string exp_gpio7_frame;
std::string exp_gpio8_frame;
std::string exp_gpio9_frame;
std::string exp_gpio10_frame;
std::string exp_gpio11_frame;
std::string exp_gpio12_frame;
std::string exp_gpio13_frame;
std::string exp_gpio14_frame;
std::string exp_gpio15_frame;
std::string exp_gpio16_frame;
std::string exp_gpio17_frame;
std::string exp_gpio18_frame;
/* motion file handling */
CRobotisMtn motions;
std::string motions_file_name;
......
bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+ 133
− 15
View file @ c434fc69
......@@ -57,6 +57,7 @@ extern "C" {
extern int exp_board_id;
extern uint16_t exp_adc_values[EXP_NUM_ADC];
extern uint16_t exp_compass_value;
extern uint8_t exp_gpio_values[EXP_NUM_GPIO];
namespace bioloid_controller_cm510
{
......@@ -244,6 +245,7 @@ namespace bioloid_controller_cm510
/* read the configuration */
this->init_controller_analog_ports(controller_nh);
this->init_exp_board_analog_ports(controller_nh);
this->init_exp_board_gpio_ports(controller_nh);
/* read the expansion board identifier */
exp_board_id=-1;
controller_nh_.getParam("exp_board_id", exp_board_id);
......@@ -378,7 +380,15 @@ namespace bioloid_controller_cm510
unsigned int i=0;
unsigned short int value;
double B,C,voltage;
std::string frame_name;
unsigned char threshold;
if(msg->header.frame_id.find("/")==std::string::npos)
frame_name=msg->header.frame_id;
else
frame_name=msg->header.frame_id.substr(msg->header.frame_id.find("/")+1);
/* handle the controller analog ports */
for(i=0;i<NUM_ADC;i++)
{
const double Vmax=3.3,Vmin=0.5;
......@@ -387,19 +397,20 @@ namespace bioloid_controller_cm510
C=(100.0-Vmax*msg->min_range*B)/Vmax;
voltage=100.0/(B*msg->range+C);// apply the approximation of the transfer function of the sensor
value=voltage*1023.0/Vmax;// transform it to a digital value
if(msg->header.frame_id==this->adc1_frame)
if(frame_name==this->adc1_frame)
adc_values[ADC_PORT_1]=value;
else if(msg->header.frame_id==this->adc2_frame)
else if(frame_name==this->adc2_frame)
adc_values[ADC_PORT_2]=value;
else if(msg->header.frame_id==this->adc3_frame)
else if(frame_name==this->adc3_frame)
adc_values[ADC_PORT_3]=value;
else if(msg->header.frame_id==this->adc4_frame)
else if(frame_name==this->adc4_frame)
adc_values[ADC_PORT_4]=value;
else if(msg->header.frame_id==this->adc5_frame)
else if(frame_name==this->adc5_frame)
adc_values[ADC_PORT_5]=value;
else if(msg->header.frame_id==this->adc6_frame)
else if(frame_name==this->adc6_frame)
adc_values[ADC_PORT_6]=value;
}
/* handle the expansion board analog ports */
if(exp_board_id!=-1)
{
const double Vmax=5.0,Vmin=0.5;
......@@ -410,24 +421,70 @@ namespace bioloid_controller_cm510
value=voltage*1023.0/Vmax;// transform it to a digital value
for(i=0;i<EXP_NUM_ADC;i++)
{
if(msg->header.frame_id==this->exp_adc1_frame)
if(frame_name==this->exp_adc1_frame)
exp_adc_values[0]=value;
else if(msg->header.frame_id==this->exp_adc2_frame)
else if(frame_name==this->exp_adc2_frame)
exp_adc_values[1]=value;
else if(msg->header.frame_id==this->exp_adc3_frame)
else if(frame_name==this->exp_adc3_frame)
exp_adc_values[2]=value;
else if(msg->header.frame_id==this->exp_adc4_frame)
else if(frame_name==this->exp_adc4_frame)
exp_adc_values[3]=value;
else if(msg->header.frame_id==this->exp_adc5_frame)
else if(frame_name==this->exp_adc5_frame)
exp_adc_values[4]=value;
else if(msg->header.frame_id==this->exp_adc6_frame)
else if(frame_name==this->exp_adc6_frame)
exp_adc_values[5]=value;
else if(msg->header.frame_id==this->exp_adc7_frame)
else if(frame_name==this->exp_adc7_frame)
exp_adc_values[6]=value;
else if(msg->header.frame_id==this->exp_adc8_frame)
else if(frame_name==this->exp_adc8_frame)
exp_adc_values[7]=value;
}
}
/* handle the feet sensors */
for(i=0;i<EXP_NUM_GPIO;i++)
{
if(msg->range<(msg->max_range/2.0))
threshold=0x01;
else
threshold=0x00;
if(frame_name==this->exp_gpio0_frame)
exp_gpio_values[0]=threshold;
else if(frame_name==this->exp_gpio1_frame)
exp_gpio_values[1]=threshold;
else if(frame_name==this->exp_gpio2_frame)
exp_gpio_values[2]=threshold;
else if(frame_name==this->exp_gpio3_frame)
exp_gpio_values[3]=threshold;
else if(frame_name==this->exp_gpio4_frame)
exp_gpio_values[4]=threshold;
else if(frame_name==this->exp_gpio5_frame)
exp_gpio_values[5]=threshold;
else if(frame_name==this->exp_gpio6_frame)
exp_gpio_values[6]=threshold;
else if(frame_name==this->exp_gpio7_frame)
exp_gpio_values[7]=threshold;
else if(frame_name==this->exp_gpio8_frame)
exp_gpio_values[8]=threshold;
else if(frame_name==this->exp_gpio9_frame)
exp_gpio_values[9]=threshold;
else if(frame_name==this->exp_gpio10_frame)
exp_gpio_values[10]=threshold;
else if(frame_name==this->exp_gpio11_frame)
exp_gpio_values[11]=threshold;
else if(frame_name==this->exp_gpio12_frame)
exp_gpio_values[12]=threshold;
else if(frame_name==this->exp_gpio13_frame)
exp_gpio_values[13]=threshold;
else if(frame_name==this->exp_gpio14_frame)
exp_gpio_values[14]=threshold;
else if(frame_name==this->exp_gpio15_frame)
exp_gpio_values[15]=threshold;
else if(frame_name==this->exp_gpio16_frame)
exp_gpio_values[16]=threshold;
else if(frame_name==this->exp_gpio17_frame)
exp_gpio_values[17]=threshold;
else if(frame_name==this->exp_gpio18_frame)
exp_gpio_values[18]=threshold;
}
}
template <class HardwareInterface>
......@@ -584,6 +641,67 @@ namespace bioloid_controller_cm510
nh.getParam("exp_adc8_frame", this->exp_adc8_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board ADC8 associated to sensor: " << this->exp_adc8_frame);
}
template <class HardwareInterface>
void BioloidControllerCM510<HardwareInterface>::init_exp_board_gpio_ports(ros::NodeHandle &nh)
{
this->exp_gpio0_frame="none";
nh.getParam("exp_gpio0_frame", this->exp_gpio0_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO0 associated to sensor: " << this->exp_gpio0_frame);
this->exp_gpio1_frame="none";
nh.getParam("exp_gpio1_frame", this->exp_gpio1_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO1 associated to sensor: " << this->exp_gpio1_frame);
this->exp_gpio2_frame="none";
nh.getParam("exp_gpio2_frame", this->exp_gpio2_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO2 associated to sensor: " << this->exp_gpio2_frame);
this->exp_gpio3_frame="none";
nh.getParam("exp_gpio3_frame", this->exp_gpio3_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO3 associated to sensor: " << this->exp_gpio3_frame);
this->exp_gpio4_frame="none";
nh.getParam("exp_gpio4_frame", this->exp_gpio4_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO4 associated to sensor: " << this->exp_gpio4_frame);
this->exp_gpio5_frame="none";
nh.getParam("exp_gpio5_frame", this->exp_gpio5_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO5 associated to sensor: " << this->exp_gpio5_frame);
this->exp_gpio6_frame="none";
nh.getParam("exp_gpio6_frame", this->exp_gpio6_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO6 associated to sensor: " << this->exp_gpio6_frame);
this->exp_gpio7_frame="none";
nh.getParam("exp_gpio7_frame", this->exp_gpio7_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO7 associated to sensor: " << this->exp_gpio7_frame);
this->exp_gpio8_frame="none";
nh.getParam("exp_gpio8_frame", this->exp_gpio8_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO8 associated to sensor: " << this->exp_gpio8_frame);
this->exp_gpio9_frame="none";
nh.getParam("exp_gpio9_frame", this->exp_gpio9_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO9 associated to sensor: " << this->exp_gpio9_frame);
this->exp_gpio10_frame="none";
nh.getParam("exp_gpio10_frame", this->exp_gpio10_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO10 associated to sensor: " << this->exp_gpio10_frame);
this->exp_gpio11_frame="none";
nh.getParam("exp_gpio11_frame", this->exp_gpio11_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO11 associated to sensor: " << this->exp_gpio11_frame);
this->exp_gpio12_frame="none";
nh.getParam("exp_gpio12_frame", this->exp_gpio12_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO12 associated to sensor: " << this->exp_gpio12_frame);
this->exp_gpio13_frame="none";
nh.getParam("exp_gpio13_frame", this->exp_gpio13_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO13 associated to sensor: " << this->exp_gpio13_frame);
this->exp_gpio14_frame="none";
nh.getParam("exp_gpio14_frame", this->exp_gpio14_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO14 associated to sensor: " << this->exp_gpio14_frame);
this->exp_gpio15_frame="none";
nh.getParam("exp_gpio15_frame", this->exp_gpio15_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO15 associated to sensor: " << this->exp_gpio15_frame);
this->exp_gpio16_frame="none";
nh.getParam("exp_gpio16_frame", this->exp_gpio16_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO16 associated to sensor: " << this->exp_gpio16_frame);
this->exp_gpio17_frame="none";
nh.getParam("exp_gpio17_frame", this->exp_gpio17_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO17 associated to sensor: " << this->exp_gpio17_frame);
this->exp_gpio18_frame="none";
nh.getParam("exp_gpio18_frame", this->exp_gpio18_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO18 associated to sensor: " << this->exp_gpio18_frame);
}
} // namespace
#endif // header guard
bioloid_controller_cm510/include/sim/exp_board_sim.h
+ 1
− 0
View file @ c434fc69
......@@ -8,6 +8,7 @@ extern "C" {
#include <avr/io.h>
#define EXP_NUM_ADC 8
#define EXP_NUM_GPIO 19
void exp_board_sim_init(void);
void exp_board_sim_loop(void);
......
bioloid_controller_cm510/src/sim/adc.c
+ 3
− 3
View file @ c434fc69
......@@ -58,10 +58,10 @@ void init_adc(void)
for(i=0;i<NUM_ADC;i++)
{
adc_values[i]=512;
adc_avg_values[i]=512;
adc_values[i]=256;
adc_avg_values[i]=256;
for(j=0;j<ADC_MAX_NUM_SAMPLES;j++)
adc_ch_data[i][j]=512;
adc_ch_data[i][j]=256;
}
adc_current_sample=0;
}
......
bioloid_controller_cm510/src/sim/dynamixel_master.c
+ 39
− 0
View file @ c434fc69
......@@ -14,6 +14,7 @@ extern uint16_t exp_adc_values[EXP_NUM_ADC];
extern uint16_t exp_adc_avg_values[EXP_NUM_ADC];
extern uint16_t exp_compass_value;
extern uint16_t exp_compass_avg_value;
extern uint8_t exp_gpio_values[EXP_NUM_GPIO];
/* private variables */
uint8_t dyn_master_tx_buffer[DYN_MASTER_MAX_TX_BUFFER_LEN];
......@@ -193,6 +194,44 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
*data=exp_compass_value;
else if(address==COMPASS_AVG_HEADING_VAL_L)
*data=exp_compass_avg_value;
else if(address==GPIO0_data)
*data=exp_gpio_values[0];
else if(address==GPIO1_data)
*data=exp_gpio_values[1];
else if(address==GPIO2_data)
*data=exp_gpio_values[2];
else if(address==GPIO3_data)
*data=exp_gpio_values[3];
else if(address==GPIO4_data)
*data=exp_gpio_values[4];
else if(address==GPIO5_data)
*data=exp_gpio_values[5];
else if(address==GPIO6_data)
*data=exp_gpio_values[6];
else if(address==GPIO7_data)
*data=exp_gpio_values[7];
else if(address==GPIO8_data)
*data=exp_gpio_values[8];
else if(address==GPIO9_data)
*data=exp_gpio_values[9];
else if(address==GPIO10_data)
*data=exp_gpio_values[10];
else if(address==GPIO11_data)
*data=exp_gpio_values[11];
else if(address==GPIO12_data)
*data=exp_gpio_values[12];
else if(address==GPIO13_data)
*data=exp_gpio_values[13];
else if(address==GPIO14_data)
*data=exp_gpio_values[14];
else if(address==GPIO15_data)
*data=exp_gpio_values[15];
else if(address==GPIO16_data)
*data=exp_gpio_values[16];
else if(address==GPIO17_data)
*data=exp_gpio_values[17];
else if(address==GPIO18_data)
*data=exp_gpio_values[18];
}
return DYN_SUCCESS;
......
bioloid_controller_cm510/src/sim/exp_board_sim.c
+ 5
− 0
View file @ c434fc69
......@@ -13,6 +13,8 @@ uint16_t exp_compass_value;
uint16_t exp_compass_avg_value;
uint8_t exp_compass_current_sample;
uint8_t exp_gpio_values[EXP_NUM_GPIO];
void exp_board_compass_avrg(uint16_t heading)
{
static unsigned long int new_average,first_value;
......@@ -62,6 +64,9 @@ void exp_board_sim_init(void)
exp_compass_value=0;
exp_compass_avg_value=0;
exp_compass_current_sample=0;
/* initialize the expansion board gpio data */
for(i=0;i<EXP_NUM_GPIO;i++)
exp_gpio_values[i]=0x00;
}
void exp_board_sim_loop(void)
......
bioloid_description/urdf/bioloid_ceabot.xacro
+ 3
− 3
View file @ c434fc69
......@@ -4,12 +4,12 @@
<xacro:include filename="$(find bioloid_description)/urdf/sharp_ir.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/feet_ir.xacro" />
<xacro:sharp_ir name="IR1" parent="base_link" update_rate="100" fov="0.05" min_range="0.1" max_range="0.8">
<xacro:sharp_ir name="IR1" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0 0.0 0.035" rpy="-1.5707 -1.5707 0" />
</xacro:sharp_ir>
<xacro:sharp_ir name="IR2" parent="base_link" update_rate="100" fov="0.05" min_range="0.1" max_range="0.8">
<xacro:sharp_ir name="IR2" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0 -0.05 0.035" rpy="-1.5707 -1.5707 0" />
</xacro:sharp_ir>
<xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="100" range="0.005"/>
<xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="20" range="0.01"/>
</robot>
bioloid_description/urdf/cny70_ir.gazebo
+ 3
− 3
View file @ c434fc69
......@@ -19,7 +19,7 @@
<sensor type="ray" name="${name}">
<always_on>true</always_on>
<update_rate>${update_rate}</update_rate>
<visualize>false</visualize>
<visualize>true</visualize>
<ray>
<scan>
<horizontal>
......@@ -42,11 +42,11 @@
</range>
</ray>
<plugin name="${name}_range_sensor" filename="libgazebo_ros_range.so">
<frameName>${name}_ir_link</frameName>
<frameName>/bioloid/${name}_ir_link</frameName>
<topicName>/bioloid/sensors/range</topicName>
<radiation>infrared</radiation>
<fov>${fov}</fov>
<gaussianNoise>0.01</gaussianNoise>
<gaussianNoise>0.0005</gaussianNoise>
<updateRate>${update_rate}</updateRate>
</plugin>
</sensor>
......
bioloid_description/urdf/cny70_ir.xacro
+ 1
− 1
View file @ c434fc69
......@@ -21,7 +21,7 @@
<material name="black"/>
</visual>
<inertial>
<mass value="0.0007" />
<mass value="0.0007"/>
<origin xyz="-0.00307736 0.0000000 0.00000000" rpy="0 0 0"/>
<inertia ixx="0.000000001" ixy="0.00000000" ixz="0.000000" iyy="0.000000001" iyz="0.00000000" izz="0.000000001" />
</inertial>
......
bioloid_description/urdf/feet_ir.xacro
+ 8
− 8
View file @ c434fc69
......@@ -34,15 +34,15 @@
</joint>
<!-- sensor links -->
<xacro:cny70_ir name="left_front_fwd" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
<xacro:cny70_ir name="left_front_fwd" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
</xacro:cny70_ir>
<xacro:cny70_ir name="left_front_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
<origin xyz="0.0225 -0.012 0.038" rpy="0 0 -1.5707" />
<xacro:cny70_ir name="left_front_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="0.0225 -0.012 0.0345" rpy="0 0 -1.5707" />
</xacro:cny70_ir>
<xacro:cny70_ir name="left_lateral_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
<xacro:cny70_ir name="left_lateral_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
</xacro:cny70_ir>
......@@ -74,15 +74,15 @@
</joint>
<!-- sensor links -->
<xacro:cny70_ir name="right_front_fwd" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
<xacro:cny70_ir name="right_front_fwd" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="-0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
</xacro:cny70_ir>
<xacro:cny70_ir name="right_front_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
<origin xyz="-0.0225 -0.012 0.038" rpy="0 0 -1.5707" />
<xacro:cny70_ir name="right_front_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="-0.0225 -0.012 0.0345" rpy="0 0 -1.5707" />
</xacro:cny70_ir>
<xacro:cny70_ir name="right_lateral_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
<xacro:cny70_ir name="right_lateral_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="-0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
</xacro:cny70_ir>
......
bioloid_description/urdf/sharp_ir.gazebo
+ 1
− 1
View file @ c434fc69
......@@ -42,7 +42,7 @@
</range>
</ray>
<plugin name="${name}_range_sensor" filename="libgazebo_ros_range.so">
<frameName>${name}_link</frameName>
<frameName>/bioloid/${name}_link</frameName>
<topicName>/bioloid/sensors/range</topicName>
<radiation>infrared</radiation>
<fov>${fov}</fov>
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment