Skip to content

Commit d183a43

Browse files
author
blukaz
committed
v0.25b tested R/W dxl opencm
1 parent 109c89d commit d183a43

File tree

5 files changed

+317
-26
lines changed

5 files changed

+317
-26
lines changed

‎ker_imu_read/CMakeLists.txt

+188
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,188 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(ker_imu_read)
3+
4+
## Find catkin macros and libraries
5+
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
6+
## is used, also find other catkin packages
7+
find_package(catkin REQUIRED COMPONENTS
8+
roscpp
9+
sensor_msgs
10+
std_msgs
11+
)
12+
13+
## System dependencies are found with CMake's conventions
14+
# find_package(Boost REQUIRED COMPONENTS system)
15+
16+
17+
## Uncomment this if the package has a setup.py. This macro ensures
18+
## modules and global scripts declared therein get installed
19+
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
20+
# catkin_python_setup()
21+
22+
################################################
23+
## Declare ROS messages, services and actions ##
24+
################################################
25+
26+
## To declare and build messages, services or actions from within this
27+
## package, follow these steps:
28+
## * Let MSG_DEP_SET be the set of packages whose message types you use in
29+
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
30+
## * In the file package.xml:
31+
## * add a build_depend tag for "message_generation"
32+
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
33+
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
34+
## but can be declared for certainty nonetheless:
35+
## * add a run_depend tag for "message_runtime"
36+
## * In this file (CMakeLists.txt):
37+
## * add "message_generation" and every package in MSG_DEP_SET to
38+
## find_package(catkin REQUIRED COMPONENTS ...)
39+
## * add "message_runtime" and every package in MSG_DEP_SET to
40+
## catkin_package(CATKIN_DEPENDS ...)
41+
## * uncomment the add_*_files sections below as needed
42+
## and list every .msg/.srv/.action file to be processed
43+
## * uncomment the generate_messages entry below
44+
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
45+
46+
## Generate messages in the 'msg' folder
47+
# add_message_files(
48+
# FILES
49+
# Message1.msg
50+
# Message2.msg
51+
# )
52+
53+
## Generate services in the 'srv' folder
54+
# add_service_files(
55+
# FILES
56+
# Service1.srv
57+
# Service2.srv
58+
# )
59+
60+
## Generate actions in the 'action' folder
61+
# add_action_files(
62+
# FILES
63+
# Action1.action
64+
# Action2.action
65+
# )
66+
67+
## Generate added messages and services with any dependencies listed here
68+
# generate_messages(
69+
# DEPENDENCIES
70+
# sensor_msgs# std_msgs
71+
# )
72+
73+
################################################
74+
## Declare ROS dynamic reconfigure parameters ##
75+
################################################
76+
77+
## To declare and build dynamic reconfigure parameters within this
78+
## package, follow these steps:
79+
## * In the file package.xml:
80+
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
81+
## * In this file (CMakeLists.txt):
82+
## * add "dynamic_reconfigure" to
83+
## find_package(catkin REQUIRED COMPONENTS ...)
84+
## * uncomment the "generate_dynamic_reconfigure_options" section below
85+
## and list every .cfg file to be processed
86+
87+
## Generate dynamic reconfigure parameters in the 'cfg' folder
88+
# generate_dynamic_reconfigure_options(
89+
# cfg/DynReconf1.cfg
90+
# cfg/DynReconf2.cfg
91+
# )
92+
93+
###################################
94+
## catkin specific configuration ##
95+
###################################
96+
## The catkin_package macro generates cmake config files for your package
97+
## Declare things to be passed to dependent projects
98+
## INCLUDE_DIRS: uncomment this if you package contains header files
99+
## LIBRARIES: libraries you create in this project that dependent projects also need
100+
## CATKIN_DEPENDS: catkin_packages dependent projects also need
101+
## DEPENDS: system dependencies of this project that dependent projects also need
102+
catkin_package(
103+
# INCLUDE_DIRS include
104+
# LIBRARIES ker_imu_read
105+
# CATKIN_DEPENDS roscpp sensor_msgs std_msgs
106+
# DEPENDS system_lib
107+
)
108+
109+
###########
110+
## Build ##
111+
###########
112+
113+
## Specify additional locations of header files
114+
## Your package locations should be listed before other locations
115+
# include_directories(include)
116+
include_directories(
117+
${catkin_INCLUDE_DIRS}
118+
)
119+
120+
## Declare a C++ library
121+
# add_library(ker_imu_read
122+
# src/${PROJECT_NAME}/ker_imu_read.cpp
123+
# )
124+
125+
## Add cmake target dependencies of the library
126+
## as an example, code may need to be generated before libraries
127+
## either from message generation or dynamic reconfigure
128+
# add_dependencies(ker_imu_read ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
129+
130+
## Declare a C++ executable
131+
# add_executable(ker_imu_read_node src/ker_imu_read_node.cpp)
132+
133+
## Add cmake target dependencies of the executable
134+
## same as for the library above
135+
# add_dependencies(ker_imu_read_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
136+
137+
## Specify libraries to link a library or executable target against
138+
# target_link_libraries(ker_imu_read_node
139+
# ${catkin_LIBRARIES}
140+
# )
141+
142+
#############
143+
## Install ##
144+
#############
145+
146+
# all install targets should use catkin DESTINATION variables
147+
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
148+
149+
## Mark executable scripts (Python etc.) for installation
150+
## in contrast to setup.py, you can choose the destination
151+
# install(PROGRAMS
152+
# scripts/my_python_script
153+
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
154+
# )
155+
156+
## Mark executables and/or libraries for installation
157+
# install(TARGETS ker_imu_read ker_imu_read_node
158+
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
159+
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
160+
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
161+
# )
162+
163+
## Mark cpp header files for installation
164+
# install(DIRECTORY include/${PROJECT_NAME}/
165+
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
166+
# FILES_MATCHING PATTERN "*.h"
167+
# PATTERN ".svn" EXCLUDE
168+
# )
169+
170+
## Mark other files for installation (e.g. launch and bag files, etc.)
171+
# install(FILES
172+
# # myfile1
173+
# # myfile2
174+
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
175+
# )
176+
177+
#############
178+
## Testing ##
179+
#############
180+
181+
## Add gtest based cpp test target and link libraries
182+
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ker_imu_read.cpp)
183+
# if(TARGET ${PROJECT_NAME}-test)
184+
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
185+
# endif()
186+
187+
## Add folders to be run by python nosetests
188+
# catkin_add_nosetests(test)

