Commit 7cff19f1 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files
parents 7d795b12 c510fd55
......@@ -91,7 +91,7 @@ catkin_package(
###########
SET(BIOLOID_FW_PATH ~/humanoids/cm510_controller_fw)
SET(ProjectPath ~/humanoids/cm510_controller_fw/examples/stairs)
SET(ProjectPath ~/humanoids/cm510_controller_fw/examples/dexter/dexter_robot/proves/stairs)
## Specify additional locations of header files
## Your package locations should be listed before other locations
......@@ -116,6 +116,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
${BIOLOID_FW_PATH}/motion/src/motion_manager.c
${BIOLOID_FW_PATH}/motion/src/action.c
${BIOLOID_FW_PATH}/motion/src/balance.c
${BIOLOID_FW_PATH}/motion/src/mtn_library.c
# dynamixel devices modules
${BIOLOID_FW_PATH}/dyn_devices/src/dyn_servos/dyn_servos.c
${BIOLOID_FW_PATH}/dyn_devices/src/exp_board/exp_board.c
......@@ -127,8 +128,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
${BIOLOID_FW_PATH}/communications/src/dynamixel.c
${BIOLOID_FW_PATH}/communications/src/serial_console.c
# main application module
${ProjectPath}/stairs.c
${ProjectPath}/../movements/mtn_library.c
${ProjectPath}/main_2.c
# AVR simulation modules
src/sim/avr_delay.c
src/sim/avr_registers.c
......
......@@ -450,7 +450,7 @@ namespace bioloid_controller_cm510
{
const double Vmax=3.3,Vmin=0.5;
// compute the range value in volts
B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-((Vmin*Vmax*msg->min_range)/Vmax));
B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-Vmin*msg->min_range);
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
......@@ -472,7 +472,7 @@ namespace bioloid_controller_cm510
{
const double Vmax=5.0,Vmin=0.5;
// compute the range value in volts
B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-((Vmin*Vmax*msg->min_range)/Vmax));
B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-Vmin*msg->min_range);
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
......@@ -499,7 +499,7 @@ namespace bioloid_controller_cm510
/* handle the feet sensors */
for(i=0;i<EXP_NUM_GPIO;i++)
{
if(msg->range<(msg->max_range/2.0))
if(msg->range>(msg->max_range/2.0))
threshold=0x01;
else
threshold=0x00;
......
<robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find bioloid_description)/urdf/bioloid.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/bioloid_low_res.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/bioloid_ceabot.gazebo" />
<xacro:include filename="$(find bioloid_description)/urdf/sensors/sharp_ir.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/sensors/feet_ir.xacro" />
<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" />
<origin xyz="-0.04 0.00 0.035" rpy="0 -1.8326 0" />
</xacro:sharp_ir>
<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.04 0.00 0.035" rpy="0 -1.3090 0" />
</xacro:sharp_ir>
<xacro:sharp_ir_long name="IRL1" parent="base_link" update_rate="20" fov="0.05" min_range="0.20" max_range="1.5">
<origin xyz="0 -0.05 0.035" rpy="-1.5707 -1.5707 0" />
</xacro:sharp_ir_long>
<!-- <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:sharp_ir name="IRLONG" parent="base_link" update_rate="20" fov="0.05" min_range="0.20" max_range="1.5">
<origin xyz="0.00 -0.05 0.035" rpy="-1.5707 -1.5707 0" />
</xacro:sharp_ir>
<!-- this is for the lateral sensors 3d component puts sensor vertical-->
<!-- this is for the lateral sensors-->
<xacro:sharp_ir name="LAT_IR_left" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0.05 0.04 0.035" rpy="-1.5707 0 0" />
<origin xyz="0.04 0.03 0.035" rpy="-1.5707 0 0" />
</xacro:sharp_ir>
<xacro:sharp_ir name="LAT_IR_right" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="-0.03 0.04 0.035" rpy="-1.5707 3.1416 0" />
<origin xyz="-0.04 0.03 0.035" rpy="-1.5707 3.1416 0" />
</xacro:sharp_ir>
......
Supports Markdown
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