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

Added a new sensor. 

Solved a problem when reading the data from the expansion board. It always returned 0.
parent 516f885d
No related branches found
No related tags found
No related merge requests found
Pipeline #
Hide whitespace changes
Inline Side-by-side
Showing
with 94 additions and 11 deletions
bioloid_control/config/bioloid_cm510_control.yaml
+ 1
− 0
View file @ 6a497459
......@@ -10,6 +10,7 @@ bioloid:
adc3_frame: gyro_x
adc4_frame: gyro_y
exp_adc1_frame: IR2_link
exp_adc8_frame: IR3_link
exp_board_id: 192
exp_gpio0_frame: left_front_fwd_ir_link
exp_gpio1_frame: left_front_dwn_ir_link
......
bioloid_controller_cm510/CMakeLists.txt
+ 4
− 2
View file @ 6a497459
......@@ -91,7 +91,8 @@ catkin_package(
###########
SET(BIOLOID_FW_PATH ~/humanoids/cm510_controller_fw)
SET (ProjectPath ~/Desktop/Laia/new_fw)
#SET (ProjectPath ~/Desktop/new_fw)
SET (ProjectPath ~/humanoids/cm510_controller_fw/motion/src/examples)
## Specify additional locations of header files
## Your package locations should be listed before other locations
......@@ -126,7 +127,8 @@ 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}/Compass/main.c
#${ProjectPath}/Compass/main.c
${ProjectPath}/sensors.c
# AVR simulation modules
src/sim/avr_delay.c
src/sim/avr_registers.c
......
bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+ 7
− 0
View file @ 6a497459
......@@ -58,6 +58,9 @@ 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];
extern "C" {
extern void exp_board_loop(void);
}
namespace bioloid_controller_cm510
{
......@@ -320,7 +323,10 @@ namespace bioloid_controller_cm510
user_init();
manager_init(num_servos);
if(exp_board_id!=-1)
{
std::cout << "init exp board" << std::endl;
exp_board_init(exp_board_id);
}
first=false;
}
......@@ -329,6 +335,7 @@ namespace bioloid_controller_cm510
real_angles[i]=joints_[i].getPosition();
pushbuttons_loop();
adc_loop();
exp_board_loop();
exp_board_sim_loop();
buzzer_loop();
TIFR4|=(1<<OCF4A);
......
bioloid_controller_cm510/src/sim/dynamixel_master.c
+ 66
− 5
View file @ 6a497459
......@@ -124,7 +124,7 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids)
uint8_t dyn_master_ping(uint8_t id)
{
if(id!=0 && id<=num_servos)
if((id!=0 && id<=num_servos) || id==(uint8_t)exp_board_id)
return DYN_SUCCESS;
else
return DYN_COMM_ERROR;
......@@ -132,7 +132,7 @@ uint8_t dyn_master_ping(uint8_t id)
uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
{
if(id==exp_board_id)
if(id==(uint8_t)exp_board_id)
{
if(address==Hardware_available)
*data=0x03;// PWM and DAC present
......@@ -156,7 +156,7 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
else
*data=0x0000;
}
else if(id==exp_board_id)
else if(id==(uint8_t)exp_board_id)
{
if(address==ADC_CHANNEL0_L)
*data=exp_adc_values[0];
......@@ -241,8 +241,69 @@ uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_
{
unsigned int i=0;
for(i=0;i<length;i++)
data[i]=0x00;
if(id==(uint8_t)exp_board_id)
{
data[0]=exp_gpio_values[0];
data[1]=exp_gpio_values[1];
data[2]=exp_gpio_values[2];
data[3]=exp_gpio_values[3];
data[4]=exp_gpio_values[4];
data[5]=exp_gpio_values[5];
data[6]=exp_gpio_values[6];
data[7]=exp_gpio_values[7];
data[8]=exp_gpio_values[8];
data[9]=exp_gpio_values[9];
data[10]=exp_gpio_values[10];
data[11]=exp_gpio_values[11];
data[12]=exp_gpio_values[12];
data[13]=exp_gpio_values[13];
data[14]=exp_gpio_values[14];
data[15]=exp_gpio_values[15];
data[16]=exp_gpio_values[16];
data[17]=exp_gpio_values[17];
data[18]=exp_gpio_values[18];
data[19]=exp_adc_values[0]%256;
data[20]=exp_adc_values[0]/256;
data[21]=exp_adc_values[1]%256;
data[22]=exp_adc_values[1]/256;
data[23]=exp_adc_values[2]%256;
data[24]=exp_adc_values[2]/256;
data[25]=exp_adc_values[3]%256;
data[26]=exp_adc_values[3]/256;
data[27]=exp_adc_values[4]%256;
data[28]=exp_adc_values[4]/256;
data[29]=exp_adc_values[5]%256;
data[30]=exp_adc_values[5]/256;
data[31]=exp_adc_values[6]%256;
data[32]=exp_adc_values[6]/256;
data[33]=exp_adc_values[7]%256;
data[34]=exp_adc_values[7]/256;
data[35]=exp_adc_avg_values[0]%256;
data[36]=exp_adc_avg_values[0]/256;
data[37]=exp_adc_avg_values[1]%256;
data[38]=exp_adc_avg_values[1]/256;
data[39]=exp_adc_avg_values[2]%256;
data[40]=exp_adc_avg_values[2]/256;
data[41]=exp_adc_avg_values[3]%256;
data[42]=exp_adc_avg_values[3]/256;
data[43]=exp_adc_avg_values[4]%256;
data[44]=exp_adc_avg_values[4]/256;
data[45]=exp_adc_avg_values[5]%256;
data[46]=exp_adc_avg_values[5]/256;
data[47]=exp_adc_avg_values[6]%256;
data[48]=exp_adc_avg_values[6]/256;
data[49]=exp_adc_avg_values[7]%256;
data[50]=exp_adc_avg_values[7]/256;
data[51]=exp_compass_value%256;
data[52]=exp_compass_value/256;
data[53]=exp_compass_avg_value%256;
data[54]=exp_compass_avg_value/256;
}
else
{
for(i=0;i<length;i++)
data[i]=0x00;
}
return DYN_SUCCESS;
}
......
bioloid_description/config/bioloid_base.rviz
+ 13
− 4
View file @ 6a497459
......@@ -9,7 +9,7 @@ Panels:
- /RobotModel1
- /TF1
Splitter Ratio: 0.5
Tree Height: 775
Tree Height: 719
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
......@@ -64,6 +64,11 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
IR2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
......@@ -173,6 +178,8 @@ Visualization Manager:
All Enabled: true
bioloid/IR1_link:
Value: true
bioloid/IR2_link:
Value: true
bioloid/base_link:
Value: true
bioloid/left_arm_high:
......@@ -220,6 +227,8 @@ Visualization Manager:
bioloid/base_link:
bioloid/IR1_link:
{}
bioloid/IR2_link:
{}
bioloid/left_leg_pelvis_yaw:
bioloid/left_leg_pelvis_roll:
bioloid/left_leg_pelvis_pitch:
......@@ -288,10 +297,10 @@ Visualization Manager:
Window Geometry:
Displays:
collapsed: false
Height: 1056
Height: 1000
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000013c00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004e80000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd00000004000000000000013c0000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000035e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000035e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002680000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
......@@ -300,6 +309,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1855
Width: 1215
X: 65
Y: 24
bioloid_description/urdf/bioloid_ceabot.xacro
+ 3
− 0
View file @ 6a497459
......@@ -10,6 +10,9 @@
<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="IR3" 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="20" range="0.01"/>
</robot>
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