‎ker_imu_read/package.xml

+56
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
<?xml version="1.0"?>
2+
<package>
3+
<name>ker_imu_read</name>
4+
<version>0.0.0</version>
5+
<description>The ker_imu_read package</description>
6+
7+
<!-- One maintainer tag required, multiple allowed, one person per tag -->
8+
<!-- Example: -->
9+
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10+
<maintainer email="luka@todo.todo">luka</maintainer>
11+
12+
13+
<!-- One license tag required, multiple allowed, one license per tag -->
14+
<!-- Commonly used license strings: -->
15+
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16+
<license>TODO</license>
17+
18+
19+
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
20+
<!-- Optional attribute type can be: website, bugtracker, or repository -->
21+
<!-- Example: -->
22+
<!-- <url type="website">http://wiki.ros.org/ker_imu_read</url> -->
23+
24+
25+
<!-- Author tags are optional, mutiple are allowed, one per tag -->
26+
<!-- Authors do not have to be maintianers, but could be -->
27+
<!-- Example: -->
28+
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29+
30+
31+
<!-- The *_depend tags are used to specify dependencies -->
32+
<!-- Dependencies can be catkin packages or system dependencies -->
33+
<!-- Examples: -->
34+
<!-- Use build_depend for packages you need at compile time: -->
35+
<!-- <build_depend>message_generation</build_depend> -->
36+
<!-- Use buildtool_depend for build tool packages: -->
37+
<!-- <buildtool_depend>catkin</buildtool_depend> -->
38+
<!-- Use run_depend for packages you need at runtime: -->
39+
<!-- <run_depend>message_runtime</run_depend> -->
40+
<!-- Use test_depend for packages you need only for testing: -->
41+
<!-- <test_depend>gtest</test_depend> -->
42+
<buildtool_depend>catkin</buildtool_depend>
43+
<build_depend>roscpp</build_depend>
44+
<build_depend>sensor_msgs</build_depend>
45+
<build_depend>std_msgs</build_depend>
46+
<run_depend>roscpp</run_depend>
47+
<run_depend>sensor_msgs</run_depend>
48+
<run_depend>std_msgs</run_depend>
49+
50+
51+
<!-- The export tag contains other, unspecified, tags -->
52+
<export>
53+
<!-- Other tools can request additional information be placed here -->
54+
55+
</export>
56+
</package>

‎ker_imu_read/src/ker_imu_read.cpp

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
#include "ros/ros.h"
2+
#include "std_msgs/String.h"
3+
4+
void chatterCallback(const std_msgs::String::ConstPtr& msg)
5+
{
6+
ROS_INFO("I heard: [%s]", msg->data.c_str());
7+
}
8+
9+
int main(int argc, char **argv)
10+
{
11+
ros::init(argc, argv, "listener");
12+
13+
ros::NodeHandle n;
14+
15+
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
16+
17+
ros::spin();
18+
19+
return 0;
20+
}

‎ker_opencm/dxl_8_v001/dxl_8_v001.ino

+51-25
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,9 @@
2929
#define P_GOAL_POSITION 30
3030
#define P_GOAL_SPEED 32
3131
#define PRESENT_POS 54
32+
#define P_Gain 29
33+
#define I_Gain 28
34+
#define D_Gain 27
3235

3336
/********* Sync write data **************
3437
* ID1, DATA1, DATA2..., ID2, DATA1, DATA2,...
@@ -43,11 +46,10 @@ Dynamixel Dxl(DXL_BUS_SERIAL1);
4346

4447
int pos, i = 0, j = 0;
4548
int pos_goal = 0, vel_goal = 0;
46-
char poruka[]="" , motor1[5],motor2[5],motor3[5],motor4[5],motor5[5],motor6[5];
47-
char send_data[] = "", mot7[5],moto8[5];
48-
unsigned int stop = 0, start = 0;
49-
word motor[8];
50-
49+
char poruka[]="";
50+
char send_data[] = "";
51+
word motor[12];
52+
int button = 0;
5153
//word SyncPage1[24]=
5254
//{
5355
// ID_NUM_1,0,300, // 3 Dynamixels are move to position 0
@@ -64,8 +66,23 @@ void setup(){
6466
// Dynamixel 2.0 Protocol -> 0: 9600, 1: 57600, 2: 115200, 3: 1Mbps
6567
Dxl.begin(3);
6668
//Set all dynamixels as same condition.
67-
Dxl.writeWord( BROADCAST_ID, P_GOAL_POSITION, 0 );
68-
Dxl.writeWord( BROADCAST_ID, P_GOAL_SPEED, 500 );
69+
Dxl.writeWord( BROADCAST_ID, P_GOAL_POSITION, 511 );
70+
delay(200);
71+
Dxl.writeWord( BROADCAST_ID, P_GOAL_SPEED, 200 );
72+
for (j = 0; j < 12; j++){
73+
if(j == 0 || j == 3 || j == 6 || j == 9){
74+
Dxl.writeByte( j+1, P_Gain, 64);
75+
Dxl.writeByte( j+1, D_Gain, 8);
76+
}
77+
Dxl.writeByte( j+1, P_Gain, 64 );
78+
Dxl.writeByte( j+1, D_Gain, 8);
79+
}
80+
// Dxl.writeByte( BROADCAST_ID, P_Gain, 64 );
81+
// Dxl.writeByte( BROADCAST_ID, D_Gain, 32 );
82+
Dxl.ledOn(BROADCAST_ID, 3);
83+
pinMode(BOARD_BUTTON_PIN, INPUT_PULLDOWN);
84+
pinMode(BOARD_LED_PIN, OUTPUT);
85+
6986
}
7087

7188
void loop(){
@@ -79,10 +96,19 @@ void loop(){
7996
// SerialUSB.write(buffer);
8097
// SerialUSB.write(">");
8198
// }
82-
start = micros();
83-
for(j = 0; j < 8; j++){
99+
if (digitalRead(BOARD_BUTTON_PIN)){
100+
button = 1;
101+
}
102+
for(j = 0; j < 12; j++){
84103
pos = Dxl.readWord(j+1, PRESENT_POS);
85-
itoa((1000*(j+1)+pos), &send_data[j*4], 10);
104+
if (j == 1 || j == 4 || j == 6 || j == 8 || j == 11){
105+
itoa(1022 - (pos), &send_data[j*3], 10);
106+
}
107+
else if (j == 9)
108+
itoa(1022 - (pos), &send_data[j*3], 10);
109+
else{
110+
itoa((pos), &send_data[j*3], 10);
111+
}
86112
}
87113
SerialUSB.write(send_data);
88114
SerialUSB.write('\r\n');
@@ -91,23 +117,24 @@ void loop(){
91117
poruka[i] = word(SerialUSB.read());
92118
i++;
93119
}
94-
if ( i > 0 ){
95-
for(j = 0; j < 4; j++){
96-
motor1[j] = poruka[j];
97-
motor2[j] = poruka[j+4];
98-
motor3[j] = poruka[j+8];
99-
motor4[j] = poruka[j+12];
100-
motor5[j] = poruka[j+16];
101-
motor6[j] = poruka[j+20];
102-
mot7[j] = poruka[j+24];
103-
moto8[j] = poruka[j+28];
104-
}
120+
if ( i > 0 && button){
105121

106-
word motor[8] = {word(atoi(&motor1[0])), word(atoi(&motor2[0])), word(atoi(&motor3[0])), word(atoi(&motor4[0])), word(atoi(&motor5[0])), word(atoi(&motor6[0])), word(atoi(&mot7[0])), word(atoi(&moto8[0]))};
107-
108-
for( j = 0; j < 8; j++){
122+
for(j = 0; j < 12; j++){
123+
motor[j] = word(poruka[j*3]-'0')*100 + word(poruka[j*3+1]-'0')*10 + word(poruka[j*3+2]-'0');
124+
}
125+
motor[1] = 1022 - motor[1];
126+
motor[4] = 1022 - motor[4];
127+
motor[6] = 1022 - motor[6];
128+
motor[8] = 1022 - motor[8];
129+
motor[9] = 1022 - motor[9] - 5;
130+
motor[11] = 1022 - motor[11];
131+
for( j = 11; j >= 0; j--){
132+
if (motor[j] == 0){
133+
motor[j] = 512;
134+
}
109135
Dxl.writeWord(j+1, P_GOAL_POSITION, motor[j]);
110136
}
137+
111138
// SyncPage1[1] = word(atoi(&motor1[0]));
112139
// SyncPage1[4] = word(atoi(&motor2[0]));
113140
// SyncPage1[7] = word(atoi(&motor3[0]));
@@ -120,7 +147,6 @@ void loop(){
120147
char poruka[] = "";
121148
i = 0;
122149
}
123-
stop = micros();
124150
//SerialUSB.print("Freq: ");
125151
//SerialUSB.println(float(1000000/(stop-start)));
126152
}

0 commit comments

Comments
 (0)