diff --git a/darwin_apps/launch/action_client.launch b/darwin_apps/launch/action_client.launch
index 3dd6eaafd260e51a712531c75a60678f8b087b17..5ee48f03d6dcc0c319ceda19a96fc6a83ff14aba 100644
--- a/darwin_apps/launch/action_client.launch
+++ b/darwin_apps/launch/action_client.launch
@@ -12,15 +12,11 @@
type="action_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/motion"
+ <remap from="/darwin/action_client/action_client/motion_action"
to="/darwin/robot/motion_action"/>
- <remap from="/darwin/action_client/set_servo_modules"
+ <remap from="/darwin/action_client/action_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
</node>
- <!-- launch dynamic reconfigure -->
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
- output="screen"/>
-
</launch>
diff --git a/darwin_apps/launch/action_client_sim.launch b/darwin_apps/launch/action_client_sim.launch
index 3c42daf4734b4f8f30b3f87d1ede152e5f192de0..901945390f4932a9aa18f2d473ebe3243d3df8a5 100644
--- a/darwin_apps/launch/action_client_sim.launch
+++ b/darwin_apps/launch/action_client_sim.launch
@@ -12,9 +12,9 @@
type="action_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/action_client/action_client/motion_action"
+ <remap from="/darwin/action_client/motion_action"
to="/darwin/robot/motion_action"/>
- <remap from="/darwin/action_client/action_client/set_servo_modules"
+ <remap from="/darwin/action_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
</node>
diff --git a/darwin_apps/launch/all_clients.launch b/darwin_apps/launch/all_clients.launch
index dd5c6a3e83dd1727959dcfe167197c952866a822..e580358f91bafae401f62293fead122767a576ac 100644
--- a/darwin_apps/launch/all_clients.launch
+++ b/darwin_apps/launch/all_clients.launch
@@ -12,9 +12,9 @@
type="action_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/motion"
+ <remap from="/darwin/action_client/action_client/motion_action"
to="/darwin/robot/motion_action"/>
- <remap from="/darwin/action_client/set_servo_modules"
+ <remap from="/darwin/action_client/action_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
</node>
@@ -24,10 +24,12 @@
type="joints_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/joint_trajectory"
+ <remap from="/darwin/joints_client/joint_trajectory/joint_trajectory_action"
to="/darwin/robot/joint_trajectory"/>
- <remap from="/darwin/joints_client/set_servo_modules"
+ <remap from="/darwin/joints_client/joint_trajectory/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/joints_client/joint_trajectory/joint_state"
+ to="/darwin/robot/joint_state"/>
</node>
<!-- launch the walk client node -->
@@ -53,12 +55,18 @@
<param name="FOOT_HEIGHT" value="0.04"/>
<param name="MAX_VEL" value="0.01"/>
<param name="MAX_ROT_VEL" value="0.01"/>
- <remap from="/darwin/walk_client/cmd_vel"
+ <remap from="/darwin/walk_client/walk_client/cmd_vel"
to="/darwin/robot/cmd_vel"/>
- <remap from="/darwin/walk_client/set_walk_params"
+ <remap from="/darwin/walk_client/walk_client/set_walk_params"
to="/darwin/robot/set_walk_params"/>
- <remap from="/darwin/walk_client/set_servo_modules"
+ <remap from="/darwin/walk_client/walk_client/get_walk_params"
+ to="/darwin/robot/get_walk_params"/>
+ <remap from="/darwin/walk_client/walk_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/walk_client/walk_client/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/walk_client/walk_client/fallen_state"
+ to="/darwin/robot/fallen_state"/>
</node>
<!-- launch the action client node -->
@@ -67,16 +75,20 @@
type="head_tracking_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/head_follow_target"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/head_tracking_action"
to="/darwin/robot/head_follow_target"/>
- <remap from="/darwin/head_tracking_client/head_target"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/head_target"
to="/darwin/robot/head_target"/>
- <remap from="/darwin/head_tracking_client/set_servo_modules"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
- <remap from="/darwin/head_tracking_client/set_pan_pid"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/set_pan_pid"
to="/darwin/robot/set_pan_pid"/>
- <remap from="/darwin/head_tracking_client/set_tilt_pid"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/get_pan_pid"
+ to="/darwin/robot/get_pan_pid"/>
+ <remap from="/darwin/head_tracking_client/head_tracking_client/set_tilt_pid"
to="/darwin/robot/set_tilt_pid"/>
+ <remap from="/darwin/head_tracking_client/head_tracking_client/get_tilt_pid"
+ to="/darwin/robot/get_tilt_pid"/>
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_apps/launch/all_clients_sim.launch b/darwin_apps/launch/all_clients_sim.launch
index f888808ef302b795ed4d3b04bd9ff09bdec97329..6704a70bc7e2fb5b88770ef6c6037ca3bf14f3b2 100644
--- a/darwin_apps/launch/all_clients_sim.launch
+++ b/darwin_apps/launch/all_clients_sim.launch
@@ -1,21 +1,26 @@
<launch>
<arg name="robot" default="darwin" />
+ <arg name="environment" default="obstacles_env_empty" />
<include file="$(find darwin_description)/launch/darwin_sim.launch">
<arg name="robot" value="$(arg robot)" />
</include>
+ <include file="$(find darwin_description)/launch/environment.launch">
+ <arg name="environment" value="$(arg environment)" />
+ </include>
+
<!-- launch the action client node -->
<node name="action_client"
pkg="action_client"
type="action_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/motion"
- to="/darwin/robot/motion_action"/> <!--Action client: action_client_alg_node.cpp --><!--Action server darwin_driver -->
- <remap from="/darwin/action_client/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/> <!--Client --><!--Service: darwin_driver -->
+ <remap from="/darwin/action_client/motion_action"
+ to="/darwin/robot/motion_action"/>
+ <remap from="/darwin/action_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
</node>
<!-- launch the action client node -->
@@ -24,10 +29,12 @@
type="joints_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/joint_trajectory"
- to="/darwin/robot/joint_trajectory"/> <!--Action client --><!--Action server: darwin_driver -->
- <remap from="/darwin/joints_client/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/> <!--Server: darwin_driver -->
+ <remap from="/darwin/joint_trajectory/joint_trajectory_action"
+ to="/darwin/robot/joint_trajectory"/>
+ <remap from="/darwin/joint_trajectory/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/joint_trajectory/joint_states"
+ to="/darwin/joint_states"/>
</node>
<!-- launch the walk client node -->
@@ -53,12 +60,18 @@
<param name="FOOT_HEIGHT" value="0.04"/>
<param name="MAX_VEL" value="0.01"/>
<param name="MAX_ROT_VEL" value="0.01"/>
- <remap from="/darwin/walk_client/cmd_vel"
- to="/darwin/robot/cmd_vel"/> <!--Publisher --><!--Subscriber: darwin_driver -->
+ <remap from="/walk_client/cmd_vel"
+ to="/darwin/robot/cmd_vel"/>
<remap from="/darwin/walk_client/set_walk_params"
- to="/darwin/robot/set_walk_params"/> <!--Service: darwin_driver -->
+ to="/darwin/robot/set_walk_params"/>
+ <remap from="/darwin/walk_client/get_walk_params"
+ to="/darwin/robot/get_walk_params"/>
<remap from="/darwin/walk_client/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/> <!--Service: darwin_driver -->
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/walk_client/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/walk_client/fallen_state"
+ to="/darwin/robot/fallen_state"/>
</node>
<!-- launch the action client node -->
@@ -67,16 +80,96 @@
type="head_tracking_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/head_follow_target"
- to="/darwin/robot/head_follow_target"/> <!--Action server: darwin_driver -->
- <remap from="/darwin/head_tracking_client/head_target"
- to="/darwin/robot/head_target"/> <!--Publisher --><!--Subscriber: darwin_driver -->
- <remap from="/darwin/head_tracking_client/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/> <!--Client --><!--Service: darwin_driver -->
+ <remap from="/darwin/head_tracking_client/head_tracking_action"
+ to="/darwin/robot/head_follow_target"/>
+ <remap from="/head_tracking_client/head_target"
+ to="/darwin/robot/head_target"/>
+ <remap from="/darwin/head_tracking_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
<remap from="/darwin/head_tracking_client/set_pan_pid"
to="/darwin/robot/set_pan_pid"/>
+ <remap from="/darwin/head_tracking_client/get_pan_pid"
+ to="/darwin/robot/get_pan_pid"/>
<remap from="/darwin/head_tracking_client/set_tilt_pid"
- to="/darwin/robot/set_tilt_pid"/> <!--Service: darwin_driver -->
+ to="/darwin/robot/set_tilt_pid"/>
+ <remap from="/darwin/head_tracking_client/get_tilt_pid"
+ to="/darwin/robot/get_tilt_pid"/>
+ </node>
+
+ <!-- launch the action client node -->
+ <node name="left_joints_cart_client"
+ pkg="joints_cart_client"
+ type="joints_cart_client"
+ output="screen"
+ ns="/darwin">
+ <remap from="/darwin/left_joints_cart_client/joints_action"
+ to="/darwin/robot/joint_trajectory"/>
+ <remap from="/darwin/left_joints_cart_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/left_joints_cart_client/get_arm_ik"
+ to="/darwin/left_arm_kinematics/get_ik"/>
+ <remap from="/darwin/left_joints_cart_client/arm_ik_set_params"
+ to="/darwin/left_arm_kinematics/set_parameters"/>
+ <remap from="/left_joints_cart_client/joint_states"
+ to="/darwin/joint_states"/>
+ </node>
+
+ <!-- load the controllers -->
+ <node name="left_arm_kinematics"
+ pkg="darwin_arm_kinematics"
+ type="darwin_arm_kinematics"
+ output="screen"
+ ns="/darwin">
+ <param name="chain_file" value="$(find darwin_arm_kinematics)/config/left_arm.xml"/>
+ <param name="tf_prefix" value="/darwin"/>
+ <param name="urdf_param" value="robot_description"/>
+ <param name="arm" value="0"/>
+ </node>
+
+ <!-- launch the action client node -->
+ <node name="right_joints_cart_client"
+ pkg="joints_cart_client"
+ type="joints_cart_client"
+ output="screen"
+ ns="/darwin">
+ <remap from="/darwin/right_joints_cart_client/joints_action"
+ to="/darwin/robot/joint_trajectory"/>
+ <remap from="/darwin/right_joints_cart_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/right_joints_cart_client/joints_cart_client/get_arm_ik"
+ to="/darwin/right_arm_kinematics/get_ik"/>
+ <remap from="/darwin/right_joints_cart_client/arm_ik_set_params"
+ to="/darwin/right_arm_kinematics/set_parameters"/>
+ <remap from="/right_joints_cart_client/joint_states"
+ to="/darwin/joint_states"/>
+ </node>
+
+ <!-- load the controllers -->
+ <node name="right_arm_kinematics"
+ pkg="darwin_arm_kinematics"
+ type="darwin_arm_kinematics"
+ output="screen"
+ ns="/darwin">
+ <param name="chain_file" value="$(find darwin_arm_kinematics)/config/right_arm.xml"/>
+ <param name="tf_prefix" value="/darwin"/>
+ <param name="urdf_param" value="robot_description"/>
+ <param name="arm" value="1"/>
+ </node>
+
+ <!-- launch the gripper client node -->
+ <node name="gripper_client"
+ pkg="gripper_client"
+ type="gripper_client"
+ output="screen"
+ ns="/darwin">
+ <remap from="/darwin/left_gripper/gripper_control"
+ to="/darwin/robot/left_gripper"/>
+ <remap from="/darwin/right_gripper/gripper_control"
+ to="/darwin/robot/right_gripper"/>
+ <remap from="/darwin/left_gripper/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/right_gripper/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_apps/launch/ceabot/ceabot_base.launch b/darwin_apps/launch/ceabot/ceabot_base.launch
new file mode 100644
index 0000000000000000000000000000000000000000..e33e9e290b99e54fc396bbd7f2251b8ee5c8b0d8
--- /dev/null
+++ b/darwin_apps/launch/ceabot/ceabot_base.launch
@@ -0,0 +1,13 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+
+ <include file="$(find darwin_description)/launch/darwin_base.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <include file="$(find darwin_apps)/launch/xs_camera_qr.launch"/>
+
+ <include file="$(find bno055_imu)/launch/darwin_bno055_imu.launch"/>
+</launch>
+
diff --git a/darwin_apps/launch/ceabot/ceabot_vision.launch b/darwin_apps/launch/ceabot/ceabot_vision.launch
deleted file mode 100644
index 859a8242ea1f754b1c6c851ce5996843dece7293..0000000000000000000000000000000000000000
--- a/darwin_apps/launch/ceabot/ceabot_vision.launch
+++ /dev/null
@@ -1,41 +0,0 @@
-<launch>
-
- <arg name="robot" default="darwin" />
-
- <include file="$(find darwin_description)/launch/darwin_base.launch">
- <arg name="robot" value="$(arg robot)" />
- </include>
-
- <include file="$(find darwin_apps)/launch/xs_camera_qr.launch"/>
-
- <!-- launch the vision_prove node -->
- <node pkg ="iri_vision_prove"
- type="iri_vision_prove"
- name="iri_vision_prove"
- output="screen">
- <remap from="/head_follow_target"
- to="/darwin/robot/head_follow_target"/>
- <remap from="/iri_vision_prove/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/>
- <remap from="/iri_vision_prove/head_target"
- to="/darwin/robot/head_target"/>
- <remap from="/iri_vision_prove/qr_pose"
- to="/darwin/qr_detector/qr_pose"/>
- <remap from="/iri_vision_prove/joint_states"
- to="/darwin/joint_states"/>
- <remap from="/joint_trajectory"
- to="/darwin/robot/joint_trajectory"/>
- <remap from="/iri_vision_prove/cmd_vel"
- to="/darwin/robot/cmd_vel"/>
- <remap from="/iri_vision_prove/set_walk_params"
- to="/darwin/robot/set_walk_params"/>
- <remap from="/motion_action"
- to="/darwin/robot/motion_action"/>
- </node>
-
- <!-- launch dynamic reconfigure -->
-<!-- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
- output="screen"/>-->
-
-</launch>
-
diff --git a/darwin_apps/launch/darwin_cpp.launch b/darwin_apps/launch/darwin_cpp.launch
new file mode 100644
index 0000000000000000000000000000000000000000..0f015b348154730bb7770308df63f23065d99858
--- /dev/null
+++ b/darwin_apps/launch/darwin_cpp.launch
@@ -0,0 +1,25 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+ <arg name="environment" default="obstacles_env" />
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <include file="$(find bioloid_description)/launch/obstacles_env.launch">
+ <arg name="environment" value="$(arg environment)" />
+ </include>
+
+ <node name="gazebo_image_to_video_dev"
+ pkg="gazebo_image_to_video_dev"
+ type="gazebo_image_to_video_dev"
+ output="screen">
+ <remap from="~/camera_in/image_raw"
+ to="/darwin/camera/image_raw"/>
+ <remap from="~/camera_in/camera_info"
+ to="/darwin/camera/camera_info"/>
+ <param name="video_dev" value="/dev/video0"/>
+ </node>
+
+</launch>
diff --git a/darwin_apps/launch/darwin_eq.launch b/darwin_apps/launch/darwin_eq.launch
new file mode 100644
index 0000000000000000000000000000000000000000..cd7f5b44c1571ae5135e7c1ceff3dd8505f62b8a
--- /dev/null
+++ b/darwin_apps/launch/darwin_eq.launch
@@ -0,0 +1,14 @@
+<launch>
+
+ <arg name="robot" default="darwin_cpp" />
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <group ns="platform">
+ <include file="$(find eq_platform_description)/launch/eq_platform.launch">
+ </include>
+ </group>
+
+</launch>
diff --git a/darwin_apps/launch/gripper_client_sim.launch b/darwin_apps/launch/gripper_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..7775da40e71ca8b978755d1d4d81cfec0975c02d
--- /dev/null
+++ b/darwin_apps/launch/gripper_client_sim.launch
@@ -0,0 +1,30 @@
+<launch>
+
+ <arg name="robot" default="darwin_sc" />
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <!-- launch the gripper client node -->
+ <node name="gripper_client"
+ pkg="gripper_client"
+ type="gripper_client"
+ output="screen"
+ ns="/darwin">
+ <remap from="/darwin/left_gripper/gripper_control"
+ to="/darwin/robot/left_gripper"/>
+ <remap from="/darwin/right_gripper/gripper_control"
+ to="/darwin/robot/right_gripper"/>
+ <remap from="/darwin/left_gripper/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/right_gripper/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
+
diff --git a/darwin_apps/launch/head_tracking_client.launch b/darwin_apps/launch/head_tracking_client.launch
index a7a75799352b28895f0c03e41af13572ae8612c9..c1e9a449a05e311c6c1fcce6d1b4937bddb46d09 100644
--- a/darwin_apps/launch/head_tracking_client.launch
+++ b/darwin_apps/launch/head_tracking_client.launch
@@ -12,16 +12,20 @@
type="head_tracking_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/head_follow_target"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/head_tracking_action"
to="/darwin/robot/head_follow_target"/>
- <remap from="/darwin/head_tracking_client/head_target"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/head_target"
to="/darwin/robot/head_target"/>
- <remap from="/darwin/head_tracking_client/set_servo_modules"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
- <remap from="/darwin/head_tracking_client/set_pan_pid"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/set_pan_pid"
to="/darwin/robot/set_pan_pid"/>
- <remap from="/darwin/head_tracking_client/set_tilt_pid"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/get_pan_pid"
+ to="/darwin/robot/get_pan_pid"/>
+ <remap from="/darwin/head_tracking_client/head_tracking_client/set_tilt_pid"
to="/darwin/robot/set_tilt_pid"/>
+ <remap from="/darwin/head_tracking_client/head_tracking_client/get_tilt_pid"
+ to="/darwin/robot/get_tilt_pid"/>
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_apps/launch/head_tracking_client_sim.launch b/darwin_apps/launch/head_tracking_client_sim.launch
index f1bb5f02915c751a64716a15d998c50531262fc7..7a6f20923f9d46f4559886ee8cb8f670aea58870 100644
--- a/darwin_apps/launch/head_tracking_client_sim.launch
+++ b/darwin_apps/launch/head_tracking_client_sim.launch
@@ -12,19 +12,19 @@
type="head_tracking_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/head_tracking_client/head_tracking_client/head_tracking_action"
+ <remap from="/darwin/head_tracking_client/head_tracking_action"
to="/darwin/robot/head_follow_target"/>
- <remap from="/darwin/head_tracking_client/head_tracking_client/head_target"
+ <remap from="/head_tracking_client/head_target"
to="/darwin/robot/head_target"/>
- <remap from="/darwin/head_tracking_client/head_tracking_client/set_servo_modules"
+ <remap from="/darwin/head_tracking_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
- <remap from="/darwin/head_tracking_client/head_tracking_client/set_pan_pid"
+ <remap from="/darwin/head_tracking_client/set_pan_pid"
to="/darwin/robot/set_pan_pid"/>
- <remap from="/darwin/head_tracking_client/head_tracking_client/get_pan_pid"
+ <remap from="/darwin/head_tracking_client/get_pan_pid"
to="/darwin/robot/get_pan_pid"/>
- <remap from="/darwin/head_tracking_client/head_tracking_client/set_tilt_pid"
+ <remap from="/darwin/head_tracking_client/set_tilt_pid"
to="/darwin/robot/set_tilt_pid"/>
- <remap from="/darwin/head_tracking_client/head_tracking_client/get_tilt_pid"
+ <remap from="/darwin/head_tracking_client/get_tilt_pid"
to="/darwin/robot/get_tilt_pid"/>
</node>
diff --git a/darwin_apps/launch/joint_trajectory_client_sim.launch b/darwin_apps/launch/joint_trajectory_client_sim.launch
index 6c8ce1512abf3e85e4a3f0c01c2b1597499647a4..05ec16f7574920a697f7bf71482aeecd6e8d42b9 100644
--- a/darwin_apps/launch/joint_trajectory_client_sim.launch
+++ b/darwin_apps/launch/joint_trajectory_client_sim.launch
@@ -12,10 +12,12 @@
type="joints_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/joint_trajectory" <!-- -->
- to="/darwin/robot/joint_trajectory"/> <!--Action server: darwin_driver_node.cpp -->
- <remap from="/darwin/joints_client/set_servo_modules" <!--Client: joints_client_alg_node.cpp -->
- to="/darwin/robot/set_servo_modules"/> <!-- Service: darwin_driver_node.cpp -->
+ <remap from="/darwin/joint_trajectory/joint_trajectory_action"
+ to="/darwin/robot/joint_trajectory"/>
+ <remap from="/darwin/joint_trajectory/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/joint_trajectory/joint_states"
+ to="/darwin/joint_states"/>
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_apps/launch/joints_cartesian_client_sim.launch b/darwin_apps/launch/joints_cartesian_client_sim.launch
index e4925c826be1ff010f467f76a5f986df4f4f5915..0271bc6119b404eaa37a0b0c72fee6266949b024 100644
--- a/darwin_apps/launch/joints_cartesian_client_sim.launch
+++ b/darwin_apps/launch/joints_cartesian_client_sim.launch
@@ -12,16 +12,20 @@
type="joints_cart_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/joints_cart_client/joints_cart_client/joints_action"
+ <remap from="/darwin/joints_cart_client/joints_action"
to="/darwin/robot/joint_trajectory"/>
- <remap from="/darwin/joints_cart_client/joints_cart_client/set_servo_modules"
+ <remap from="/darwin/joints_cart_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
- <remap from="/darwin/joints_cart_client/joints_cart_client/get_arm_ik"
+ <remap from="/darwin/joints_cart_client/get_arm_ik"
to="/darwin/left_arm_kinematics/get_ik"/>
- <remap from="/darwin/joints_cart_client/joints_cart_client/arm_ik_set_params"
+ <remap from="/darwin/joints_cart_client/arm_ik_set_params"
to="/darwin/left_arm_kinematics/set_parameters"/>
- <remap from="/darwin/joints_cart_client/joints_cart_client/joint_states"
+ <remap from="/darwin/joints_cart_client/joint_states"
to="/darwin/joint_states"/>
+ <param name="frame_id" value="/darwin/base_arm_l"/>
+ <param name="x_pos" value="-0.0686496"/>
+ <param name="y_pos" value="-0.0499658"/>
+ <param name="z_pos" value="0.0237217"/>
</node>
<!-- load the controllers -->
diff --git a/darwin_apps/launch/nav_client_sim.launch b/darwin_apps/launch/nav_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..02b718798fa908a31ee459836dbd8ace358d7e3a
--- /dev/null
+++ b/darwin_apps/launch/nav_client_sim.launch
@@ -0,0 +1,61 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+ <arg name="environment" default="obstacles_env_empty" />
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <include file="$(find bioloid_description)/launch/obstacles_env.launch">
+ <arg name="environment" value="$(arg environment)" />
+ </include>
+
+ <include file="$(find darwin_nav)/launch/darwin_global_loc_sim.launch"/>
+
+ <!-- Move Base -->
+ <node pkg="move_base"
+ type="move_base"
+ name="move_base"
+ output="screen">
+ <remap from="cmd_vel" to="/darwin/robot/cmd_vel"/>
+ <remap from="odom" to="/darwin/robot/odom"/>
+ <!-- Main parameters -->
+ <rosparam file="$(find darwin_nav)/config/move_base_params.yaml" command="load"/>
+ <!-- LOCAL PLANNER -->
+ <rosparam file="$(find darwin_nav)/config/dwa_local_planner_params.yaml" command="load"/>
+ <rosparam file="$(find darwin_nav)/config/costmap_common_params.yaml" command="load" ns="local_costmap"/>
+ <rosparam file="$(find darwin_nav)/config/local_costmap_params.yaml" command="load" ns="local_costmap"/>
+ <!-- GLOBAL PLANNER -->
+ <rosparam file="$(find darwin_nav)/config/global_planner_params.yaml" command="load"/>
+ <rosparam file="$(find darwin_nav)/config/costmap_common_params.yaml" command="load" ns="global_costmap"/>
+ <rosparam file="$(find darwin_nav)/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
+ </node>
+
+ <!-- launch the navigation client node -->
+ <node name="nav_client"
+ pkg="nav_client"
+ type="nav_client"
+ output="screen"
+ ns="/darwin">
+ <remap from="/darwin/nav_client/nav_client/nav_module/"
+ to="/move_base"/>
+ <remap from="/darwin/nav_client/nav_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/nav_client/nav_client/set_walk_params"
+ to="/darwin/robot/set_walk_params"/>
+ <remap from="/darwin/nav_client/nav_client/get_walk_params"
+ to="/darwin/robot/get_walk_params"/>
+ <remap from="/darwin/nav_client/nav_client/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/nav_client/nav_client/fallen_state"
+ to="/darwin/robot/fallen_state"/>
+
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
+
diff --git a/darwin_apps/launch/qr_head_tracking_sim.launch b/darwin_apps/launch/qr_head_tracking_sim.launch
index 64a92d58cc1590ef46d97e9907eb4902ced76fde..bc53da15442a03aa470bcef78b40979d3b961d93 100644
--- a/darwin_apps/launch/qr_head_tracking_sim.launch
+++ b/darwin_apps/launch/qr_head_tracking_sim.launch
@@ -17,18 +17,18 @@
type="qr_head_tracking"
output="screen"
ns="/darwin">
- <remap from="/darwin/qr_head_tracking/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/>
- <remap from="/darwin/head_follow_target"
+ <remap from="/darwin/qr_head_tracking/head_tracking_action"
to="/darwin/robot/head_follow_target"/>
<remap from="/darwin/qr_head_tracking/head_target"
to="/darwin/robot/head_target"/>
+ <remap from="/darwin/qr_head_tracking/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
<remap from="/darwin/qr_head_tracking/set_pan_pid"
to="/darwin/robot/set_pan_pid"/>
- <remap from="/darwin/qr_head_tracking/set_tilt_pid"
- to="/darwin/robot/set_tilt_pid"/>
<remap from="/darwin/qr_head_tracking/get_pan_pid"
to="/darwin/robot/get_pan_pid"/>
+ <remap from="/darwin/qr_head_tracking/set_tilt_pid"
+ to="/darwin/robot/set_tilt_pid"/>
<remap from="/darwin/qr_head_tracking/get_tilt_pid"
to="/darwin/robot/get_tilt_pid"/>
<remap from="/darwin/qr_head_tracking/joint_states"
diff --git a/darwin_apps/launch/stairs_client.launch b/darwin_apps/launch/stairs_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..cdeccaad3fa14f8eaff34fb117f7cd30cd7672cc
--- /dev/null
+++ b/darwin_apps/launch/stairs_client.launch
@@ -0,0 +1,55 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+
+ <include file="$(find darwin_description)/launch/darwin_base.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <!-- launch the stairs client node -->
+ <node name="stairs_client"
+ pkg="stairs_client"
+ type="stairs_client"
+ output="screen"
+ ns="/darwin">
+ <param name="SHIFT_WEIGHT_RIGHT_TIME" value="1.6"/>
+ <param name="RISE_RIGHT_FOOT_TIME" value="3.2"/>
+ <param name="ADVANCE_RIGHT_FOOT_TIME" value="4.8"/>
+ <param name="CONTACT_RIGHT_FOOT_TIME" value="6.4"/>
+ <param name="SHIFT_WEIGHT_RIGHT_TIME" value="8.0"/>
+ <param name="RISE_LEFT_FOOT_TIME" value="9.6"/>
+ <param name="ADVANCE_LEFT_FOOT_TIME" value="11.2"/>
+ <param name="CONTACT_LEFT_FOOT_TIME" value="12.8"/>
+ <param name="CENTER_WEIGHT_TIME" value="14.4"/>
+ <param name="X_OFFSET" value="-0.01"/>
+ <param name="Y_OFFSET" value="0.005"/>
+ <param name="Z_OFFSET" value="0.02"/>
+ <param name="R_OFFSET" value="0.0"/>
+ <param name="P_OFFSET" value="0.0"/>
+ <param name="A_OFFSET" value="0.0"/>
+ <param name="Y_SHIFT" value="0.04"/>
+ <param name="X_SHIFT" value="0.08"/>
+ <param name="Z_OVERSHOOT" value="0.015"/>
+ <param name="Z_HEIGHT" value="0.03"/>
+ <param name="HIP_PITCH_OFFSET" value="0.23"/>
+ <param name="R_SHIFT" value="0.05"/>
+ <param name="P_SHIFT" value="0.1"/>
+ <param name="A_SHIFT" value="0.3"/>
+ <param name="Y_SPREAD" value="0.02"/>
+ <param name="X_SHIFT_BODY" value="0.035"/>
+ <remap from="/darwin/stairs_client/stairs_client/climb_stairs"
+ to="/darwin/robot/climb_stairs"/>
+ <remap from="/darwin/stairs_client/stairs_client/set_stairs_params"
+ to="/darwin/robot/set_stairs_params"/>
+ <remap from="/darwin/stairs_client/stairs_client/get_stairs_params"
+ to="/darwin/robot/get_stairs_params"/>
+ <remap from="/darwin/stairs_client/stairs_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/stairs_client/stairs_client/fallen_state"
+ to="/darwin/robot/fallen_state"/>
+ </node>
+
+ <include file="$(find ir_foot_sensor)/launch/darwin_two_feet_nodelet.launch">
+ </include>
+</launch>
+
diff --git a/darwin_apps/launch/stairs_client_sim.launch b/darwin_apps/launch/stairs_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..666526143d14da3535600ffec552e7a7b3dd9984
--- /dev/null
+++ b/darwin_apps/launch/stairs_client_sim.launch
@@ -0,0 +1,61 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+ <arg name="environment" default="stairs_env" />
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <include file="$(find bioloid_description)/launch/stairs_env.launch">
+ <arg name="environment" value="$(arg environment)" />
+ </include>
+
+ <!-- launch the stairs client node -->
+ <node name="stairs_client"
+ pkg="stairs_client"
+ type="stairs_client"
+ output="screen"
+ ns="/darwin">
+ <param name="SHIFT_WEIGHT_RIGHT_TIME" value="1.6"/>
+ <param name="RISE_RIGHT_FOOT_TIME" value="3.2"/>
+ <param name="ADVANCE_RIGHT_FOOT_TIME" value="4.8"/>
+ <param name="CONTACT_RIGHT_FOOT_TIME" value="6.4"/>
+ <param name="SHIFT_WEIGHT_RIGHT_TIME" value="8.0"/>
+ <param name="RISE_LEFT_FOOT_TIME" value="9.6"/>
+ <param name="ADVANCE_LEFT_FOOT_TIME" value="11.2"/>
+ <param name="CONTACT_LEFT_FOOT_TIME" value="12.8"/>
+ <param name="CENTER_WEIGHT_TIME" value="14.4"/>
+ <param name="X_OFFSET" value="-0.01"/>
+ <param name="Y_OFFSET" value="0.005"/>
+ <param name="Z_OFFSET" value="0.02"/>
+ <param name="R_OFFSET" value="0.0"/>
+ <param name="P_OFFSET" value="0.0"/>
+ <param name="A_OFFSET" value="0.0"/>
+ <param name="Y_SHIFT" value="0.04"/>
+ <param name="X_SHIFT" value="0.08"/>
+ <param name="Z_OVERSHOOT" value="0.015"/>
+ <param name="Z_HEIGHT" value="0.03"/>
+ <param name="HIP_PITCH_OFFSET" value="0.23"/>
+ <param name="R_SHIFT" value="0.05"/>
+ <param name="P_SHIFT" value="0.1"/>
+ <param name="A_SHIFT" value="0.3"/>
+ <param name="Y_SPREAD" value="0.02"/>
+ <param name="X_SHIFT_BODY" value="0.035"/>
+ <remap from="/darwin/stairs_client/climb_stairs"
+ to="/darwin/robot/climb_stairs"/>
+ <remap from="/darwin/stairs_client/set_stairs_params"
+ to="/darwin/robot/set_stairs_params"/>
+ <remap from="/darwin/stairs_client/get_stairs_params"
+ to="/darwin/robot/get_stairs_params"/>
+ <remap from="/darwin/stairs_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/stairs_client/fallen_state"
+ to="/darwin/robot/fallen_state"/>
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
diff --git a/darwin_apps/launch/walk_client.launch b/darwin_apps/launch/walk_client.launch
index 3defde4e0882102683bc12844001219fe3b7f6c4..14947a0d3c8a09093f6bc1c9066aabfe760a9a79 100644
--- a/darwin_apps/launch/walk_client.launch
+++ b/darwin_apps/launch/walk_client.launch
@@ -29,12 +29,18 @@
<param name="FOOT_HEIGHT" value="0.04"/>
<param name="MAX_VEL" value="0.01"/>
<param name="MAX_ROT_VEL" value="0.01"/>
- <remap from="/darwin/walk_client/cmd_vel"
+ <remap from="/darwin/walk_client/walk_client/cmd_vel"
to="/darwin/robot/cmd_vel"/>
- <remap from="/darwin/walk_client/set_walk_params"
+ <remap from="/darwin/walk_client/walk_client/set_walk_params"
to="/darwin/robot/set_walk_params"/>
- <remap from="/darwin/walk_client/set_servo_modules"
+ <remap from="/darwin/walk_client/walk_client/get_walk_params"
+ to="/darwin/robot/get_walk_params"/>
+ <remap from="/darwin/walk_client/walk_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/walk_client/walk_client/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/walk_client/walk_client/fallen_state"
+ to="/darwin/robot/fallen_state"/>
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_apps/launch/walk_client_sim.launch b/darwin_apps/launch/walk_client_sim.launch
index 800df6d1b0c4de168b7df202e3fc52e0c1d4b9dd..16e47a033687821589b5aec737eb2378f43a6dbd 100644
--- a/darwin_apps/launch/walk_client_sim.launch
+++ b/darwin_apps/launch/walk_client_sim.launch
@@ -29,16 +29,18 @@
<param name="FOOT_HEIGHT" value="0.04"/>
<param name="MAX_VEL" value="0.01"/>
<param name="MAX_ROT_VEL" value="0.01"/>
- <remap from="/darwin/walk_client/walk_client/cmd_vel"
+ <remap from="/walk_client/cmd_vel"
to="/darwin/robot/cmd_vel"/>
- <remap from="/darwin/walk_client/walk_client/set_walk_params"
+ <remap from="/darwin/walk_client/set_walk_params"
to="/darwin/robot/set_walk_params"/>
- <remap from="/darwin/walk_client/walk_client/get_walk_params"
+ <remap from="/darwin/walk_client/get_walk_params"
to="/darwin/robot/get_walk_params"/>
- <remap from="/darwin/walk_client/walk_client/set_servo_modules"
+ <remap from="/darwin/walk_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
- <remap from="/darwin/walk_client/walk_client/joint_states"
+ <remap from="/walk_client/joint_states"
to="/darwin/joint_states"/>
+ <remap from="/walk_client/fallen_state"
+ to="/darwin/robot/fallen_state"/>
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_arm_kinematics/config/left_arm_sc.xml b/darwin_arm_kinematics/config/left_arm_sc.xml
new file mode 100644
index 0000000000000000000000000000000000000000..e6bb35ed391fe0ac8e0b6dfac506928dbb2efc5a
--- /dev/null
+++ b/darwin_arm_kinematics/config/left_arm_sc.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+
+<darwin_kin_chain xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="darwin_kin_chain.xsd">
+ <joint>
+ <name>j_shoulder_pitch_l</name>
+ <params>
+ <a>-0.016</a>
+ <alpha>1.5707</alpha>
+ <d>0.02165</d>
+ <theta>3.14159</theta>
+ </params>
+ <max_angle>1.5707</max_angle>
+ <min_angle>-1.5707</min_angle>
+ </joint>
+ <joint>
+ <name>j_shoulder_roll_l</name>
+ <params>
+ <a>-0.06</a>
+ <alpha>1.5707</alpha>
+ <d>-0.016</d>
+ <theta>-0.7854</theta>
+ </params>
+ <max_angle>1.5707</max_angle>
+ <min_angle>-1.5707</min_angle>
+ </joint>
+ <joint>
+ <name>j_elbow_l</name>
+ <params>
+ <a>-0.095</a>
+ <alpha>1.5707</alpha>
+ <d>0</d>
+ <theta>1.5707</theta>
+ </params>
+ <max_angle>1.5707</max_angle>
+ <min_angle>-1.5707</min_angle>
+ </joint>
+</darwin_kin_chain>
diff --git a/darwin_arm_kinematics/config/right_arm_sc.xml b/darwin_arm_kinematics/config/right_arm_sc.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9532914328a76786a7026c8a00bbf93745f85f0b
--- /dev/null
+++ b/darwin_arm_kinematics/config/right_arm_sc.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+
+<darwin_kin_chain xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="darwin_kin_chain.xsd">
+ <joint>
+ <name>j_shoulder_pitch_l</name>
+ <params>
+ <a>-0.016</a>
+ <alpha>-1.5707</alpha>
+ <d>0.02165</d>
+ <theta>3.14159</theta>
+ </params>
+ <max_angle>1.5707</max_angle>
+ <min_angle>-1.5707</min_angle>
+ </joint>
+ <joint>
+ <name>j_shoulder_roll_l</name>
+ <params>
+ <a>-0.06</a>
+ <alpha>1.5707</alpha>
+ <d>-0.016</d>
+ <theta>0.7854</theta>
+ </params>
+ <max_angle>1.5707</max_angle>
+ <min_angle>-1.5707</min_angle>
+ </joint>
+ <joint>
+ <name>j_elbow_l</name>
+ <params>
+ <a>-0.095</a>
+ <alpha>1.5707</alpha>
+ <d>0</d>
+ <theta>1.5707</theta>
+ </params>
+ <max_angle>1.5707</max_angle>
+ <min_angle>-1.5707</min_angle>
+ </joint>
+</darwin_kin_chain>
diff --git a/darwin_arm_kinematics/launch/left_arm_kin_test.launch b/darwin_arm_kinematics/launch/left_arm_kin_test.launch
index f399a016e0d2eca6f4d198b8fef8351745d2e91b..e75ab51f72ea32006662941bb51078dfb21a433a 100644
--- a/darwin_arm_kinematics/launch/left_arm_kin_test.launch
+++ b/darwin_arm_kinematics/launch/left_arm_kin_test.launch
@@ -4,7 +4,7 @@
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/$(arg robot).xacro'" />
+ command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/$(arg robot).xacro'" />
<!-- load the controllers -->
<node name="left_arm_kinematics"
diff --git a/darwin_control/config/darwin_control.yaml b/darwin_control/config/darwin.yaml
similarity index 100%
rename from darwin_control/config/darwin_control.yaml
rename to darwin_control/config/darwin.yaml
diff --git a/darwin_control/config/darwin_ceabot.yaml b/darwin_control/config/darwin_ceabot.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..37ead9a1fc24a58ecc67a270435da0813f03991e
--- /dev/null
+++ b/darwin_control/config/darwin_ceabot.yaml
@@ -0,0 +1,133 @@
+darwin:
+ # Publish all joint states -----------------------------------
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 50
+
+ darwin_controller:
+ left_sensor_foot_present: true
+ right_sensor_foot_present: true
+ type: effort_controllers/DarwinController
+ joints:
+ - j_shoulder_pitch_r
+ - j_shoulder_pitch_l
+ - j_shoulder_roll_r
+ - j_shoulder_roll_l
+ - j_elbow_r
+ - j_elbow_l
+ - j_hip_yaw_r
+ - j_hip_yaw_l
+ - j_hip_roll_r
+ - j_hip_roll_l
+ - j_hip_pitch_r
+ - j_hip_pitch_l
+ - j_knee_r
+ - j_knee_l
+ - j_ankle_pitch_r
+ - j_ankle_pitch_l
+ - j_ankle_roll_r
+ - j_ankle_roll_l
+ - j_pan
+ - j_tilt
+ gains:
+ j_shoulder_pitch_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_shoulder_roll_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_elbow_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_shoulder_pitch_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_shoulder_roll_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_elbow_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_yaw_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_roll_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_pitch_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_knee_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_pitch_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_roll_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_yaw_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_roll_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_pitch_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_knee_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_pitch_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_roll_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_pan:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_tilt:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+
diff --git a/darwin_control/config/darwin_cpp.yaml b/darwin_control/config/darwin_cpp.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2d1b7e9288c658dc2770dacf79b9005969584275
--- /dev/null
+++ b/darwin_control/config/darwin_cpp.yaml
@@ -0,0 +1,133 @@
+darwin:
+ # Publish all joint states -----------------------------------
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 50
+
+ darwin_controller_cpp:
+ dyn_serial_device: '/dev/pts/11'
+ imu_serial_device: '/dev/pts/25'
+ type: effort_controllers/DarwinControllerCPP
+ joints:
+ - j_shoulder_pitch_r
+ - j_shoulder_pitch_l
+ - j_shoulder_roll_r
+ - j_shoulder_roll_l
+ - j_elbow_r
+ - j_elbow_l
+ - j_hip_yaw_r
+ - j_hip_yaw_l
+ - j_hip_roll_r
+ - j_hip_roll_l
+ - j_hip_pitch_r
+ - j_hip_pitch_l
+ - j_knee_r
+ - j_knee_l
+ - j_ankle_pitch_r
+ - j_ankle_pitch_l
+ - j_ankle_roll_r
+ - j_ankle_roll_l
+ - j_pan
+ - j_tilt
+ gains:
+ j_shoulder_pitch_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_shoulder_roll_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_elbow_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_shoulder_pitch_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_shoulder_roll_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_elbow_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_yaw_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_roll_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_pitch_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_knee_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_pitch_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_roll_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_yaw_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_roll_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_pitch_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_knee_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_pitch_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_roll_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_pan:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_tilt:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+
diff --git a/darwin_control/config/darwin_sc.yaml b/darwin_control/config/darwin_sc.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4c201c43823e95f9d2d293688c27f46ff57ba861
--- /dev/null
+++ b/darwin_control/config/darwin_sc.yaml
@@ -0,0 +1,160 @@
+darwin:
+ # Publish all joint states -----------------------------------
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 50
+
+ darwin_controller:
+ use_smart_charger: true
+ initial_charge: 2200
+ discharge_rate: 6000
+ charge_current: 2200
+ charge_station_link: station_connector_link
+ charge_robot_link: r_gripper_bot
+ type: effort_controllers/DarwinController
+ joints:
+ - j_shoulder_pitch_r
+ - j_shoulder_pitch_l
+ - j_shoulder_roll_r
+ - j_shoulder_roll_l
+ - j_elbow_r
+ - j_elbow_l
+ - j_hip_yaw_r
+ - j_hip_yaw_l
+ - j_hip_roll_r
+ - j_hip_roll_l
+ - j_hip_pitch_r
+ - j_hip_pitch_l
+ - j_knee_r
+ - j_knee_l
+ - j_ankle_pitch_r
+ - j_ankle_pitch_l
+ - j_ankle_roll_r
+ - j_ankle_roll_l
+ - j_pan
+ - j_tilt
+ - j_gripper_top_l
+ - j_gripper_bot_l
+ - j_gripper_top_r
+ - j_gripper_bot_r
+ gains:
+ j_shoulder_pitch_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_shoulder_roll_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_elbow_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_shoulder_pitch_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_shoulder_roll_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_elbow_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_yaw_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_roll_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_pitch_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_knee_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_pitch_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_roll_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_yaw_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_roll_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_hip_pitch_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_knee_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_pitch_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_ankle_roll_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_pan:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_tilt:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_gripper_top_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_gripper_bot_l:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_gripper_top_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
+ j_gripper_bot_r:
+ p: 16.0
+ d: 0.0
+ i: 0.0
+ i_clamp: 100
diff --git a/darwin_control/launch/darwin_control.launch b/darwin_control/launch/darwin_ceabot_control.launch
similarity index 83%
rename from darwin_control/launch/darwin_control.launch
rename to darwin_control/launch/darwin_ceabot_control.launch
index 4de395f8f72f8b346849c2a2f3725188e612a799..dbc31ed935ff261b451e93f0729aba8b82a0d178 100644
--- a/darwin_control/launch/darwin_control.launch
+++ b/darwin_control/launch/darwin_ceabot_control.launch
@@ -1,9 +1,10 @@
<launch>
<arg name="config_file" default="darwin_config.xml"/>
+ <arg name="robot" default="darwin"/>
<!-- Load joint controller configurations from YAML file to parameter server -->
- <rosparam file="$(find darwin_control)/config/darwin_control.yaml" command="load"/>
+ <rosparam file="$(find darwin_control)/config/$(arg robot).yaml" command="load"/>
<param name="motion_file" value="$(find darwin_controller)/config/motion_1024.bin"/>
<param name="config_file" value="$(find darwin_controller)/src/xml/$(arg config_file)"/>
diff --git a/darwin_control/launch/darwin_cpp_control.launch b/darwin_control/launch/darwin_cpp_control.launch
new file mode 100644
index 0000000000000000000000000000000000000000..1481ebd360c9214b05390ae5827d2a1873a2059e
--- /dev/null
+++ b/darwin_control/launch/darwin_cpp_control.launch
@@ -0,0 +1,22 @@
+<launch>
+
+ <arg name="config_file" default="darwin_config.xml"/>
+ <arg name="robot" default="darwin"/>
+
+ <!-- Load joint controller configurations from YAML file to parameter server -->
+ <rosparam file="$(find darwin_control)/config/$(arg robot).yaml" command="load"/>
+
+ <param name="motion_file" value="$(find darwin_controller_cpp)/config/motion_1024.bin"/>
+ <param name="config_file" value="$(find darwin_controller_cpp)/src/xml/$(arg config_file)"/>
+
+ <!-- load the controllers -->
+ <node name="controller_spawner"
+ pkg="controller_manager"
+ type="spawner"
+ respawn="false"
+ output="screen"
+ ns="/darwin"
+ args="darwin_controller_cpp joint_state_controller">
+ </node>
+</launch>
+
diff --git a/darwin_control/launch/darwin_sc_control.launch b/darwin_control/launch/darwin_sc_control.launch
new file mode 100644
index 0000000000000000000000000000000000000000..dbc31ed935ff261b451e93f0729aba8b82a0d178
--- /dev/null
+++ b/darwin_control/launch/darwin_sc_control.launch
@@ -0,0 +1,22 @@
+<launch>
+
+ <arg name="config_file" default="darwin_config.xml"/>
+ <arg name="robot" default="darwin"/>
+
+ <!-- Load joint controller configurations from YAML file to parameter server -->
+ <rosparam file="$(find darwin_control)/config/$(arg robot).yaml" command="load"/>
+
+ <param name="motion_file" value="$(find darwin_controller)/config/motion_1024.bin"/>
+ <param name="config_file" value="$(find darwin_controller)/src/xml/$(arg config_file)"/>
+
+ <!-- load the controllers -->
+ <node name="controller_spawner"
+ pkg="controller_manager"
+ type="spawner"
+ respawn="false"
+ output="screen"
+ ns="/darwin"
+ args="darwin_controller joint_state_controller">
+ </node>
+</launch>
+
diff --git a/darwin_controller/CMakeLists.txt b/darwin_controller/CMakeLists.txt
index be33c40c68fdb84b26037da20eeff03aa60ab770..48c2d895dcd483eb349e026107c7389790a30242 100644
--- a/darwin_controller/CMakeLists.txt
+++ b/darwin_controller/CMakeLists.txt
@@ -4,11 +4,12 @@ project(darwin_controller)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
-find_package(catkin REQUIRED roscpp urdf controller_interface hardware_interface controller_manager actionlib_msgs iri_action_server geometry_msgs sensor_msgs control_msgs trajectory_msgs humanoid_common_msgs)
+find_package(catkin REQUIRED roscpp urdf controller_interface hardware_interface controller_manager actionlib_msgs iri_action_server geometry_msgs sensor_msgs control_msgs trajectory_msgs humanoid_common_msgs gazebo_ros humanoid_modules)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
FIND_PACKAGE(iriutils REQUIRED)
+find_package(gazebo REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
@@ -78,7 +79,7 @@ FIND_PACKAGE(iriutils REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
- CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager actionlib_msgs iri_action_server geometry_msgs sensor_msgs control_msgs trajectory_msgs humanoid_common_msgs
+ CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager actionlib_msgs iri_action_server geometry_msgs sensor_msgs control_msgs trajectory_msgs humanoid_common_msgs gazebo_ros humanoid_modules
# DEPENDS system_lib
)
@@ -86,6 +87,9 @@ catkin_package(
## Build ##
###########
+link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
SET(DARWIN_FW_PATH ~/humanoids/darwin_stm32_fw)
SET(STM32_HAL_PATH ~/humanoids/stm32_hal)
SET(STM32_LIBRARIES_PATH ~/humanoids/stm32_libraries)
@@ -101,6 +105,7 @@ include_directories(${STM32_LIBRARIES_PATH}/utils/include)
include_directories(${STM32_HAL_PATH}/f1/include)
include_directories(${STM32_HAL_PATH}/f1/include/core)
include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${GAZEBO_INCLUDE_DIRS})
add_definitions(-DSTM32F103xE)
add_definitions(-DUSE_HAL_DRIVER)
@@ -121,6 +126,8 @@ add_library(${PROJECT_NAME} src/darwin_controller.cpp
${DARWIN_FW_PATH}/src/motion_manager.c
${DARWIN_FW_PATH}/src/smart_charger.c
${DARWIN_FW_PATH}/src/grippers.c
+ ${DARWIN_FW_PATH}/src/gpio.c
+ ${DARWIN_FW_PATH}/src/stairs.c
src/sim/stm32_hal.c
src/sim/stm32_cortex.c
src/sim/stm32_gpio.c
@@ -147,6 +154,8 @@ add_dependencies(darwin_controller humanoid_common_msgs_generate_messages_cpp)
# )
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
+
#############
## Install ##
#############
diff --git a/darwin_controller/include/darwin_controller.h b/darwin_controller/include/darwin_controller.h
index a6081df9908e0abf4d1d73d5f177e122055f1377..84345adc2844422d5605ac4c2cdaa40f469806e6 100644
--- a/darwin_controller/include/darwin_controller.h
+++ b/darwin_controller/include/darwin_controller.h
@@ -43,11 +43,12 @@
// ROS
#include <ros/node_handle.h>
-#include "std_msgs/Int32.h"
-#include "sensor_msgs/Imu.h"
-#include "geometry_msgs/Twist.h"
-#include "trajectory_msgs/JointTrajectoryPoint.h"
-#include "control_msgs/JointTrajectoryAction.h"
+#include <std_msgs/Int8.h>
+#include <sensor_msgs/Imu.h>
+#include "sensor_msgs/Range.h"
+#include <geometry_msgs/Twist.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include <control_msgs/JointTrajectoryAction.h>
// URDF
#include <urdf/model.h>
@@ -59,13 +60,26 @@
#include <iri_action_server/iri_action_server.h>
#include <humanoid_common_msgs/humanoid_motionAction.h>
+#include <humanoid_common_msgs/humanoid_gripperAction.h>
#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
+#include <humanoid_common_msgs/humanoid_stairsAction.h>
#include <humanoid_common_msgs/set_walk_params.h>
#include <humanoid_common_msgs/get_walk_params.h>
+#include <humanoid_common_msgs/set_stairs_params.h>
+#include <humanoid_common_msgs/get_stairs_params.h>
#include <humanoid_common_msgs/set_servo_modules.h>
#include <humanoid_common_msgs/set_pid.h>
#include <humanoid_common_msgs/get_pid.h>
#include <humanoid_common_msgs/leds.h>
+#include <humanoid_common_msgs/get_smart_charger_config.h>
+#include <humanoid_common_msgs/set_smart_charger_config.h>
+#include <humanoid_common_msgs/smart_charger_data.h>
+#include <humanoid_common_msgs/ir_foot_data.h>
+#include <humanoid_modules/humanoid_module.h>
+
+#include <gazebo/transport/transport.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/gazebo.hh>
#include "mutex.h"
@@ -123,11 +137,39 @@ namespace darwin_controller
void follow_target_getResultCallback(humanoid_common_msgs::humanoid_follow_targetResultPtr& result);
void follow_target_getFeedbackCallback(humanoid_common_msgs::humanoid_follow_targetFeedbackPtr& feedback);
+ IriActionServer<humanoid_common_msgs::humanoid_gripperAction> *left_gripper_aserver_;
+ void left_gripper_startCallback(const humanoid_common_msgs::humanoid_gripperGoalConstPtr& goal);
+ void left_gripper_stopCallback(void);
+ bool left_gripper_isFinishedCallback(void);
+ bool left_gripper_hasSucceedCallback(void);
+ void left_gripper_getResultCallback(humanoid_common_msgs::humanoid_gripperResultPtr& result);
+ void left_gripper_getFeedbackCallback(humanoid_common_msgs::humanoid_gripperFeedbackPtr& feedback);
+
+ IriActionServer<humanoid_common_msgs::humanoid_gripperAction> *right_gripper_aserver_;
+ void right_gripper_startCallback(const humanoid_common_msgs::humanoid_gripperGoalConstPtr& goal);
+ void right_gripper_stopCallback(void);
+ bool right_gripper_isFinishedCallback(void);
+ bool right_gripper_hasSucceedCallback(void);
+ void right_gripper_getResultCallback(humanoid_common_msgs::humanoid_gripperResultPtr& result);
+ void right_gripper_getFeedbackCallback(humanoid_common_msgs::humanoid_gripperFeedbackPtr& feedback);
+
+ IriActionServer<humanoid_common_msgs::humanoid_stairsAction> *stairs_aserver_;
+ void stairs_startCallback(const humanoid_common_msgs::humanoid_stairsGoalConstPtr& goal);
+ void stairs_stopCallback(void);
+ bool stairs_isFinishedCallback(void);
+ bool stairs_hasSucceedCallback(void);
+ void stairs_getResultCallback(humanoid_common_msgs::humanoid_stairsResultPtr& result);
+ void stairs_getFeedbackCallback(humanoid_common_msgs::humanoid_stairsFeedbackPtr& feedback);
+
// service servers
ros::ServiceServer set_walk_params_server_;
bool set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res);
ros::ServiceServer get_walk_params_server_;
bool get_walk_paramsCallback(humanoid_common_msgs::get_walk_params::Request &req, humanoid_common_msgs::get_walk_params::Response &res);
+ ros::ServiceServer set_stairs_params_server_;
+ bool set_stairs_paramsCallback(humanoid_common_msgs::set_stairs_params::Request &req, humanoid_common_msgs::set_stairs_params::Response &res);
+ ros::ServiceServer get_stairs_params_server_;
+ bool get_stairs_paramsCallback(humanoid_common_msgs::get_stairs_params::Request &req, humanoid_common_msgs::get_stairs_params::Response &res);
ros::ServiceServer set_servo_modules_server_;
bool set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res);
ros::ServiceServer get_pan_pid_server_;
@@ -140,8 +182,11 @@ namespace darwin_controller
bool set_tilt_pidCallback(humanoid_common_msgs::set_pid::Request &req, humanoid_common_msgs::set_pid::Response &res);
// topic subscribers
- ros::Subscriber imu_sub;
- void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
+ ros::Subscriber gyro_sub;
+ void gyro_callback(const sensor_msgs::Imu::ConstPtr& msg);
+
+ ros::Subscriber accel_sub;
+ void accel_callback(const sensor_msgs::Imu::ConstPtr& msg);
ros::Subscriber cmd_vel_sub;
void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
@@ -154,7 +199,48 @@ namespace darwin_controller
ros::Subscriber leds_subscriber_;
void leds_callback(const humanoid_common_msgs::leds::ConstPtr& msg);
+ ros::Subscriber range_sub;
+ void range_callback(const sensor_msgs::Range::ConstPtr& msg);
+
CMutex walk_access;
+
+ // feet sensors publihsers
+ ros::Publisher left_foot_sensor_data_publisher_;
+ humanoid_common_msgs::ir_foot_data left_foot_sensor_data_msg_;
+ bool left_sensor_foot_present;
+
+ ros::Publisher right_foot_sensor_data_publisher_;
+ humanoid_common_msgs::ir_foot_data right_foot_sensor_data_msg_;
+ bool right_sensor_foot_present;
+
+ // fall detection publisher
+ ros::Publisher fallen_state_publisher_;
+ std_msgs::Int8 fallen_state_msg_;
+
+ // smart charger attributes
+ // topic publihsers
+ ros::Publisher smart_charger_data_publisher_;
+ humanoid_common_msgs::smart_charger_data smart_charger_data_smart_charger_data_msg_;
+ // service servers
+ ros::ServiceServer get_smart_charger_config_server_;
+ bool get_smart_charger_configCallback(humanoid_common_msgs::get_smart_charger_config::Request &req, humanoid_common_msgs::get_smart_charger_config::Response &res);
+ ros::ServiceServer set_smart_charger_config_server_;
+ bool set_smart_charger_configCallback(humanoid_common_msgs::set_smart_charger_config::Request &req, humanoid_common_msgs::set_smart_charger_config::Response &res);
+ humanoid_common_msgs::smart_charger_config sc_config;
+ // internal attributes
+ bool use_smart_charger;
+ double initial_charge;
+ double current_charge;
+ double discharge_rate;
+ double charge_current;
+ std::string charge_station_link;
+ std::string charge_robot_link;
+ bool charger_connected;
+ bool charger_enabled;
+ // gazebo interface for contacts
+ gazebo::transport::NodePtr gazebo_node;
+ gazebo::transport::SubscriberPtr contacts_subs;
+ void OnContacts(ConstContactsPtr &msg);
};
} // namespace
diff --git a/darwin_controller/include/darwin_controller_impl.h b/darwin_controller/include/darwin_controller_impl.h
index f3c9975d4356abd0a6228badaaf366be57c74ad8..35648137c45521209a7e38840f397f4ecfd01613 100644
--- a/darwin_controller/include/darwin_controller_impl.h
+++ b/darwin_controller/include/darwin_controller_impl.h
@@ -8,9 +8,11 @@
#include "walking.h"
#include "joint_motion.h"
#include "head_tracking.h"
+#include "grippers.h"
#include "ram.h"
#include "imu.h"
#include "darwin_math.h"
+#include "stairs.h"
#ifdef _HAVE_XSD
#include "../src/xml/darwin_config.hxx"
#endif
@@ -29,38 +31,15 @@ extern TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
//IMU module external variables
extern int32_t gyro_data[3];
+extern int32_t accel_data[3];
-const std::string servo_names[MANAGER_MAX_NUM_SERVOS]={std::string("Servo0"),
- std::string("j_shoulder_pitch_r"),
- std::string("j_shoulder_pitch_l"),
- std::string("j_shoulder_roll_r"),
- std::string("j_shoulder_roll_l"),
- std::string("j_elbow_r"),
- std::string("j_elbow_l"),
- std::string("j_hip_yaw_r"),
- std::string("j_hip_yaw_l"),
- std::string("j_hip_roll_r"),
- std::string("j_hip_roll_l"),
- std::string("j_hip_pitch_r"),
- std::string("j_hip_pitch_l"),
- std::string("j_knee_r"),
- std::string("j_knee_l"),
- std::string("j_ankle_pitch_r"),
- std::string("j_ankle_pitch_l"),
- std::string("j_ankle_roll_r"),
- std::string("j_ankle_roll_l"),
- std::string("j_pan"),
- std::string("j_tilt"),
- std::string("Servo21"),
- std::string("Servo22"),
- std::string("Servo23"),
- std::string("Servo24"),
- std::string("Servo25"),
- std::string("Servo26"),
- std::string("Servo27"),
- std::string("Servo28"),
- std::string("Servo29"),
- std::string("Servo30")};
+//parameters default values
+#define DEFAULT_ENABLE_SMART_CHARGER false
+#define DEFAULT_INITIAL_CHARGE 2200.0
+#define DEFAULT_DISCHARGE_RATE 6000.0
+#define DEFAULT_CHARGE_CURRENT 2200.0
+#define DEFAULT_CHARGE_STATION_LINK "none"
+#define DEFAULT_CHARGE_ROBOT_LINK "none"
namespace darwin_controller
{
@@ -178,6 +157,47 @@ namespace darwin_controller
// Cache controller node handle
controller_nh_ = controller_nh;
+ // smart charger interface
+ this->use_smart_charger=DEFAULT_ENABLE_SMART_CHARGER;
+ controller_nh_.getParam("use_smart_charger", this->use_smart_charger);
+ ROS_DEBUG_STREAM_NAMED(name_, "Smart charger is: " << this->use_smart_charger);
+ if(this->use_smart_charger)
+ {
+ this->sc_config.enable=false;
+ this->sc_config.disable=false;
+ this->sc_config.period=1.500;
+ this->sc_config.current=4.000;
+ // initialize gazebo interfaces
+ this->gazebo_node=gazebo::transport::NodePtr(new gazebo::transport::Node());
+ this->gazebo_node->Init();
+ this->contacts_subs=this->gazebo_node->Subscribe("/gazebo/default/physics/contacts",&DarwinController<HardwareInterface>::OnContacts,this);
+ // initialize publisher
+ this->smart_charger_data_publisher_ = root_nh.advertise<humanoid_common_msgs::smart_charger_data>("robot/smart_charger_data", 1);
+ // initialize service servers
+ this->get_smart_charger_config_server_=root_nh.advertiseService("robot/get_smart_charger_config", &DarwinController<HardwareInterface>::get_smart_charger_configCallback, this);
+ this->set_smart_charger_config_server_=root_nh.advertiseService("robot/set_smart_charger_config", &DarwinController<HardwareInterface>::set_smart_charger_configCallback, this);
+ // read parameters
+ this->initial_charge=DEFAULT_INITIAL_CHARGE;
+ controller_nh_.getParam("initial_charge", this->initial_charge);
+ ROS_DEBUG_STREAM_NAMED(name_, "Initial charge set to: " << this->initial_charge);
+ this->current_charge=this->initial_charge;
+ this->discharge_rate=DEFAULT_DISCHARGE_RATE;
+ controller_nh_.getParam("discharge_rate", this->discharge_rate);
+ ROS_DEBUG_STREAM_NAMED(name_, "Discharge rate set to: " << this->discharge_rate);
+ this->charge_current=DEFAULT_CHARGE_CURRENT;
+ controller_nh_.getParam("charge_current", this->charge_current);
+ ROS_DEBUG_STREAM_NAMED(name_, "Charge current set to: " << this->charge_current);
+ this->charge_station_link=DEFAULT_CHARGE_STATION_LINK;
+ controller_nh_.getParam("charge_station_link", this->charge_station_link);
+ ROS_DEBUG_STREAM_NAMED(name_, "Charger station link set to: " << this->charge_station_link);
+ this->charge_robot_link=DEFAULT_CHARGE_ROBOT_LINK;
+ controller_nh_.getParam("charge_robot_link", this->charge_robot_link);
+ ROS_DEBUG_STREAM_NAMED(name_, "Charger robot link set to: " << this->charge_robot_link);
+
+ this->charger_connected=false;
+ this->charger_enabled=false;
+ }
+
/* initialize actions */
this->motion_action_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_motionAction>(root_nh,"robot/motion_action");
this->motion_action_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_startCallback, this, _1));
@@ -206,20 +226,97 @@ namespace darwin_controller
this->follow_target_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_getFeedbackCallback, this, _1));
this->follow_target_aserver_->start();
+ this->left_gripper_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_gripperAction>(root_nh,"robot/left_gripper");
+ this->left_gripper_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::left_gripper_startCallback, this, _1));
+ this->left_gripper_aserver_->registerStopCallback(boost::bind(& DarwinController<HardwareInterface>::left_gripper_stopCallback, this));
+ this->left_gripper_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::left_gripper_isFinishedCallback, this));
+ this->left_gripper_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::left_gripper_hasSucceedCallback, this));
+ this->left_gripper_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::left_gripper_getResultCallback, this, _1));
+ this->left_gripper_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::left_gripper_getFeedbackCallback, this, _1));
+ this->left_gripper_aserver_->start();
+
+ this->right_gripper_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_gripperAction>(root_nh,"robot/right_gripper");
+ this->right_gripper_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::right_gripper_startCallback, this, _1));
+ this->right_gripper_aserver_->registerStopCallback(boost::bind(& DarwinController<HardwareInterface>::right_gripper_stopCallback, this));
+ this->right_gripper_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::right_gripper_isFinishedCallback, this));
+ this->right_gripper_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::right_gripper_hasSucceedCallback, this));
+ this->right_gripper_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::right_gripper_getResultCallback, this, _1));
+ this->right_gripper_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::right_gripper_getFeedbackCallback, this, _1));
+ this->right_gripper_aserver_->start();
+
+ this->stairs_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_stairsAction>(root_nh,"robot/climb_stairs");
+ this->stairs_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_startCallback, this, _1));
+ this->stairs_aserver_->registerStopCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_stopCallback, this));
+ this->stairs_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_isFinishedCallback, this));
+ this->stairs_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_hasSucceedCallback, this));
+ this->stairs_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_getResultCallback, this, _1));
+ this->stairs_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_getFeedbackCallback, this, _1));
+ this->stairs_aserver_->start();
+
/* initialize services */
this->set_servo_modules_server_=root_nh.advertiseService("robot/set_servo_modules", &DarwinController<HardwareInterface>::set_servo_modulesCallback, this);
this->set_walk_params_server_=root_nh.advertiseService("robot/set_walk_params", &DarwinController<HardwareInterface>::set_walk_paramsCallback, this);
this->get_walk_params_server_=root_nh.advertiseService("robot/get_walk_params", &DarwinController<HardwareInterface>::get_walk_paramsCallback, this);
+ this->set_stairs_params_server_=root_nh.advertiseService("robot/set_stairs_params", &DarwinController<HardwareInterface>::set_stairs_paramsCallback, this);
+ this->get_stairs_params_server_=root_nh.advertiseService("robot/get_stairs_params", &DarwinController<HardwareInterface>::get_stairs_paramsCallback, this);
this->set_pan_pid_server_=root_nh.advertiseService("robot/set_pan_pid", &DarwinController<HardwareInterface>::set_pan_pidCallback, this);
this->get_pan_pid_server_=root_nh.advertiseService("robot/get_pan_pid", &DarwinController<HardwareInterface>::get_pan_pidCallback, this);
this->set_tilt_pid_server_=root_nh.advertiseService("robot/set_tilt_pid", &DarwinController<HardwareInterface>::set_tilt_pidCallback, this);
this->get_tilt_pid_server_=root_nh.advertiseService("robot/get_tilt_pid", &DarwinController<HardwareInterface>::get_tilt_pidCallback, this);
// initialize topics
- this->imu_sub=root_nh.subscribe("sensors/raw_gyro", 1, &DarwinController<HardwareInterface>::imu_callback,this);
+ this->gyro_sub=root_nh.subscribe("sensors/raw_gyro", 1, &DarwinController<HardwareInterface>::gyro_callback,this);
+ this->accel_sub=root_nh.subscribe("sensors/raw_accel", 1, &DarwinController<HardwareInterface>::accel_callback,this);
this->cmd_vel_sub=root_nh.subscribe("robot/cmd_vel", 1, &DarwinController<HardwareInterface>::cmd_vel_callback,this);
this->head_target_sub=root_nh.subscribe("robot/head_target", 1, &DarwinController<HardwareInterface>::head_target_callback,this);
+ // initialize publisher
+ this->fallen_state_publisher_ = root_nh.advertise<std_msgs::Int8>("robot/fallen_state", 1);
+
+ this->left_sensor_foot_present=false;
+ controller_nh_.getParam("left_sensor_foot_present", this->left_sensor_foot_present);
+ ROS_DEBUG_STREAM_NAMED(name_, "left sensor foot is: " << this->left_sensor_foot_present);
+ if(this->left_sensor_foot_present)
+ {
+ this->left_foot_sensor_data_publisher_ = root_nh.advertise<humanoid_common_msgs::ir_foot_data>("sensors/left_foot_data",1);
+ this->left_foot_sensor_data_msg_.names.resize(10);
+ this->left_foot_sensor_data_msg_.names[0]="down_left_middle";
+ this->left_foot_sensor_data_msg_.names[1]="down_left_rear";
+ this->left_foot_sensor_data_msg_.names[2]="down_analog";
+ this->left_foot_sensor_data_msg_.names[3]="down_left_front";
+ this->left_foot_sensor_data_msg_.names[4]="down_right_rear";
+ this->left_foot_sensor_data_msg_.names[5]="down_right_middle";
+ this->left_foot_sensor_data_msg_.names[6]="down_right_front";
+ this->left_foot_sensor_data_msg_.names[7]="front_left";
+ this->left_foot_sensor_data_msg_.names[8]="front_right";
+ this->left_foot_sensor_data_msg_.names[9]="front_analog";
+ this->left_foot_sensor_data_msg_.voltages.resize(10);
+ this->left_foot_sensor_data_msg_.status.resize(10);
+ }
+
+ this->right_sensor_foot_present=false;
+ controller_nh_.getParam("right_sensor_foot_present", this->right_sensor_foot_present);
+ ROS_DEBUG_STREAM_NAMED(name_, "right sensor foot is: " << this->right_sensor_foot_present);
+ if(this->right_sensor_foot_present)
+ {
+ this->right_foot_sensor_data_publisher_ = root_nh.advertise<humanoid_common_msgs::ir_foot_data>("sensors/right_foot_data",1);
+ this->right_foot_sensor_data_msg_.names.resize(10);
+ this->right_foot_sensor_data_msg_.names[0]="down_left_middle";
+ this->right_foot_sensor_data_msg_.names[1]="down_left_rear";
+ this->right_foot_sensor_data_msg_.names[2]="down_analog";
+ this->right_foot_sensor_data_msg_.names[3]="down_left_front";
+ this->right_foot_sensor_data_msg_.names[4]="down_right_rear";
+ this->right_foot_sensor_data_msg_.names[5]="down_right_middle";
+ this->right_foot_sensor_data_msg_.names[6]="down_right_front";
+ this->right_foot_sensor_data_msg_.names[7]="front_left";
+ this->right_foot_sensor_data_msg_.names[8]="front_right";
+ this->right_foot_sensor_data_msg_.names[9]="front_analog";
+ this->right_foot_sensor_data_msg_.voltages.resize(10);
+ this->right_foot_sensor_data_msg_.status.resize(10);
+ }
+ if(this->right_sensor_foot_present || this->right_sensor_foot_present)
+ this->range_sub=root_nh.subscribe("sensors/range", 1, &DarwinController<HardwareInterface>::range_callback,this);
+
// Controller name
name_=getLeafNamespace(controller_nh_);
@@ -282,11 +379,16 @@ namespace darwin_controller
manager_init(7800);// motion manager period in us
ROS_INFO("CDarwinSim : motion manager initialized");
/* go to the walk ready position */
- for(unsigned int i=0;i<n_joints;++i)
+ for(unsigned int i=0;i<20;++i)
{
manager_set_module(i+1,MM_ACTION);
manager_enable_servo(i+1);
}
+ for(unsigned int i=20;i<n_joints;++i)
+ {
+ manager_set_module(i+1,MM_JOINTS);
+ manager_enable_servo(i+1);
+ }
manager_enable_balance();// enable balance
action_load_page(WALK_READY);
action_start_page();
@@ -311,11 +413,74 @@ namespace darwin_controller
for (unsigned int i = 0; i < this->joints_.size(); ++i)
{
target_angles[i] = ((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0;
- const double command = pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
+ double command = pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
this->joints_[i].setCommand(command);
}
if(is_walking() && ((current_time-this->walk_timeout).toSec()>1.0))
walking_stop();
+ if(this->use_smart_charger)
+ {
+ if(this->charger_enabled)
+ {
+ this->smart_charger_data_smart_charger_data_msg_.header.stamp=ros::Time::now();
+ if(this->charger_connected)
+ {
+ this->smart_charger_data_smart_charger_data_msg_.avg_time_empty=(uint16_t)-1;
+ this->current_charge+=this->charge_current*period.toSec()/3600.0;
+ if(this->current_charge>this->initial_charge)
+ this->current_charge=this->initial_charge;
+ else
+ this->smart_charger_data_smart_charger_data_msg_.batt_status="Connected and charging";
+ this->smart_charger_data_smart_charger_data_msg_.avg_time_full=(uint16_t)(((this->initial_charge-this->current_charge)/this->charge_current)*60.0);
+ }
+ else
+ {
+ this->current_charge-=this->discharge_rate*period.toSec()/3600.0;
+ if(this->current_charge<0.0)
+ {
+ this->current_charge=0.0;
+ ROS_ERROR("Battery died out before it could be recharged");
+ }
+ this->smart_charger_data_smart_charger_data_msg_.avg_time_empty=(uint16_t)((this->current_charge/this->discharge_rate)*60.0);
+ this->smart_charger_data_smart_charger_data_msg_.avg_time_full=(uint16_t)-1;
+ this->smart_charger_data_smart_charger_data_msg_.batt_status="Connected";
+ }
+ this->smart_charger_data_publisher_.publish(this->smart_charger_data_smart_charger_data_msg_);
+ }
+ }
+ // detect fall condition
+ this->fallen_state_msg_.data=(unsigned char)manager_get_fallen_position();
+ this->fallen_state_publisher_.publish(this->fallen_state_msg_);
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::OnContacts(ConstContactsPtr &msg)
+ {
+ for(unsigned int i=0;i<msg->contact_size();i++)
+ {
+ if(msg->contact(i).collision1().find(this->charge_station_link)!=std::string::npos)
+ {
+ if(msg->contact(i).collision2().find(this->charge_robot_link)!=std::string::npos)
+ {
+ this->charger_connected=true;
+ break;
+ }
+ else
+ this->charger_connected=false;
+ }
+ else if(msg->contact(i).collision1().find(this->charge_robot_link)!=std::string::npos)
+ {
+ if(msg->contact(i).collision2().find(this->charge_station_link)!=std::string::npos)
+ {
+ this->charger_connected=true;
+ break;
+ }
+ else
+ this->charger_connected=false;
+ }
+ else
+ this->charger_connected=false;
+ }
}
/************************************ motion_action *********************************************/
@@ -486,6 +651,155 @@ namespace darwin_controller
}
/************************************ follow_target_action *********************************************/
+/************************************ left_gripper_action *********************************************/
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::left_gripper_startCallback(const humanoid_common_msgs::humanoid_gripperGoalConstPtr& goal)
+ {
+ if(goal->open)
+ {
+ ROS_INFO_STREAM("CDarwinSim : Start openning the left gripper");
+ grippers_open(GRIPPER_LEFT);
+ }
+ else
+ {
+ ROS_INFO_STREAM("CDarwinSim : Start closing the left gripper");
+ grippers_close(GRIPPER_LEFT);
+ }
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::left_gripper_stopCallback(void)
+ {
+ ROS_INFO("CDarwinSim : Stop left gripper motion");
+ grippers_stop(GRIPPER_LEFT);
+ }
+
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::left_gripper_isFinishedCallback(void)
+ {
+ if(gripper_is_moving(GRIPPER_LEFT))
+ return false;
+ else
+ return true;
+ }
+
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::left_gripper_hasSucceedCallback(void)
+ {
+ return true;
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::left_gripper_getResultCallback(humanoid_common_msgs::humanoid_gripperResultPtr& result)
+ {
+
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::left_gripper_getFeedbackCallback(humanoid_common_msgs::humanoid_gripperFeedbackPtr& feedback)
+ {
+ feedback->current_angle_top=((manager_current_angles[L_GRIPPER_TOP]*3.14159)/180.0)/65536.0;
+ feedback->current_angle_bot=((manager_current_angles[L_GRIPPER_BOT]*3.14159)/180.0)/65536.0;
+ }
+/************************************ left_gripper_action *********************************************/
+
+/************************************ right_gripper_action *********************************************/
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::right_gripper_startCallback(const humanoid_common_msgs::humanoid_gripperGoalConstPtr& goal)
+ {
+ if(goal->open)
+ {
+ ROS_INFO_STREAM("CDarwinSim : Start openning the right gripper");
+ grippers_open(GRIPPER_RIGHT);
+ }
+ else
+ {
+ ROS_INFO_STREAM("CDarwinSim : Start closing the right gripper");
+ grippers_close(GRIPPER_RIGHT);
+ }
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::right_gripper_stopCallback(void)
+ {
+ ROS_INFO("CDarwinSim : Stop right gripper motion ");
+ grippers_stop(GRIPPER_RIGHT);
+ }
+
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::right_gripper_isFinishedCallback(void)
+ {
+ if(gripper_is_moving(GRIPPER_RIGHT))
+ return false;
+ else
+ return true;
+ }
+
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::right_gripper_hasSucceedCallback(void)
+ {
+ return true;
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::right_gripper_getResultCallback(humanoid_common_msgs::humanoid_gripperResultPtr& result)
+ {
+
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::right_gripper_getFeedbackCallback(humanoid_common_msgs::humanoid_gripperFeedbackPtr& feedback)
+ {
+ feedback->current_angle_top=((manager_current_angles[R_GRIPPER_TOP]*3.14159)/180.0)/65536.0;
+ feedback->current_angle_bot=((manager_current_angles[R_GRIPPER_BOT]*3.14159)/180.0)/65536.0;
+ }
+/************************************ right_gripper_action *********************************************/
+
+/************************************ stairs_action *********************************************/
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::stairs_startCallback(const humanoid_common_msgs::humanoid_stairsGoalConstPtr& goal)
+ {
+ ROS_INFO("CDarwinSim : start climbing stairs ");
+ if(goal->up)
+ stairs_start(0x01);
+ else
+ stairs_start(0x00);
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::stairs_stopCallback(void)
+ {
+ ROS_INFO("CDarwinSim : Stop climbing stairs ");
+ stairs_stop();
+ }
+
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::stairs_isFinishedCallback(void)
+ {
+ if(is_climbing_stairs())
+ return false;
+ else
+ return true;
+ }
+
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::stairs_hasSucceedCallback(void)
+ {
+ return true;
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::stairs_getResultCallback(humanoid_common_msgs::humanoid_stairsResultPtr& result)
+ {
+
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::stairs_getFeedbackCallback(humanoid_common_msgs::humanoid_stairsFeedbackPtr& feedback)
+ {
+ feedback->current_phase=stairs_get_phase();
+ }
+/************************************ stairs_action *********************************************/
/************************************ set_walk_params *******************************************/
template <class HardwareInterface>
@@ -513,6 +827,8 @@ namespace darwin_controller
this->walk_access.exit();
res.ret=true;
+
+ return true;
}
/************************************ set_walk_params *******************************************/
@@ -567,6 +883,106 @@ namespace darwin_controller
}
/************************************ get_walk_params *******************************************/
+/************************************ set_stairs_params *******************************************/
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::set_stairs_paramsCallback(humanoid_common_msgs::set_stairs_params::Request &req, humanoid_common_msgs::set_stairs_params::Response &res)
+ {
+ ROS_INFO("CDarwinSim : Setting stairs parameters ...");
+ ram_write_word(DARWIN_STAIRS_PHASE1_TIME_L,(unsigned short int)(req.params.PHASE1_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE2_TIME_L,(unsigned short int)(req.params.PHASE2_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE3_TIME_L,(unsigned short int)(req.params.PHASE3_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE4_TIME_L,(unsigned short int)(req.params.PHASE4_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE5_TIME_L,(unsigned short int)(req.params.PHASE5_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE6_TIME_L,(unsigned short int)(req.params.PHASE6_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE7_TIME_L,(unsigned short int)(req.params.PHASE7_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE8_TIME_L,(unsigned short int)(req.params.PHASE8_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE9_TIME_L,(unsigned short int)(req.params.PHASE9_TIME*1000.0));
+ ram_write_byte(DARWIN_STAIRS_X_OFFSET,(unsigned char)(req.params.X_OFFSET*1000.0));
+ ram_write_byte(DARWIN_STAIRS_Y_OFFSET,(unsigned char)(req.params.Y_OFFSET*1000.0));
+ ram_write_byte(DARWIN_STAIRS_Z_OFFSET,(unsigned char)(req.params.Z_OFFSET*1000.0));
+ ram_write_byte(DARWIN_STAIRS_R_OFFSET,(unsigned char)((req.params.R_OFFSET*180/PI)*8.0));
+ ram_write_byte(DARWIN_STAIRS_P_OFFSET,(unsigned char)((req.params.P_OFFSET*180/PI)*8.0));
+ ram_write_byte(DARWIN_STAIRS_A_OFFSET,(unsigned char)((req.params.A_OFFSET*180/PI)*8.0));
+ ram_write_byte(DARWIN_STAIRS_Y_SHIFT,(unsigned char)(req.params.Y_SHIFT*1000.0));
+ ram_write_byte(DARWIN_STAIRS_X_SHIFT,(unsigned char)(req.params.X_SHIFT*1000.0));
+ ram_write_byte(DARWIN_STAIRS_Z_OVERSHOOT,(unsigned char)(req.params.Z_OVERSHOOT*1000.0));
+ ram_write_byte(DARWIN_STAIRS_Z_HEIGHT,(unsigned char)(req.params.Z_HEIGHT*1000.0));
+ ram_write_word(DARWIN_STAIRS_HIP_PITCH_OFF_L,(unsigned short int)((req.params.HIP_PITCH_OFFSET*180.0/PI)*1024.0));
+ ram_write_byte(DARWIN_STAIRS_R_SHIFT,(unsigned char)((req.params.R_SHIFT*180/PI)*4.0));
+ ram_write_byte(DARWIN_STAIRS_P_SHIFT,(unsigned char)((req.params.P_SHIFT*180/PI)*4.0));
+ ram_write_byte(DARWIN_STAIRS_A_SHIFT,(unsigned char)((req.params.A_SHIFT*180/PI)*4.0));
+ ram_write_byte(DARWIN_STAIRS_Y_SPREAD,(unsigned char)(req.params.Y_SPREAD*1000.0));
+ ram_write_byte(DARWIN_STAIRS_X_SHIFT_BODY,(unsigned char)(req.params.X_SHIFT_BODY*1000.0));
+
+ res.ret=true;
+
+ return true;
+ }
+/************************************ set_stairs_params *******************************************/
+
+/************************************ get_stairs_params *******************************************/
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::get_stairs_paramsCallback(humanoid_common_msgs::get_stairs_params::Request &req, humanoid_common_msgs::get_stairs_params::Response &res)
+ {
+ unsigned char byte_value;
+ unsigned short int word_value;
+
+ ROS_INFO("CDarwinSim : Getting current stairs parameters ...");
+ ram_read_word(DARWIN_STAIRS_PHASE1_TIME_L,&word_value);
+ res.params.PHASE1_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE2_TIME_L,&word_value);
+ res.params.PHASE2_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE3_TIME_L,&word_value);
+ res.params.PHASE3_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE4_TIME_L,&word_value);
+ res.params.PHASE4_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE5_TIME_L,&word_value);
+ res.params.PHASE5_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE6_TIME_L,&word_value);
+ res.params.PHASE6_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE7_TIME_L,&word_value);
+ res.params.PHASE7_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE8_TIME_L,&word_value);
+ res.params.PHASE8_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE9_TIME_L,&word_value);
+ res.params.PHASE9_TIME=((double)word_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_X_OFFSET,&byte_value);
+ res.params.X_OFFSET=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_Y_OFFSET,&byte_value);
+ res.params.Y_OFFSET=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_Z_OFFSET,&byte_value);
+ res.params.Z_OFFSET=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_R_OFFSET,&byte_value);
+ res.params.R_OFFSET=((double)byte_value)*PI/1440.0;
+ ram_read_byte(DARWIN_STAIRS_P_OFFSET,&byte_value);
+ res.params.P_OFFSET=((double)byte_value)*PI/1440.0;
+ ram_read_byte(DARWIN_STAIRS_A_OFFSET,&byte_value);
+ res.params.A_OFFSET=((double)byte_value)*PI/1440.0;
+ ram_read_byte(DARWIN_STAIRS_Y_SHIFT,&byte_value);
+ res.params.Y_SHIFT=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_X_SHIFT,&byte_value);
+ res.params.X_SHIFT=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_Z_OVERSHOOT,&byte_value);
+ res.params.Z_OVERSHOOT=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_Z_HEIGHT,&byte_value);
+ res.params.Z_HEIGHT=((double)byte_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_HIP_PITCH_OFF_L,&word_value);
+ res.params.HIP_PITCH_OFFSET=(((double)word_value)*PI/180.0)/1024.0;
+ ram_read_byte(DARWIN_STAIRS_R_SHIFT,&byte_value);
+ res.params.R_SHIFT=((double)byte_value)*PI/720.0;
+ ram_read_byte(DARWIN_STAIRS_P_SHIFT,&byte_value);
+ res.params.P_SHIFT=((double)byte_value)*PI/720.0;
+ ram_read_byte(DARWIN_STAIRS_A_SHIFT,&byte_value);
+ res.params.A_SHIFT=((double)byte_value)*PI/720.0;
+ ram_read_byte(DARWIN_STAIRS_Y_SPREAD,&byte_value);
+ res.params.Y_SPREAD=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_X_SHIFT_BODY,&byte_value);
+ res.params.X_SHIFT_BODY=((double)byte_value)/1000.0;
+
+ return true;
+ }
+/************************************ get_walk_params *******************************************/
+
/************************************ set_servo_modules *****************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res)
@@ -672,9 +1088,40 @@ namespace darwin_controller
}
/************************************ get_tilt_pid *****************************************/
+/****************************** get_smart_charger_config ***********************************/
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::get_smart_charger_configCallback(humanoid_common_msgs::get_smart_charger_config::Request &req, humanoid_common_msgs::get_smart_charger_config::Response &res)
+ {
+ res.charger_config=this->sc_config;
+
+ return true;
+ }
+/************************************ get_tilt_pid *****************************************/
+
+/****************************** set_smart_charger_config ***********************************/
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::set_smart_charger_configCallback(humanoid_common_msgs::set_smart_charger_config::Request &req, humanoid_common_msgs::set_smart_charger_config::Response &res)
+ {
+ this->sc_config=req.charger_config;
+ if(this->sc_config.enable)
+ {
+ this->sc_config.enable=false;
+ this->charger_enabled=true;
+ }
+ else if(this->sc_config.disable)
+ {
+ this->sc_config.disable=false;
+ this->charger_enabled=false;
+ }
+ res.success=true;
+
+ return true;
+ }
+/************************************ get_tilt_pid *****************************************/
+
/****************************************** imu *************************************************/
template <class HardwareInterface>
- void DarwinController<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
+ void DarwinController<HardwareInterface>::gyro_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
double gyro_x,gyro_y;
@@ -684,7 +1131,7 @@ namespace darwin_controller
gyro_x=500.0;
else if(gyro_x<-500.0)
gyro_x=-500.0;
- gyro_data[0]=(int32_t)(gyro_x*1000.0);
+ gyro_data[0]=-(int32_t)(gyro_x*1000.0);
gyro_y=msg->angular_velocity.z*180.0/3.14159;
if(gyro_y>500.0)
gyro_y=500.0;
@@ -692,8 +1139,147 @@ namespace darwin_controller
gyro_y=-500.0;
gyro_data[1]=-(int32_t)(gyro_y*1000.0);
}
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::accel_callback(const sensor_msgs::Imu::ConstPtr& msg)
+ {
+ accel_data[0]=(int32_t)msg->linear_acceleration.x*1000.0;
+ accel_data[1]=(int32_t)msg->linear_acceleration.y*1000.0;
+ accel_data[2]=(int32_t)msg->linear_acceleration.z*1000.0;
+ }
/****************************************** imu *************************************************/
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::range_callback(const sensor_msgs::Range::ConstPtr& msg)
+ {
+ std::string frame_name;
+ unsigned char threshold;
+ static unsigned int remaining_left_sensors=10,remaining_right_sensors=10;
+
+ 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 feet sensors */
+ if(msg->range>(msg->max_range/2.0))
+ threshold=0x01;
+ else
+ threshold=0x00;
+ if(this->left_sensor_foot_present)
+ {
+ if(frame_name=="left_down_right_rear_ir_link")
+ {
+ this->left_foot_sensor_data_msg_.voltages[4]=msg->range;
+ this->left_foot_sensor_data_msg_.status[4]=threshold;
+ remaining_left_sensors--;
+ }
+ else if(frame_name=="left_down_right_middle_ir_link")
+ {
+ this->left_foot_sensor_data_msg_.voltages[5]=msg->range;
+ this->left_foot_sensor_data_msg_.status[5]=threshold;
+ remaining_left_sensors--;
+ }
+ else if(frame_name=="left_down_right_front_ir_link")
+ {
+ this->left_foot_sensor_data_msg_.voltages[6]=msg->range;
+ this->left_foot_sensor_data_msg_.status[6]=threshold;
+ remaining_left_sensors--;
+ }
+ else if(frame_name=="left_down_left_rear_ir_link")
+ {
+ this->left_foot_sensor_data_msg_.voltages[1]=msg->range;
+ this->left_foot_sensor_data_msg_.status[1]=threshold;
+ remaining_left_sensors--;
+ }
+ else if(frame_name=="left_down_left_middle_ir_link")
+ {
+ this->left_foot_sensor_data_msg_.voltages[0]=msg->range;
+ this->left_foot_sensor_data_msg_.status[0]=threshold;
+ remaining_left_sensors--;
+ }
+ else if(frame_name=="left_down_left_front_ir_link")
+ {
+ this->left_foot_sensor_data_msg_.voltages[3]=msg->range;
+ this->left_foot_sensor_data_msg_.status[3]=threshold;
+ remaining_left_sensors--;
+ }
+ else if(frame_name=="left_front_right_ir_link")
+ {
+ this->left_foot_sensor_data_msg_.voltages[8]=msg->range;
+ this->left_foot_sensor_data_msg_.status[8]=threshold;
+ remaining_left_sensors--;
+ }
+ else if(frame_name=="left_front_left_ir_link")
+ {
+ this->left_foot_sensor_data_msg_.voltages[7]=msg->range;
+ this->left_foot_sensor_data_msg_.status[7]=threshold;
+ remaining_left_sensors--;
+ }
+ if(remaining_left_sensors==0)
+ {
+ remaining_left_sensors=10;
+ this->left_foot_sensor_data_publisher_.publish(this->left_foot_sensor_data_msg_);
+ }
+ }
+ if(this->right_sensor_foot_present)
+ {
+ if(frame_name=="right_down_right_rear_ir_link")
+ {
+ this->right_foot_sensor_data_msg_.voltages[4]=msg->range;
+ this->right_foot_sensor_data_msg_.status[4]=threshold;
+ remaining_right_sensors--;
+ }
+ else if(frame_name=="right_down_right_middle_ir_link")
+ {
+ this->right_foot_sensor_data_msg_.voltages[5]=msg->range;
+ this->right_foot_sensor_data_msg_.status[5]=threshold;
+ remaining_right_sensors--;
+ }
+ else if(frame_name=="right_down_right_front_ir_link")
+ {
+ this->right_foot_sensor_data_msg_.voltages[6]=msg->range;
+ this->right_foot_sensor_data_msg_.status[6]=threshold;
+ remaining_right_sensors--;
+ }
+ else if(frame_name=="right_down_left_rear_ir_link")
+ {
+ this->right_foot_sensor_data_msg_.voltages[1]=msg->range;
+ this->right_foot_sensor_data_msg_.status[1]=threshold;
+ remaining_right_sensors--;
+ }
+ else if(frame_name=="right_down_left_middle_ir_link")
+ {
+ this->right_foot_sensor_data_msg_.voltages[0]=msg->range;
+ this->right_foot_sensor_data_msg_.status[0]=threshold;
+ remaining_right_sensors--;
+ }
+ else if(frame_name=="right_down_left_front_ir_link")
+ {
+ this->right_foot_sensor_data_msg_.voltages[3]=msg->range;
+ this->right_foot_sensor_data_msg_.status[3]=threshold;
+ remaining_right_sensors--;
+ }
+ else if(frame_name=="right_front_right_ir_link")
+ {
+ this->right_foot_sensor_data_msg_.voltages[8]=msg->range;
+ this->right_foot_sensor_data_msg_.status[8]=threshold;
+ remaining_right_sensors--;
+ }
+ else if(frame_name=="right_front_left_ir_link")
+ {
+ this->right_foot_sensor_data_msg_.voltages[7]=msg->range;
+ this->right_foot_sensor_data_msg_.status[7]=threshold;
+ remaining_right_sensors--;
+ }
+ if(remaining_right_sensors==0)
+ {
+ remaining_right_sensors=10;
+ this->right_foot_sensor_data_publisher_.publish(this->right_foot_sensor_data_msg_);
+ }
+ }
+ }
+
/****************************************** cmd_vel *********************************************/
template <class HardwareInterface>
void DarwinController<HardwareInterface>::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
diff --git a/darwin_controller/include/sim/stm32f103xe.h b/darwin_controller/include/sim/stm32f103xe.h
index b9758e77478527f241f4a9d836848b9c24c9f121..cba8183be54376fac60634622d4706956798bbff 100755
--- a/darwin_controller/include/sim/stm32f103xe.h
+++ b/darwin_controller/include/sim/stm32f103xe.h
@@ -179,8 +179,8 @@ typedef enum
typedef struct
{
__IO uint32_t SR;
- __IO uint32_t CR1;
- __IO uint32_t CR2;
+ __IO uint32_t sim_CR1;
+ __IO uint32_t sim_CR2;
__IO uint32_t SMPR1;
__IO uint32_t SMPR2;
__IO uint32_t JOFR1;
@@ -516,8 +516,8 @@ typedef struct
typedef struct
{
- __IO uint32_t CR1;
- __IO uint32_t CR2;
+ __IO uint32_t sim_CR1;
+ __IO uint32_t sim_CR2;
__IO uint32_t OAR1;
__IO uint32_t OAR2;
__IO uint32_t DR;
@@ -621,8 +621,8 @@ typedef struct
typedef struct
{
- __IO uint32_t CR1;
- __IO uint32_t CR2;
+ __IO uint32_t sim_CR1;
+ __IO uint32_t sim_CR2;
__IO uint32_t SR;
__IO uint32_t DR;
__IO uint32_t CRCPR;
@@ -637,8 +637,8 @@ typedef struct
*/
typedef struct
{
- __IO uint32_t CR1; /*!< TIM control register 1, Address offset: 0x00 */
- __IO uint32_t CR2; /*!< TIM control register 2, Address offset: 0x04 */
+ __IO uint32_t sim_CR1; /*!< TIM control register 1, Address offset: 0x00 */
+ __IO uint32_t sim_CR2; /*!< TIM control register 2, Address offset: 0x04 */
__IO uint32_t SMCR; /*!< TIM slave Mode Control register, Address offset: 0x08 */
__IO uint32_t DIER; /*!< TIM DMA/interrupt enable register, Address offset: 0x0C */
__IO uint32_t SR; /*!< TIM status register, Address offset: 0x10 */
@@ -673,9 +673,9 @@ typedef struct
__IO uint32_t SR; /*!< USART Status register, Address offset: 0x00 */
__IO uint32_t DR; /*!< USART Data register, Address offset: 0x04 */
__IO uint32_t BRR; /*!< USART Baud rate register, Address offset: 0x08 */
- __IO uint32_t CR1; /*!< USART Control register 1, Address offset: 0x0C */
- __IO uint32_t CR2; /*!< USART Control register 2, Address offset: 0x10 */
- __IO uint32_t CR3; /*!< USART Control register 3, Address offset: 0x14 */
+ __IO uint32_t sim_CR1; /*!< USART Control register 1, Address offset: 0x0C */
+ __IO uint32_t sim_CR2; /*!< USART Control register 2, Address offset: 0x10 */
+ __IO uint32_t sim_CR3; /*!< USART Control register 3, Address offset: 0x14 */
__IO uint32_t GTPR; /*!< USART Guard time and prescaler register, Address offset: 0x18 */
} USART_TypeDef;
diff --git a/darwin_controller/package.xml b/darwin_controller/package.xml
index 7b737d775af75ff48183c1b2e802f1b9e7f641f6..dd820ffa20e9f66cf2d80df497410f4f0d25c178 100644
--- a/darwin_controller/package.xml
+++ b/darwin_controller/package.xml
@@ -55,6 +55,8 @@
<build_depend>hardware_interface</build_depend>
<build_depend>controller_manager</build_depend> <!--Tests-only!-->
+ <build_depend>gazebo_ros</build_depend>
+ <build_depend>humanoid_modules</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>urdf</run_depend>
@@ -71,6 +73,8 @@
<run_depend>hardware_interface</run_depend>
<run_depend>controller_manager</run_depend> <!--Tests-only!-->
+ <run_depend>gazebo_ros</run_depend>
+ <run_depend>humanoid_modules</run_depend>
<export>
<controller_interface plugin="${prefix}/ros_control_plugins.xml"/>
diff --git a/darwin_controller/src/sim/imu.c b/darwin_controller/src/sim/imu.c
index 2a7631530920be8b2159ff7c7a762cec7f8dbe0f..c3a8c924a70f501d49777fedd50a624ef84d8e78 100755
--- a/darwin_controller/src/sim/imu.c
+++ b/darwin_controller/src/sim/imu.c
@@ -6,6 +6,7 @@ int16_t gyro_center[3];
int32_t gyro_data[3];
uint8_t gyro_calibration;
uint16_t gyro_calibration_num_samples;
+int32_t accel_data[3];
// public functions
void imu_init(void)
@@ -18,6 +19,9 @@ void imu_init(void)
gyro_data[2]=0;
gyro_calibration=0x00;
imu_set_calibration_samples(1024);
+ accel_data[0]=0;
+ accel_data[1]=0;
+ accel_data[2]=0;
}
void imu_start(void)
@@ -51,6 +55,13 @@ void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
*z=gyro_data[2]-gyro_center[2];
}
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
+{
+ *x=accel_data[0];
+ *y=accel_data[1];
+ *z=accel_data[2];
+}
+
// operation functions
uint8_t imu_in_range(unsigned short int address,unsigned short int length)
{
diff --git a/darwin_controller/src/sim/stm32_dyn_master.c b/darwin_controller/src/sim/stm32_dyn_master.c
index bdbe516177c01a03bec5a0496ef6d84402264248..1364dbe89a24d4499416d8d7e211522c1d80ba0e 100644
--- a/darwin_controller/src/sim/stm32_dyn_master.c
+++ b/darwin_controller/src/sim/stm32_dyn_master.c
@@ -15,12 +15,12 @@ void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeo
master->rx_timeout_ms=timeout_ms;
}
-inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level)
+void dyn_master_set_return_level(TDynamixelMaster *master,TReturnLevel level)
{
master->return_level=level;
}
-inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
+TReturnLevel dyn_master_get_return_level(TDynamixelMaster *master)
{
return master->return_level;
}
diff --git a/darwin_controller/src/sim/stm32_dyn_master_servos.c b/darwin_controller/src/sim/stm32_dyn_master_servos.c
index c71ec1e72e951d547983d8ac8b28443683c05702..fe9c25d983c2b6fb018d3abe958dd316fcf4b074 100644
--- a/darwin_controller/src/sim/stm32_dyn_master_servos.c
+++ b/darwin_controller/src/sim/stm32_dyn_master_servos.c
@@ -8,12 +8,12 @@ void darwin_dyn_master_init(void)
darwin_dyn_master.version=DYN_VER1;
}
-inline void darwin_dyn_master_enable_power(void)
+void darwin_dyn_master_enable_power(void)
{
/* do nothing */
}
-inline void darwin_dyn_master_disable_power(void)
+void darwin_dyn_master_disable_power(void)
{
/* do nothing */
}
diff --git a/darwin_controller/src/sim/stm32_dyn_master_v2_servos.c b/darwin_controller/src/sim/stm32_dyn_master_v2_servos.c
index 7c087def6e83315a391d14a7aa29d9774a0f3911..6f3f61726cd78112e2a22589c4e98b49e4e8845e 100644
--- a/darwin_controller/src/sim/stm32_dyn_master_v2_servos.c
+++ b/darwin_controller/src/sim/stm32_dyn_master_v2_servos.c
@@ -7,3 +7,12 @@ void darwin_dyn_master_v2_init(void)
darwin_dyn_master_v2.version=DYN_VER2;
}
+inline void darwin_dyn_master_v2_enable_power(void)
+{
+ /* do nothing */
+}
+
+inline void darwin_dyn_master_v2_disable_power(void)
+{
+ /* do nothing */
+}
diff --git a/darwin_controller/src/sim/stm32_eeprom.c b/darwin_controller/src/sim/stm32_eeprom.c
index 925ce2a48d67c22c1af1097e916a76abdb2b3d01..79adadc22b64b54dcb89aeb7e87c0ec8f2d41124 100644
--- a/darwin_controller/src/sim/stm32_eeprom.c
+++ b/darwin_controller/src/sim/stm32_eeprom.c
@@ -85,21 +85,59 @@ unsigned char eeprom_data[]={DEFAULT_DEVICE_MODEL&0xFF,
DEFAULT_HEAD_TILT_D>>8,
DEFAULT_HEAD_TILT_I_CLAMP&0xFF,
DEFAULT_HEAD_TILT_I_CLAMP>>8,
- DEFAULT_GRIPPER_LEFT_ID,
+ DEFAULT_GRIPPER_LEFT_TOP_ID,
+ DEFAULT_GRIPPER_LEFT_BOT_ID,
DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF,
DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8,
DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF,
DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8,
DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF,
DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8,
- DEFAULT_GRIPPER_RIGHT_ID,
+ DEFAULT_GRIPPER_RIGHT_TOP_ID,
+ DEFAULT_GRIPPER_RIGHT_BOT_ID,
DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF,
DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8,
DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF,
DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8,
DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF,
- DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8
-};
+ DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8,
+ DEFAULT_SMART_CHARGER_PERIOD&0xFF,
+ DEFAULT_SMART_CHARGER_PERIOD>>8,
+ DEFAULT_STAIRS_PHASE1_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE1_TIME>>8,
+ DEFAULT_STAIRS_PHASE2_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE2_TIME>>8,
+ DEFAULT_STAIRS_PHASE3_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE3_TIME>>8,
+ DEFAULT_STAIRS_PHASE4_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE4_TIME>>8,
+ DEFAULT_STAIRS_PHASE5_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE5_TIME>>8,
+ DEFAULT_STAIRS_PHASE6_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE6_TIME>>8,
+ DEFAULT_STAIRS_PHASE7_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE7_TIME>>8,
+ DEFAULT_STAIRS_PHASE8_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE8_TIME>>8,
+ DEFAULT_STAIRS_PHASE9_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE9_TIME>>8,
+ DEFAULT_STAIRS_X_OFFSET&0xFF,
+ DEFAULT_STAIRS_Y_OFFSET&0xFF,
+ DEFAULT_STAIRS_Z_OFFSET&0xFF,
+ DEFAULT_STAIRS_R_OFFSET,
+ DEFAULT_STAIRS_P_OFFSET,
+ DEFAULT_STAIRS_Y_OFFSET,
+ DEFAULT_STAIRS_Y_SHIFT,
+ DEFAULT_STAIRS_X_SHIFT,
+ DEFAULT_STAIRS_Z_OVERSHOOT,
+ DEFAULT_STAIRS_Z_HEIGHT,
+ DEFAULT_STAIRS_HIP_PITCH_OFF&0xFF,
+ DEFAULT_STAIRS_HIP_PITCH_OFF>>8,
+ DEFAULT_STAIRS_R_SHIFT,
+ DEFAULT_STAIRS_P_SHIFT,
+ DEFAULT_STAIRS_A_SHIFT,
+ DEFAULT_STAIRS_Y_SPREAD,
+ DEFAULT_STAIRS_X_SHIFT_BODY};
uint16_t EE_Init(void)
{
diff --git a/darwin_controller_cpp/CMakeLists.txt b/darwin_controller_cpp/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0dd0b4000dd89bedd1d7ed158260bba30a33d93b
--- /dev/null
+++ b/darwin_controller_cpp/CMakeLists.txt
@@ -0,0 +1,215 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(darwin_controller_cpp)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED roscpp urdf controller_interface hardware_interface controller_manager sensor_msgs gazebo_ros)
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(comm REQUIRED)
+FIND_PACKAGE(dynamixel REQUIRED)
+FIND_PACKAGE(bno055_imu_sim REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependencies might have been
+## pulled in transitively but can be declared for certainty nonetheless:
+## * add a build_depend tag for "message_generation"
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager sensor_msgs gazebo_ros
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
+SET(DARWIN_FW_PATH ~/humanoids/darwin_stm32_fw)
+SET(STM32_HAL_PATH ~/humanoids/stm32_hal)
+SET(STM32_LIBRARIES_PATH ~/humanoids/stm32_libraries)
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(include include/sim ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+include_directories(${DARWIN_FW_PATH}/include/)
+include_directories(${STM32_LIBRARIES_PATH}/dynamixel_base/include)
+include_directories(${STM32_LIBRARIES_PATH}/comm/include)
+include_directories(${STM32_LIBRARIES_PATH}/f1/usart/include)
+include_directories(${STM32_LIBRARIES_PATH}/utils/include)
+include_directories(${STM32_HAL_PATH}/f1/include)
+include_directories(${STM32_HAL_PATH}/f1/include/core)
+include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${comm_INCLUDE_DIR})
+include_directories(${dynamixel_INCLUDE_DIR})
+include_directories(${bno055_imu_sim_INCLUDE_DIR})
+
+add_definitions(-DSTM32F103xE)
+add_definitions(-DUSE_HAL_DRIVER)
+
+## Declare a cpp library
+# add_library(darwin_controller
+# src/${PROJECT_NAME}/darwin_controller.cpp
+# )
+add_library(${PROJECT_NAME} src/darwin_controller_cpp.cpp
+ ${DARWIN_FW_PATH}/src/ram.c
+ ${DARWIN_FW_PATH}/src/action.c
+ ${DARWIN_FW_PATH}/src/walking.c
+ ${DARWIN_FW_PATH}/src/darwin_kinematics.c
+ ${DARWIN_FW_PATH}/src/darwin_math.c
+ ${DARWIN_FW_PATH}/src/motion_pages.c
+ ${DARWIN_FW_PATH}/src/joint_motion.c
+ ${DARWIN_FW_PATH}/src/head_tracking.c
+ ${DARWIN_FW_PATH}/src/motion_manager.c
+ ${DARWIN_FW_PATH}/src/smart_charger.c
+ ${DARWIN_FW_PATH}/src/grippers.c
+ ${DARWIN_FW_PATH}/src/gpio.c
+ ${DARWIN_FW_PATH}/src/stairs.c
+ ${DARWIN_FW_PATH}/src/adc_dma.c
+ src/sim/stm32_hal.c
+ src/sim/stm32_cortex.c
+ src/sim/stm32_gpio.c
+ src/sim/stm32_timer.c
+ src/sim/stm32_timer_ex.c
+ src/sim/stm32_dma.c
+ src/sim/stm32_dyn_master.c
+ src/sim/stm32_dyn_slave.c
+ src/sim/stm32_dyn_master_servos.c
+ src/sim/stm32_dyn_master_v2_servos.c
+ src/sim/stm32_eeprom.c
+ src/sim/imu.c
+ include/darwin_controller_cpp.h
+ include/darwin_controller_cpp_impl.h)
+
+## Declare a cpp executable
+# add_executable(darwin_controller_node src/darwin_controller_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+add_dependencies(darwin_controller_cpp humanoid_common_msgs_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(darwin_controller_node
+# ${catkin_LIBRARIES}
+# )
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${comm_LIBRARY})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${dynamixel_LIBRARY})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${bno055_imu_sim_LIBRARY})
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS darwin_controller darwin_controller_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_darwin_controller.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/darwin_controller_cpp/config/motion_1024.bin b/darwin_controller_cpp/config/motion_1024.bin
new file mode 100755
index 0000000000000000000000000000000000000000..7727404629dd0e67c0641c6d104d2d02125a929d
Binary files /dev/null and b/darwin_controller_cpp/config/motion_1024.bin differ
diff --git a/darwin_controller_cpp/include/darwin_controller_cpp.h b/darwin_controller_cpp/include/darwin_controller_cpp.h
new file mode 100644
index 0000000000000000000000000000000000000000..808e89e5c6f4dac55cfee9bb316788ebad515133
--- /dev/null
+++ b/darwin_controller_cpp/include/darwin_controller_cpp.h
@@ -0,0 +1,112 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics S.L. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Adolfo Rodriguez Tsouroukdissian, Stuart Glaser
+
+#ifndef DARWIN_CONTROLLER_CPP_H
+#define DARWIN_CONTROLLER_CPP_H
+
+// C++ standard
+#include <cassert>
+#include <iterator>
+#include <stdexcept>
+#include <string>
+
+// Boost
+#include <boost/shared_ptr.hpp>
+#include <boost/scoped_ptr.hpp>
+
+// ROS
+#include <ros/node_handle.h>
+
+// URDF
+#include <urdf/model.h>
+
+// ros_controls
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <control_toolbox/pid.h>
+#include <sensor_msgs/Imu.h>
+#include <tf/transform_datatypes.h>
+
+#include "mutex.h"
+#include "dynamixel_slave_serial.h"
+#include "bno055_imu_sim.h"
+
+namespace darwin_controller_cpp
+{
+ /**
+ * \brief
+ *
+ */
+ template <class HardwareInterface>
+ class DarwinControllerCPP : public controller_interface::Controller<HardwareInterface>
+ {
+ public:
+ DarwinControllerCPP();
+ ~DarwinControllerCPP();
+
+ bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+ void starting(const ros::Time& time);
+ void stopping(const ros::Time& time);
+ void update(const ros::Time& time, const ros::Duration& period);
+
+ private:
+ typedef typename HardwareInterface::ResourceHandleType JointHandle;
+ typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
+
+ CDynamixelSlaveSerial *dyn_slave;
+
+ CBNO055IMUSim *imu_sim;
+
+ std::string name_; ///< Controller name.
+ std::vector<JointHandle> joints_; ///< Handles to controlled joints.
+ std::vector<std::string> joint_names_; ///< Controlled joint names.
+ std::vector<PidPtr> pids_;
+
+ // ROS API
+ ros::NodeHandle controller_nh_;
+
+ // topic subscribers
+ ros::Subscriber gyro_sub;
+ void gyro_callback(const sensor_msgs::Imu::ConstPtr& msg);
+
+ ros::Subscriber accel_sub;
+ void accel_callback(const sensor_msgs::Imu::ConstPtr& msg);
+
+ ros::Subscriber imu_sub;
+ void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
+
+ CMutex walk_access;
+ };
+
+} // namespace
+
+#include <darwin_controller_cpp_impl.h>
+
+#endif // header guard
diff --git a/darwin_controller_cpp/include/darwin_controller_cpp_impl.h b/darwin_controller_cpp/include/darwin_controller_cpp_impl.h
new file mode 100644
index 0000000000000000000000000000000000000000..e10277b25955849a46006cbf448c51d5e3ee4b9b
--- /dev/null
+++ b/darwin_controller_cpp/include/darwin_controller_cpp_impl.h
@@ -0,0 +1,380 @@
+#ifndef DARWIN_CONTROLLER_CPP_IMP_H
+#define DARWIN_CONTROLLER_CPP_IMP_H
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+#include "action.h"
+#include "action_id.h"
+#include "walking.h"
+#include "joint_motion.h"
+#include "head_tracking.h"
+#include "grippers.h"
+#include "ram.h"
+#include "imu.h"
+#include "darwin_math.h"
+#include "stairs.h"
+#include "darwin_dyn_slave.h"
+
+#ifdef _HAVE_XSD
+#include "../src/xml/darwin_config.hxx"
+#endif
+
+#include "exceptions.h"
+
+extern "C" {
+ // motion_manager private function declaration
+ void TIM5_IRQHandler(void);
+}
+
+// motion manager external variables
+extern int num_servos;
+extern double real_angles[PAGE_MAX_NUM_SERVOS];
+
+// action module external variables
+extern TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
+
+//IMU module external variables
+extern int32_t gyro_data[3];
+extern int32_t accel_data[3];
+
+// dynamixel slave external variables
+extern "C" {
+ extern void darwin_on_ping(void);
+ extern unsigned char darwin_on_read(unsigned short int address, unsigned short int length, unsigned char *data);
+ extern unsigned char darwin_on_write(unsigned short int address, unsigned short int length, unsigned char *data);
+}
+
+//parameters default values
+#define DEFAULT_ENABLE_SMART_CHARGER false
+#define DEFAULT_INITIAL_CHARGE 2200.0
+#define DEFAULT_DISCHARGE_RATE 6000.0
+#define DEFAULT_CHARGE_CURRENT 2200.0
+#define DEFAULT_CHARGE_STATION_LINK "none"
+#define DEFAULT_CHARGE_ROBOT_LINK "none"
+
+namespace darwin_controller_cpp
+{
+ namespace internal
+ {
+ std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string& param_name)
+ {
+ using namespace XmlRpc;
+ XmlRpcValue xml_array;
+ if (!nh.getParam(param_name, xml_array))
+ {
+ ROS_ERROR_STREAM("CDarwinSim : Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+ if (xml_array.getType() != XmlRpcValue::TypeArray)
+ {
+ ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter is not an array (namespace: " <<
+ nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+
+ std::vector<std::string> out;
+ for (int i = 0; i < xml_array.size(); ++i)
+ {
+ if (xml_array[i].getType() != XmlRpcValue::TypeString)
+ {
+ ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter contains a non-string element (namespace: " <<
+ nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+ out.push_back(static_cast<std::string>(xml_array[i]));
+ }
+ return out;
+ }
+
+ boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
+ {
+ boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
+
+ std::string urdf_str;
+ // Check for robot_description in proper namespace
+ if (nh.getParam(param_name, urdf_str))
+ {
+ if (!urdf->initString(urdf_str))
+ {
+ ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
+ nh.getNamespace() << ").");
+ return boost::shared_ptr<urdf::Model>();
+ }
+ }
+ // Check for robot_description in root
+ else if (!urdf->initParam("robot_description"))
+ {
+ ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter");
+ return boost::shared_ptr<urdf::Model>();
+ }
+ return urdf;
+ }
+
+ typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
+ std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
+ {
+ std::vector<UrdfJointConstPtr> out;
+ for (unsigned int i = 0; i < joint_names.size(); ++i)
+ {
+ UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
+ if (urdf_joint)
+ {
+ out.push_back(urdf_joint);
+ }
+ else
+ {
+ ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names[i] << "' in URDF model.");
+ return std::vector<UrdfJointConstPtr>();
+ }
+ }
+ return out;
+ }
+
+ std::string getLeafNamespace(const ros::NodeHandle& nh)
+ {
+ const std::string complete_ns = nh.getNamespace();
+ std::size_t id = complete_ns.find_last_of("/");
+ return complete_ns.substr(id + 1);
+ }
+
+ } // namespace
+
+ template <class HardwareInterface>
+ inline void DarwinControllerCPP<HardwareInterface>::starting(const ros::Time& time)
+ {
+ for(unsigned int i = 0; i <this->joints_.size(); ++i)
+ {
+ pids_[i]->reset();
+ this->joints_[i].setCommand(0.0);
+ }
+ }
+
+ template <class HardwareInterface>
+ inline void DarwinControllerCPP<HardwareInterface>::stopping(const ros::Time& time)
+ {
+ }
+
+ template <class HardwareInterface>
+ DarwinControllerCPP<HardwareInterface>::DarwinControllerCPP()
+ {
+ this->dyn_slave=NULL;
+ this->imu_sim=NULL;
+ }
+
+ template <class HardwareInterface>
+ bool DarwinControllerCPP<HardwareInterface>::init(HardwareInterface* hw,ros::NodeHandle& root_nh,ros::NodeHandle& controller_nh)
+ {
+ using namespace internal;
+ std::string serial_device;
+
+ // Cache controller node handle
+ controller_nh_ = controller_nh;
+
+ // initialize topics
+ this->gyro_sub=root_nh.subscribe("sensors/raw_gyro", 1, &DarwinControllerCPP<HardwareInterface>::gyro_callback,this);
+ this->accel_sub=root_nh.subscribe("sensors/raw_accel", 1, &DarwinControllerCPP<HardwareInterface>::accel_callback,this);
+ this->imu_sub=root_nh.subscribe("sensors/raw_imu", 1, &DarwinControllerCPP<HardwareInterface>::imu_callback,this);
+
+ // Controller name
+ name_=getLeafNamespace(controller_nh_);
+
+ // List of controlled joints
+ joint_names_=getStrings(controller_nh_,"joints");
+ if(joint_names_.empty())
+ return false;
+ const unsigned int n_joints=joint_names_.size();
+
+ // URDF joints
+ boost::shared_ptr<urdf::Model> urdf=getUrdf(root_nh, "robot_description");
+ if(!urdf)
+ return false;
+
+ std::vector<UrdfJointConstPtr> urdf_joints=getUrdfJoints(*urdf, joint_names_);
+ if(urdf_joints.empty())
+ return false;
+ assert(n_joints==urdf_joints.size());
+ num_servos=n_joints;// set the number of servos to the number of joint of the urdf model
+
+ // Initialize members
+ this->joints_.resize(n_joints);
+ this->pids_.resize(n_joints);
+ for(unsigned int i=0;i<n_joints;++i)
+ {
+ // Joint handle
+ try {
+ joints_[i]=hw->getHandle(joint_names_[i]);
+ }catch(...){
+ ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names_[i] << "' in '" <<
+ this->getHardwareInterfaceType() << "'.");
+ return false;
+ }
+ // Node handle to PID gains
+ ros::NodeHandle joint_nh(this->controller_nh_,std::string("gains/")+joint_names_[i]);
+
+ // Init PID gains from ROS parameter server
+ this->pids_[i].reset(new control_toolbox::Pid());
+ if(!this->pids_[i]->init(joint_nh))
+ {
+ ROS_WARN_STREAM("CDarwinSim : Failed to initialize PID gains from ROS parameter server.");
+ return false;
+ }
+
+ // Whether a joint is continuous (ie. has angle wraparound)
+ const std::string not_if = urdf_joints[i]->type == urdf::Joint::CONTINUOUS ? "" : "non-";
+
+ ROS_DEBUG_STREAM("CDarwinSim : Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << this->getHardwareInterfaceType() << "'.");
+ }
+
+ ROS_DEBUG_STREAM("CDarwinSim : Initialized controller '" << name_ << "' with:" <<
+ "\n- Number of joints: " << joints_.size() <<
+ "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'");
+
+ /* initialize the RAM */
+ ram_init();
+ /* initialize the IMU */
+ imu_init();
+ /* initialize dynamixel slave */
+ darwin_dyn_slave_init();
+ // initialize dynamixel slave server
+ ROS_INFO("Load dynamixel slave parameters");
+ serial_device="/dev/ttyUSB0";
+ controller_nh_.getParam("dyn_serial_device", serial_device);
+ ROS_INFO_STREAM("dyn_serial_device is: " << serial_device);
+ try{
+ this->dyn_slave=new CDynamixelSlaveSerial("dyn_slave",serial_device,dyn_version2);
+ this->dyn_slave->set_baudrate(2000000/(ram_data[DARWIN_BAUD_RATE]+1));
+ this->dyn_slave->set_return_delay(ram_data[DARWIN_RETURN_DELAY_TIME]);
+ this->dyn_slave->set_return_delay(ram_data[DARWIN_RETURN_LEVEL]);
+ this->dyn_slave->add_slave(ram_data[DARWIN_ID],boost::bind(&darwin_on_ping),boost::bind(&darwin_on_read,_1,_2,_3),boost::bind(&darwin_on_write,_1,_2,_3));
+ }catch(CException &e){
+ if(this->dyn_slave!=NULL)
+ {
+ delete this->dyn_slave;
+ this->dyn_slave=NULL;
+ }
+ ROS_ERROR_STREAM(e.what());
+ }
+ // initialize IMU simulator
+ ROS_INFO("Load IMU simulator");
+ serial_device="/dev/ttyUSB0";
+ controller_nh_.getParam("imu_serial_device", serial_device);
+ ROS_INFO_STREAM("dyn_serial_device is: " << serial_device);
+ try{
+ this->imu_sim=new CBNO055IMUSim("darwin_imu",serial_device);
+ this->imu_sim->set_calibrated();
+ }catch(CException &e){
+ if(this->imu_sim!=NULL)
+ {
+ delete this->imu_sim;
+ this->imu_sim=NULL;
+ }
+ ROS_ERROR_STREAM(e.what());
+ }
+ /* initialize motion modules */
+ manager_init(7800);// motion manager period in us
+ ROS_INFO("CDarwinSim : motion manager initialized");
+ /* go to the walk ready position */
+ for(unsigned int i=0;i<20;++i)
+ {
+ manager_set_module(i+1,MM_ACTION);
+ manager_enable_servo(i+1);
+ }
+ for(unsigned int i=20;i<n_joints;++i)
+ {
+ manager_set_module(i+1,MM_JOINTS);
+ manager_enable_servo(i+1);
+ }
+ manager_enable_balance();// enable balance
+ action_load_page(WALK_READY);
+ action_start_page();
+
+ return true;
+ }
+
+ template <class HardwareInterface>
+ void DarwinControllerCPP<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
+ {
+ static bool gyro_calibrated=false;
+ static ros::Time last_time=ros::Time(0,0);
+ ros::Time current_time=ros::Time::now();
+ std::vector<double> target_angles(joints_.size());
+
+ /* get the actual simulation angles */
+ for(unsigned int i=0;i<this->joints_.size();++i)
+ real_angles[i]=joints_[i].getPosition();
+ __HAL_TIM_SET_FLAG(TIM5,TIM_FLAG_CC1);
+ __HAL_TIM_SET_IT_SOURCE(TIM5,TIM_IT_CC1);
+ TIM5_IRQHandler();
+ for (unsigned int i = 0; i < this->joints_.size(); ++i)
+ {
+ target_angles[i] = ((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0;
+ double command = pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
+ this->joints_[i].setCommand(command);
+ }
+ }
+
+/****************************************** imu *************************************************/
+ template <class HardwareInterface>
+ void DarwinControllerCPP<HardwareInterface>::gyro_callback(const sensor_msgs::Imu::ConstPtr& msg)
+ {
+ double gyro_x,gyro_y;
+
+ // saturate to +/- 500 dps;
+ gyro_x=msg->angular_velocity.y*180.0/3.14159;
+ if(gyro_x>500.0)
+ gyro_x=500.0;
+ else if(gyro_x<-500.0)
+ gyro_x=-500.0;
+ gyro_data[0]=-(int32_t)(gyro_x*1000.0);
+ gyro_y=msg->angular_velocity.z*180.0/3.14159;
+ if(gyro_y>500.0)
+ gyro_y=500.0;
+ else if(gyro_y<-500.0)
+ gyro_y=-500.0;
+ gyro_data[1]=-(int32_t)(gyro_y*1000.0);
+ }
+
+ template <class HardwareInterface>
+ void DarwinControllerCPP<HardwareInterface>::accel_callback(const sensor_msgs::Imu::ConstPtr& msg)
+ {
+ accel_data[0]=(int32_t)msg->linear_acceleration.x*1000.0;
+ accel_data[1]=(int32_t)msg->linear_acceleration.y*1000.0;
+ accel_data[2]=(int32_t)msg->linear_acceleration.z*1000.0;
+ }
+
+ template <class HardwareInterface>
+ void DarwinControllerCPP<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
+ {
+ if(this->imu_sim!=NULL)
+ {
+ this->imu_sim->set_quaternion_data(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
+ tf::Quaternion q(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
+ tf::Matrix3x3 m(q);
+ double roll, pitch, yaw;
+ m.getRPY(roll, pitch, yaw);
+ this->imu_sim->set_euler_angles_data(yaw,roll,pitch);
+ this->imu_sim->set_linear_accel_data(msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z);
+ this->imu_sim->set_gyro_data(msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z);
+ }
+ }
+
+/****************************************** imu *************************************************/
+
+ template <class HardwareInterface>
+ DarwinControllerCPP<HardwareInterface>::~DarwinControllerCPP()
+ {
+ if(this->dyn_slave!=NULL)
+ {
+ delete this->dyn_slave;
+ this->dyn_slave=NULL;
+ }
+ if(this->imu_sim!=NULL)
+ {
+ delete this->imu_sim;
+ this->imu_sim=NULL;
+ }
+ }
+
+} // namespace
+
+#endif // header guard
diff --git a/darwin_controller_cpp/include/sim/stm32f103xe.h b/darwin_controller_cpp/include/sim/stm32f103xe.h
new file mode 100755
index 0000000000000000000000000000000000000000..cba8183be54376fac60634622d4706956798bbff
--- /dev/null
+++ b/darwin_controller_cpp/include/sim/stm32f103xe.h
@@ -0,0 +1,6750 @@
+/**
+ ******************************************************************************
+ * @file stm32f103xe.h
+ * @author MCD Application Team
+ * @version V4.0.1
+ * @date 31-July-2015
+ * @brief CMSIS Cortex-M3 Device Peripheral Access Layer Header File.
+ * This file contains all the peripheral register's definitions, bits
+ * definitions and memory mapping for STM32F1xx devices.
+ *
+ * This file contains:
+ * - Data structures and the address mapping for all peripherals
+ * - Peripheral's registers declarations and bits definition
+ * - Macros to access peripheral’s registers hardware
+ *
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>© COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+
+/** @addtogroup CMSIS
+ * @{
+ */
+
+/** @addtogroup stm32f103xe
+ * @{
+ */
+
+#ifndef __STM32F103xE_H
+#define __STM32F103xE_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/** @addtogroup Configuration_section_for_CMSIS
+ * @{
+ */
+/**
+ * @brief Configuration of the Cortex-M3 Processor and Core Peripherals
+ */
+ #define __MPU_PRESENT 0 /*!< Other STM32 devices does not provide an MPU */
+#define __CM3_REV 0x0200 /*!< Core Revision r2p0 */
+#define __NVIC_PRIO_BITS 4 /*!< STM32 uses 4 Bits for the Priority Levels */
+#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */
+
+/**
+ * @}
+ */
+
+/** @addtogroup Peripheral_interrupt_number_definition
+ * @{
+ */
+
+/**
+ * @brief STM32F10x Interrupt Number Definition, according to the selected device
+ * in @ref Library_configuration_section
+ */
+
+ /*!< Interrupt Number Definition */
+typedef enum
+{
+/****** Cortex-M3 Processor Exceptions Numbers ***************************************************/
+ NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */
+ MemoryManagement_IRQn = -12, /*!< 4 Cortex-M3 Memory Management Interrupt */
+ BusFault_IRQn = -11, /*!< 5 Cortex-M3 Bus Fault Interrupt */
+ UsageFault_IRQn = -10, /*!< 6 Cortex-M3 Usage Fault Interrupt */
+ SVCall_IRQn = -5, /*!< 11 Cortex-M3 SV Call Interrupt */
+ DebugMonitor_IRQn = -4, /*!< 12 Cortex-M3 Debug Monitor Interrupt */
+ PendSV_IRQn = -2, /*!< 14 Cortex-M3 Pend SV Interrupt */
+ SysTick_IRQn = -1, /*!< 15 Cortex-M3 System Tick Interrupt */
+
+/****** STM32 specific Interrupt Numbers *********************************************************/
+ WWDG_IRQn = 0, /*!< Window WatchDog Interrupt */
+ PVD_IRQn = 1, /*!< PVD through EXTI Line detection Interrupt */
+ TAMPER_IRQn = 2, /*!< Tamper Interrupt */
+ RTC_IRQn = 3, /*!< RTC global Interrupt */
+ FLASH_IRQn = 4, /*!< FLASH global Interrupt */
+ RCC_IRQn = 5, /*!< RCC global Interrupt */
+ EXTI0_IRQn = 6, /*!< EXTI Line0 Interrupt */
+ EXTI1_IRQn = 7, /*!< EXTI Line1 Interrupt */
+ EXTI2_IRQn = 8, /*!< EXTI Line2 Interrupt */
+ EXTI3_IRQn = 9, /*!< EXTI Line3 Interrupt */
+ EXTI4_IRQn = 10, /*!< EXTI Line4 Interrupt */
+ DMA1_Channel1_IRQn = 11, /*!< DMA1 Channel 1 global Interrupt */
+ DMA1_Channel2_IRQn = 12, /*!< DMA1 Channel 2 global Interrupt */
+ DMA1_Channel3_IRQn = 13, /*!< DMA1 Channel 3 global Interrupt */
+ DMA1_Channel4_IRQn = 14, /*!< DMA1 Channel 4 global Interrupt */
+ DMA1_Channel5_IRQn = 15, /*!< DMA1 Channel 5 global Interrupt */
+ DMA1_Channel6_IRQn = 16, /*!< DMA1 Channel 6 global Interrupt */
+ DMA1_Channel7_IRQn = 17, /*!< DMA1 Channel 7 global Interrupt */
+ ADC1_2_IRQn = 18, /*!< ADC1 and ADC2 global Interrupt */
+ USB_HP_CAN1_TX_IRQn = 19, /*!< USB Device High Priority or CAN1 TX Interrupts */
+ USB_LP_CAN1_RX0_IRQn = 20, /*!< USB Device Low Priority or CAN1 RX0 Interrupts */
+ CAN1_RX1_IRQn = 21, /*!< CAN1 RX1 Interrupt */
+ CAN1_SCE_IRQn = 22, /*!< CAN1 SCE Interrupt */
+ EXTI9_5_IRQn = 23, /*!< External Line[9:5] Interrupts */
+ TIM1_BRK_IRQn = 24, /*!< TIM1 Break Interrupt */
+ TIM1_UP_IRQn = 25, /*!< TIM1 Update Interrupt */
+ TIM1_TRG_COM_IRQn = 26, /*!< TIM1 Trigger and Commutation Interrupt */
+ TIM1_CC_IRQn = 27, /*!< TIM1 Capture Compare Interrupt */
+ TIM2_IRQn = 28, /*!< TIM2 global Interrupt */
+ TIM3_IRQn = 29, /*!< TIM3 global Interrupt */
+ TIM4_IRQn = 30, /*!< TIM4 global Interrupt */
+ I2C1_EV_IRQn = 31, /*!< I2C1 Event Interrupt */
+ I2C1_ER_IRQn = 32, /*!< I2C1 Error Interrupt */
+ I2C2_EV_IRQn = 33, /*!< I2C2 Event Interrupt */
+ I2C2_ER_IRQn = 34, /*!< I2C2 Error Interrupt */
+ SPI1_IRQn = 35, /*!< SPI1 global Interrupt */
+ SPI2_IRQn = 36, /*!< SPI2 global Interrupt */
+ USART1_IRQn = 37, /*!< USART1 global Interrupt */
+ USART2_IRQn = 38, /*!< USART2 global Interrupt */
+ USART3_IRQn = 39, /*!< USART3 global Interrupt */
+ EXTI15_10_IRQn = 40, /*!< External Line[15:10] Interrupts */
+ RTC_Alarm_IRQn = 41, /*!< RTC Alarm through EXTI Line Interrupt */
+ USBWakeUp_IRQn = 42, /*!< USB Device WakeUp from suspend through EXTI Line Interrupt */
+ TIM8_BRK_IRQn = 43, /*!< TIM8 Break Interrupt */
+ TIM8_UP_IRQn = 44, /*!< TIM8 Update Interrupt */
+ TIM8_TRG_COM_IRQn = 45, /*!< TIM8 Trigger and Commutation Interrupt */
+ TIM8_CC_IRQn = 46, /*!< TIM8 Capture Compare Interrupt */
+ ADC3_IRQn = 47, /*!< ADC3 global Interrupt */
+ FSMC_IRQn = 48, /*!< FSMC global Interrupt */
+ SDIO_IRQn = 49, /*!< SDIO global Interrupt */
+ TIM5_IRQn = 50, /*!< TIM5 global Interrupt */
+ SPI3_IRQn = 51, /*!< SPI3 global Interrupt */
+ UART4_IRQn = 52, /*!< UART4 global Interrupt */
+ UART5_IRQn = 53, /*!< UART5 global Interrupt */
+ TIM6_IRQn = 54, /*!< TIM6 global Interrupt */
+ TIM7_IRQn = 55, /*!< TIM7 global Interrupt */
+ DMA2_Channel1_IRQn = 56, /*!< DMA2 Channel 1 global Interrupt */
+ DMA2_Channel2_IRQn = 57, /*!< DMA2 Channel 2 global Interrupt */
+ DMA2_Channel3_IRQn = 58, /*!< DMA2 Channel 3 global Interrupt */
+ DMA2_Channel4_5_IRQn = 59, /*!< DMA2 Channel 4 and Channel 5 global Interrupt */
+} IRQn_Type;
+
+
+/**
+ * @}
+ */
+
+#include "core_cm3.h"
+#include "system_stm32f1xx.h"
+#include <stdint.h>
+
+/** @addtogroup Peripheral_registers_structures
+ * @{
+ */
+
+/**
+ * @brief Analog to Digital Converter
+ */
+
+typedef struct
+{
+ __IO uint32_t SR;
+ __IO uint32_t sim_CR1;
+ __IO uint32_t sim_CR2;
+ __IO uint32_t SMPR1;
+ __IO uint32_t SMPR2;
+ __IO uint32_t JOFR1;
+ __IO uint32_t JOFR2;
+ __IO uint32_t JOFR3;
+ __IO uint32_t JOFR4;
+ __IO uint32_t HTR;
+ __IO uint32_t LTR;
+ __IO uint32_t SQR1;
+ __IO uint32_t SQR2;
+ __IO uint32_t SQR3;
+ __IO uint32_t JSQR;
+ __IO uint32_t JDR1;
+ __IO uint32_t JDR2;
+ __IO uint32_t JDR3;
+ __IO uint32_t JDR4;
+ __IO uint32_t DR;
+} ADC_TypeDef;
+
+/**
+ * @brief Backup Registers
+ */
+
+typedef struct
+{
+ uint32_t RESERVED0;
+ __IO uint32_t DR1;
+ __IO uint32_t DR2;
+ __IO uint32_t DR3;
+ __IO uint32_t DR4;
+ __IO uint32_t DR5;
+ __IO uint32_t DR6;
+ __IO uint32_t DR7;
+ __IO uint32_t DR8;
+ __IO uint32_t DR9;
+ __IO uint32_t DR10;
+ __IO uint32_t RTCCR;
+ __IO uint32_t CR;
+ __IO uint32_t CSR;
+ uint32_t RESERVED13[2];
+ __IO uint32_t DR11;
+ __IO uint32_t DR12;
+ __IO uint32_t DR13;
+ __IO uint32_t DR14;
+ __IO uint32_t DR15;
+ __IO uint32_t DR16;
+ __IO uint32_t DR17;
+ __IO uint32_t DR18;
+ __IO uint32_t DR19;
+ __IO uint32_t DR20;
+ __IO uint32_t DR21;
+ __IO uint32_t DR22;
+ __IO uint32_t DR23;
+ __IO uint32_t DR24;
+ __IO uint32_t DR25;
+ __IO uint32_t DR26;
+ __IO uint32_t DR27;
+ __IO uint32_t DR28;
+ __IO uint32_t DR29;
+ __IO uint32_t DR30;
+ __IO uint32_t DR31;
+ __IO uint32_t DR32;
+ __IO uint32_t DR33;
+ __IO uint32_t DR34;
+ __IO uint32_t DR35;
+ __IO uint32_t DR36;
+ __IO uint32_t DR37;
+ __IO uint32_t DR38;
+ __IO uint32_t DR39;
+ __IO uint32_t DR40;
+ __IO uint32_t DR41;
+ __IO uint32_t DR42;
+} BKP_TypeDef;
+
+/**
+ * @brief Controller Area Network TxMailBox
+ */
+
+typedef struct
+{
+ __IO uint32_t TIR;
+ __IO uint32_t TDTR;
+ __IO uint32_t TDLR;
+ __IO uint32_t TDHR;
+} CAN_TxMailBox_TypeDef;
+
+/**
+ * @brief Controller Area Network FIFOMailBox
+ */
+
+typedef struct
+{
+ __IO uint32_t RIR;
+ __IO uint32_t RDTR;
+ __IO uint32_t RDLR;
+ __IO uint32_t RDHR;
+} CAN_FIFOMailBox_TypeDef;
+
+/**
+ * @brief Controller Area Network FilterRegister
+ */
+
+typedef struct
+{
+ __IO uint32_t FR1;
+ __IO uint32_t FR2;
+} CAN_FilterRegister_TypeDef;
+
+/**
+ * @brief Controller Area Network
+ */
+
+typedef struct
+{
+ __IO uint32_t MCR;
+ __IO uint32_t MSR;
+ __IO uint32_t TSR;
+ __IO uint32_t RF0R;
+ __IO uint32_t RF1R;
+ __IO uint32_t IER;
+ __IO uint32_t ESR;
+ __IO uint32_t BTR;
+ uint32_t RESERVED0[88];
+ CAN_TxMailBox_TypeDef sTxMailBox[3];
+ CAN_FIFOMailBox_TypeDef sFIFOMailBox[2];
+ uint32_t RESERVED1[12];
+ __IO uint32_t FMR;
+ __IO uint32_t FM1R;
+ uint32_t RESERVED2;
+ __IO uint32_t FS1R;
+ uint32_t RESERVED3;
+ __IO uint32_t FFA1R;
+ uint32_t RESERVED4;
+ __IO uint32_t FA1R;
+ uint32_t RESERVED5[8];
+ CAN_FilterRegister_TypeDef sFilterRegister[14];
+} CAN_TypeDef;
+
+/**
+ * @brief CRC calculation unit
+ */
+
+typedef struct
+{
+ __IO uint32_t DR; /*!< CRC Data register, Address offset: 0x00 */
+ __IO uint8_t IDR; /*!< CRC Independent data register, Address offset: 0x04 */
+ uint8_t RESERVED0; /*!< Reserved, Address offset: 0x05 */
+ uint16_t RESERVED1; /*!< Reserved, Address offset: 0x06 */
+ __IO uint32_t CR; /*!< CRC Control register, Address offset: 0x08 */
+} CRC_TypeDef;
+
+/**
+ * @brief Digital to Analog Converter
+ */
+
+typedef struct
+{
+ __IO uint32_t CR;
+ __IO uint32_t SWTRIGR;
+ __IO uint32_t DHR12R1;
+ __IO uint32_t DHR12L1;
+ __IO uint32_t DHR8R1;
+ __IO uint32_t DHR12R2;
+ __IO uint32_t DHR12L2;
+ __IO uint32_t DHR8R2;
+ __IO uint32_t DHR12RD;
+ __IO uint32_t DHR12LD;
+ __IO uint32_t DHR8RD;
+ __IO uint32_t DOR1;
+ __IO uint32_t DOR2;
+} DAC_TypeDef;
+
+/**
+ * @brief Debug MCU
+ */
+
+typedef struct
+{
+ __IO uint32_t IDCODE;
+ __IO uint32_t CR;
+}DBGMCU_TypeDef;
+
+/**
+ * @brief DMA Controller
+ */
+
+typedef struct
+{
+ __IO uint32_t CCR;
+ __IO uint32_t CNDTR;
+ __IO uint32_t CPAR;
+ __IO uint32_t CMAR;
+} DMA_Channel_TypeDef;
+
+typedef struct
+{
+ __IO uint32_t ISR;
+ __IO uint32_t IFCR;
+} DMA_TypeDef;
+
+
+
+/**
+ * @brief External Interrupt/Event Controller
+ */
+
+typedef struct
+{
+ __IO uint32_t IMR;
+ __IO uint32_t EMR;
+ __IO uint32_t RTSR;
+ __IO uint32_t FTSR;
+ __IO uint32_t SWIER;
+ __IO uint32_t PR;
+} EXTI_TypeDef;
+
+/**
+ * @brief FLASH Registers
+ */
+
+typedef struct
+{
+ __IO uint32_t ACR;
+ __IO uint32_t KEYR;
+ __IO uint32_t OPTKEYR;
+ __IO uint32_t SR;
+ __IO uint32_t CR;
+ __IO uint32_t AR;
+ __IO uint32_t RESERVED;
+ __IO uint32_t OBR;
+ __IO uint32_t WRPR;
+} FLASH_TypeDef;
+
+/**
+ * @brief Option Bytes Registers
+ */
+
+typedef struct
+{
+ __IO uint16_t RDP;
+ __IO uint16_t USER;
+ __IO uint16_t Data0;
+ __IO uint16_t Data1;
+ __IO uint16_t WRP0;
+ __IO uint16_t WRP1;
+ __IO uint16_t WRP2;
+ __IO uint16_t WRP3;
+} OB_TypeDef;
+
+/**
+ * @brief Flexible Static Memory Controller
+ */
+
+typedef struct
+{
+ __IO uint32_t BTCR[8];
+} FSMC_Bank1_TypeDef;
+
+/**
+ * @brief Flexible Static Memory Controller Bank1E
+ */
+
+typedef struct
+{
+ __IO uint32_t BWTR[7];
+} FSMC_Bank1E_TypeDef;
+
+/**
+ * @brief Flexible Static Memory Controller Bank2
+ */
+
+typedef struct
+{
+ __IO uint32_t PCR2; /*!< NAND Flash control register 2, Address offset: 0x60 */
+ __IO uint32_t SR2; /*!< NAND Flash FIFO status and interrupt register 2, Address offset: 0x64 */
+ __IO uint32_t PMEM2; /*!< NAND Flash Common memory space timing register 2, Address offset: 0x68 */
+ __IO uint32_t PATT2; /*!< NAND Flash Attribute memory space timing register 2, Address offset: 0x6C */
+ uint32_t RESERVED0; /*!< Reserved, 0x70 */
+ __IO uint32_t ECCR2; /*!< NAND Flash ECC result registers 2, Address offset: 0x74 */
+ uint32_t RESERVED1; /*!< Reserved, 0x78 */
+ uint32_t RESERVED2; /*!< Reserved, 0x7C */
+ __IO uint32_t PCR3; /*!< NAND Flash control register 3, Address offset: 0x80 */
+ __IO uint32_t SR3; /*!< NAND Flash FIFO status and interrupt register 3, Address offset: 0x84 */
+ __IO uint32_t PMEM3; /*!< NAND Flash Common memory space timing register 3, Address offset: 0x88 */
+ __IO uint32_t PATT3; /*!< NAND Flash Attribute memory space timing register 3, Address offset: 0x8C */
+ uint32_t RESERVED3; /*!< Reserved, 0x90 */
+ __IO uint32_t ECCR3; /*!< NAND Flash ECC result registers 3, Address offset: 0x94 */
+} FSMC_Bank2_3_TypeDef;
+
+/**
+ * @brief Flexible Static Memory Controller Bank4
+ */
+
+typedef struct
+{
+ __IO uint32_t PCR4;
+ __IO uint32_t SR4;
+ __IO uint32_t PMEM4;
+ __IO uint32_t PATT4;
+ __IO uint32_t PIO4;
+} FSMC_Bank4_TypeDef;
+
+/**
+ * @brief General Purpose I/O
+ */
+
+typedef struct
+{
+ __IO uint32_t CRL;
+ __IO uint32_t CRH;
+ __IO uint32_t IDR;
+ __IO uint32_t ODR;
+ __IO uint32_t BSRR;
+ __IO uint32_t BRR;
+ __IO uint32_t LCKR;
+} GPIO_TypeDef;
+
+/**
+ * @brief Alternate Function I/O
+ */
+
+typedef struct
+{
+ __IO uint32_t EVCR;
+ __IO uint32_t MAPR;
+ __IO uint32_t EXTICR[4];
+ uint32_t RESERVED0;
+ __IO uint32_t MAPR2;
+} AFIO_TypeDef;
+/**
+ * @brief Inter Integrated Circuit Interface
+ */
+
+typedef struct
+{
+ __IO uint32_t sim_CR1;
+ __IO uint32_t sim_CR2;
+ __IO uint32_t OAR1;
+ __IO uint32_t OAR2;
+ __IO uint32_t DR;
+ __IO uint32_t SR1;
+ __IO uint32_t SR2;
+ __IO uint32_t CCR;
+ __IO uint32_t TRISE;
+} I2C_TypeDef;
+
+/**
+ * @brief Independent WATCHDOG
+ */
+
+typedef struct
+{
+ __IO uint32_t KR; /*!< Key register, Address offset: 0x00 */
+ __IO uint32_t PR; /*!< Prescaler register, Address offset: 0x04 */
+ __IO uint32_t RLR; /*!< Reload register, Address offset: 0x08 */
+ __IO uint32_t SR; /*!< Status register, Address offset: 0x0C */
+} IWDG_TypeDef;
+
+/**
+ * @brief Power Control
+ */
+
+typedef struct
+{
+ __IO uint32_t CR;
+ __IO uint32_t CSR;
+} PWR_TypeDef;
+
+/**
+ * @brief Reset and Clock Control
+ */
+
+typedef struct
+{
+ __IO uint32_t CR;
+ __IO uint32_t CFGR;
+ __IO uint32_t CIR;
+ __IO uint32_t APB2RSTR;
+ __IO uint32_t APB1RSTR;
+ __IO uint32_t AHBENR;
+ __IO uint32_t APB2ENR;
+ __IO uint32_t APB1ENR;
+ __IO uint32_t BDCR;
+ __IO uint32_t CSR;
+
+
+} RCC_TypeDef;
+
+/**
+ * @brief Real-Time Clock
+ */
+
+typedef struct
+{
+ __IO uint32_t CRH;
+ __IO uint32_t CRL;
+ __IO uint32_t PRLH;
+ __IO uint32_t PRLL;
+ __IO uint32_t DIVH;
+ __IO uint32_t DIVL;
+ __IO uint32_t CNTH;
+ __IO uint32_t CNTL;
+ __IO uint32_t ALRH;
+ __IO uint32_t ALRL;
+} RTC_TypeDef;
+
+/**
+ * @brief SD host Interface
+ */
+
+typedef struct
+{
+ __IO uint32_t POWER;
+ __IO uint32_t CLKCR;
+ __IO uint32_t ARG;
+ __IO uint32_t CMD;
+ __I uint32_t RESPCMD;
+ __I uint32_t RESP1;
+ __I uint32_t RESP2;
+ __I uint32_t RESP3;
+ __I uint32_t RESP4;
+ __IO uint32_t DTIMER;
+ __IO uint32_t DLEN;
+ __IO uint32_t DCTRL;
+ __I uint32_t DCOUNT;
+ __I uint32_t STA;
+ __IO uint32_t ICR;
+ __IO uint32_t MASK;
+ uint32_t RESERVED0[2];
+ __I uint32_t FIFOCNT;
+ uint32_t RESERVED1[13];
+ __IO uint32_t FIFO;
+} SDIO_TypeDef;
+
+/**
+ * @brief Serial Peripheral Interface
+ */
+
+typedef struct
+{
+ __IO uint32_t sim_CR1;
+ __IO uint32_t sim_CR2;
+ __IO uint32_t SR;
+ __IO uint32_t DR;
+ __IO uint32_t CRCPR;
+ __IO uint32_t RXCRCR;
+ __IO uint32_t TXCRCR;
+ __IO uint32_t I2SCFGR;
+ __IO uint32_t I2SPR;
+} SPI_TypeDef;
+
+/**
+ * @brief TIM Timers
+ */
+typedef struct
+{
+ __IO uint32_t sim_CR1; /*!< TIM control register 1, Address offset: 0x00 */
+ __IO uint32_t sim_CR2; /*!< TIM control register 2, Address offset: 0x04 */
+ __IO uint32_t SMCR; /*!< TIM slave Mode Control register, Address offset: 0x08 */
+ __IO uint32_t DIER; /*!< TIM DMA/interrupt enable register, Address offset: 0x0C */
+ __IO uint32_t SR; /*!< TIM status register, Address offset: 0x10 */
+ __IO uint32_t EGR; /*!< TIM event generation register, Address offset: 0x14 */
+ __IO uint32_t CCMR1; /*!< TIM capture/compare mode register 1, Address offset: 0x18 */
+ __IO uint32_t CCMR2; /*!< TIM capture/compare mode register 2, Address offset: 0x1C */
+ __IO uint32_t CCER; /*!< TIM capture/compare enable register, Address offset: 0x20 */
+ __IO uint32_t CNT; /*!< TIM counter register, Address offset: 0x24 */
+ __IO uint32_t PSC; /*!< TIM prescaler register, Address offset: 0x28 */
+ __IO uint32_t ARR; /*!< TIM auto-reload register, Address offset: 0x2C */
+ __IO uint32_t RCR; /*!< TIM repetition counter register, Address offset: 0x30 */
+ __IO uint32_t CCR1; /*!< TIM capture/compare register 1, Address offset: 0x34 */
+ __IO uint32_t CCR2; /*!< TIM capture/compare register 2, Address offset: 0x38 */
+ __IO uint32_t CCR3; /*!< TIM capture/compare register 3, Address offset: 0x3C */
+ __IO uint32_t CCR4; /*!< TIM capture/compare register 4, Address offset: 0x40 */
+ __IO uint32_t BDTR; /*!< TIM break and dead-time register, Address offset: 0x44 */
+ __IO uint32_t DCR; /*!< TIM DMA control register, Address offset: 0x48 */
+ __IO uint32_t DMAR; /*!< TIM DMA address for full transfer register, Address offset: 0x4C */
+ __IO uint32_t OR; /*!< TIM option register, Address offset: 0x50 */
+}TIM_TypeDef;
+
+#define __HAL_TIM_SET_FLAG(__HANDLE__, __FLAG__) (((__HANDLE__)->SR |= (__FLAG__)))
+
+#define __HAL_TIM_SET_IT_SOURCE(__HANDLE__, __INTERRUPT__) (((__HANDLE__)->DIER |= (__INTERRUPT__)))
+
+/**
+ * @brief Universal Synchronous Asynchronous Receiver Transmitter
+ */
+
+typedef struct
+{
+ __IO uint32_t SR; /*!< USART Status register, Address offset: 0x00 */
+ __IO uint32_t DR; /*!< USART Data register, Address offset: 0x04 */
+ __IO uint32_t BRR; /*!< USART Baud rate register, Address offset: 0x08 */
+ __IO uint32_t sim_CR1; /*!< USART Control register 1, Address offset: 0x0C */
+ __IO uint32_t sim_CR2; /*!< USART Control register 2, Address offset: 0x10 */
+ __IO uint32_t sim_CR3; /*!< USART Control register 3, Address offset: 0x14 */
+ __IO uint32_t GTPR; /*!< USART Guard time and prescaler register, Address offset: 0x18 */
+} USART_TypeDef;
+
+/**
+ * @brief Universal Serial Bus Full Speed Device
+ */
+
+typedef struct
+{
+ __IO uint16_t EP0R; /*!< USB Endpoint 0 register, Address offset: 0x00 */
+ __IO uint16_t RESERVED0; /*!< Reserved */
+ __IO uint16_t EP1R; /*!< USB Endpoint 1 register, Address offset: 0x04 */
+ __IO uint16_t RESERVED1; /*!< Reserved */
+ __IO uint16_t EP2R; /*!< USB Endpoint 2 register, Address offset: 0x08 */
+ __IO uint16_t RESERVED2; /*!< Reserved */
+ __IO uint16_t EP3R; /*!< USB Endpoint 3 register, Address offset: 0x0C */
+ __IO uint16_t RESERVED3; /*!< Reserved */
+ __IO uint16_t EP4R; /*!< USB Endpoint 4 register, Address offset: 0x10 */
+ __IO uint16_t RESERVED4; /*!< Reserved */
+ __IO uint16_t EP5R; /*!< USB Endpoint 5 register, Address offset: 0x14 */
+ __IO uint16_t RESERVED5; /*!< Reserved */
+ __IO uint16_t EP6R; /*!< USB Endpoint 6 register, Address offset: 0x18 */
+ __IO uint16_t RESERVED6; /*!< Reserved */
+ __IO uint16_t EP7R; /*!< USB Endpoint 7 register, Address offset: 0x1C */
+ __IO uint16_t RESERVED7[17]; /*!< Reserved */
+ __IO uint16_t CNTR; /*!< Control register, Address offset: 0x40 */
+ __IO uint16_t RESERVED8; /*!< Reserved */
+ __IO uint16_t ISTR; /*!< Interrupt status register, Address offset: 0x44 */
+ __IO uint16_t RESERVED9; /*!< Reserved */
+ __IO uint16_t FNR; /*!< Frame number register, Address offset: 0x48 */
+ __IO uint16_t RESERVEDA; /*!< Reserved */
+ __IO uint16_t DADDR; /*!< Device address register, Address offset: 0x4C */
+ __IO uint16_t RESERVEDB; /*!< Reserved */
+ __IO uint16_t BTABLE; /*!< Buffer Table address register, Address offset: 0x50 */
+ __IO uint16_t RESERVEDC; /*!< Reserved */
+} USB_TypeDef;
+
+
+/**
+ * @brief Window WATCHDOG
+ */
+
+typedef struct
+{
+ __IO uint32_t CR; /*!< WWDG Control register, Address offset: 0x00 */
+ __IO uint32_t CFR; /*!< WWDG Configuration register, Address offset: 0x04 */
+ __IO uint32_t SR; /*!< WWDG Status register, Address offset: 0x08 */
+} WWDG_TypeDef;
+
+/**
+ * @}
+ */
+
+/** @addtogroup Peripheral_memory_map
+ * @{
+ */
+
+
+#define FLASH_BASE ((uint32_t)0x08000000) /*!< FLASH base address in the alias region */
+#define FLASH_BANK1_END ((uint32_t)0x0807FFFF) /*!< FLASH END address of bank1 */
+#define SRAM_BASE ((uint32_t)0x20000000) /*!< SRAM base address in the alias region */
+#define PERIPH_BASE ((uint32_t)0x40000000) /*!< Peripheral base address in the alias region */
+
+#define SRAM_BB_BASE ((uint32_t)0x22000000) /*!< SRAM base address in the bit-band region */
+#define PERIPH_BB_BASE ((uint32_t)0x42000000) /*!< Peripheral base address in the bit-band region */
+
+#define FSMC_BASE ((uint32_t)0x60000000) /*!< FSMC base address */
+#define FSMC_R_BASE ((uint32_t)0xA0000000) /*!< FSMC registers base address */
+
+/*!< Peripheral memory map */
+#define APB1PERIPH_BASE PERIPH_BASE
+#define APB2PERIPH_BASE (PERIPH_BASE + 0x10000)
+#define AHBPERIPH_BASE (PERIPH_BASE + 0x20000)
+
+/*#define TIM2_BASE (APB1PERIPH_BASE + 0x0000)
+#define TIM3_BASE (APB1PERIPH_BASE + 0x0400)
+#define TIM4_BASE (APB1PERIPH_BASE + 0x0800)
+#define TIM5_BASE (APB1PERIPH_BASE + 0x0C00)
+#define TIM6_BASE (APB1PERIPH_BASE + 0x1000)
+#define TIM7_BASE (APB1PERIPH_BASE + 0x1400)
+#define RTC_BASE (APB1PERIPH_BASE + 0x2800)
+#define WWDG_BASE (APB1PERIPH_BASE + 0x2C00)
+#define IWDG_BASE (APB1PERIPH_BASE + 0x3000)
+#define SPI2_BASE (APB1PERIPH_BASE + 0x3800)
+#define SPI3_BASE (APB1PERIPH_BASE + 0x3C00)
+#define USART2_BASE (APB1PERIPH_BASE + 0x4400)
+#define USART3_BASE (APB1PERIPH_BASE + 0x4800)
+#define UART4_BASE (APB1PERIPH_BASE + 0x4C00)
+#define UART5_BASE (APB1PERIPH_BASE + 0x5000)
+#define I2C1_BASE (APB1PERIPH_BASE + 0x5400)
+#define I2C2_BASE (APB1PERIPH_BASE + 0x5800)
+#define CAN1_BASE (APB1PERIPH_BASE + 0x6400)
+#define BKP_BASE (APB1PERIPH_BASE + 0x6C00)
+#define PWR_BASE (APB1PERIPH_BASE + 0x7000)
+#define DAC_BASE (APB1PERIPH_BASE + 0x7400)
+#define AFIO_BASE (APB2PERIPH_BASE + 0x0000)
+#define EXTI_BASE (APB2PERIPH_BASE + 0x0400)
+#define GPIOA_BASE (APB2PERIPH_BASE + 0x0800)
+#define GPIOB_BASE (APB2PERIPH_BASE + 0x0C00)
+#define GPIOC_BASE (APB2PERIPH_BASE + 0x1000)
+#define GPIOD_BASE (APB2PERIPH_BASE + 0x1400)
+#define GPIOE_BASE (APB2PERIPH_BASE + 0x1800)
+#define GPIOF_BASE (APB2PERIPH_BASE + 0x1C00)
+#define GPIOG_BASE (APB2PERIPH_BASE + 0x2000)
+#define ADC1_BASE (APB2PERIPH_BASE + 0x2400)
+#define ADC2_BASE (APB2PERIPH_BASE + 0x2800)
+#define TIM1_BASE (APB2PERIPH_BASE + 0x2C00)
+#define SPI1_BASE (APB2PERIPH_BASE + 0x3000)
+#define TIM8_BASE (APB2PERIPH_BASE + 0x3400)
+#define USART1_BASE (APB2PERIPH_BASE + 0x3800)
+#define ADC3_BASE (APB2PERIPH_BASE + 0x3C00)
+
+#define SDIO_BASE (PERIPH_BASE + 0x18000)
+
+#define DMA1_BASE (AHBPERIPH_BASE + 0x0000)
+#define DMA1_Channel1_BASE (AHBPERIPH_BASE + 0x0008)
+#define DMA1_Channel2_BASE (AHBPERIPH_BASE + 0x001C)
+#define DMA1_Channel3_BASE (AHBPERIPH_BASE + 0x0030)
+#define DMA1_Channel4_BASE (AHBPERIPH_BASE + 0x0044)
+#define DMA1_Channel5_BASE (AHBPERIPH_BASE + 0x0058)
+#define DMA1_Channel6_BASE (AHBPERIPH_BASE + 0x006C)
+#define DMA1_Channel7_BASE (AHBPERIPH_BASE + 0x0080)
+#define DMA2_BASE (AHBPERIPH_BASE + 0x0400)
+#define DMA2_Channel1_BASE (AHBPERIPH_BASE + 0x0408)
+#define DMA2_Channel2_BASE (AHBPERIPH_BASE + 0x041C)
+#define DMA2_Channel3_BASE (AHBPERIPH_BASE + 0x0430)
+#define DMA2_Channel4_BASE (AHBPERIPH_BASE + 0x0444)
+#define DMA2_Channel5_BASE (AHBPERIPH_BASE + 0x0458)
+#define RCC_BASE (AHBPERIPH_BASE + 0x1000)
+#define CRC_BASE (AHBPERIPH_BASE + 0x3000)
+
+#define FLASH_R_BASE (AHBPERIPH_BASE + 0x2000)
+#define OB_BASE ((uint32_t)0x1FFFF800)
+
+
+#define FSMC_BANK1 (FSMC_BASE)
+#define FSMC_BANK1_1 (FSMC_BANK1)
+#define FSMC_BANK1_2 (FSMC_BANK1 + 0x04000000)
+#define FSMC_BANK1_3 (FSMC_BANK1 + 0x08000000)
+#define FSMC_BANK1_4 (FSMC_BANK1 + 0x0C000000)
+
+#define FSMC_BANK2 (FSMC_BASE + 0x10000000)
+#define FSMC_BANK3 (FSMC_BASE + 0x20000000)
+#define FSMC_BANK4 (FSMC_BASE + 0x30000000)
+
+#define FSMC_BANK1_R_BASE (FSMC_R_BASE + 0x0000)
+#define FSMC_BANK1E_R_BASE (FSMC_R_BASE + 0x0104)
+#define FSMC_BANK2_3_R_BASE (FSMC_R_BASE + 0x0060)
+#define FSMC_BANK4_R_BASE (FSMC_R_BASE + 0x00A0)
+
+#define DBGMCU_BASE ((uint32_t)0xE0042000)
+
+// USB device FS
+#define USB_BASE (APB1PERIPH_BASE + 0x00005C00)
+#define USB_PMAADDR (APB1PERIPH_BASE + 0x00006000)
+*/
+
+/**
+ * @}
+ */
+
+/** @addtogroup Peripheral_declaration
+ * @{
+ */
+
+TIM_TypeDef TIM2_BASE;
+#define TIM2 (&TIM2_BASE)
+TIM_TypeDef TIM3_BASE;
+#define TIM3 (&TIM3_BASE)
+TIM_TypeDef TIM4_BASE;
+#define TIM4 (&TIM4_BASE)
+TIM_TypeDef TIM5_BASE;
+#define TIM5 (&TIM5_BASE)
+TIM_TypeDef TIM6_BASE;
+#define TIM6 (&TIM6_BASE)
+TIM_TypeDef TIM7_BASE;
+#define TIM7 (&TIM7_BASE)
+RTC_TypeDef RTC_BASE;
+#define RTC (&RTC_BASE)
+WWDG_TypeDef WWDG_BASE;
+#define WWDG (&WWDG_BASE)
+IWDG_TypeDef IWDG_BASE;
+#define IWDG (&IWDG_BASE)
+SPI_TypeDef SPI2_BASE;
+#define SPI2 (&SPI2_BASE)
+SPI_TypeDef SPI3_BASE;
+#define SPI3 (&SPI3_BASE)
+USART_TypeDef USART2_BASE;
+#define USART2 (&USART2_BASE)
+USART_TypeDef USART3_BASE;
+#define USART3 (&USART3_BASE)
+USART_TypeDef USART4_BASE;
+#define UART4 (&UART4_BASE)
+USART_TypeDef USART5_BASE;
+#define UART5 (&UART5_BASE)
+I2C_TypeDef I2C1_BASE;
+#define I2C1 (&I2C1_BASE)
+I2C_TypeDef I2C2_BASE;
+#define I2C2 (&I2C2_BASE)
+USB_TypeDef USB_BASE;
+#define USB (&USB_BASE)
+CAN_TypeDef CAN1_BASE;
+#define CAN1 (&CAN1_BASE)
+BKP_TypeDef BKP_BASE;
+#define BKP (&BKP_BASE)
+PWR_TypeDef PWR_BASE;
+#define PWR (&PWR_BASE)
+DAC_TypeDef DAC_BASE;
+#define DAC (&DAC_BASE)
+AFIO_TypeDef AFIO_BASE;
+#define AFIO (&AFIO_BASE)
+EXTI_TypeDef EXTI_BASE;
+#define EXTI (&EXTI_BASE)
+GPIO_TypeDef GPIOA_BASE;
+#define GPIOA (&GPIOA_BASE)
+GPIO_TypeDef GPIOB_BASE;
+#define GPIOB (&GPIOB_BASE)
+GPIO_TypeDef GPIOC_BASE;
+#define GPIOC (&GPIOC_BASE)
+GPIO_TypeDef GPIOD_BASE;
+#define GPIOD (&GPIOD_BASE)
+GPIO_TypeDef GPIOE_BASE;
+#define GPIOE (&GPIOE_BASE)
+GPIO_TypeDef GPIOF_BASE;
+#define GPIOF (&GPIOF_BASE)
+GPIO_TypeDef GPIOG_BASE;
+#define GPIOG (&GPIOG_BASE)
+ADC_TypeDef ADC1_BASE;
+#define ADC1 (&ADC1_BASE)
+ADC_TypeDef ADC2_BASE;
+#define ADC2 (&ADC2_BASE)
+TIM_TypeDef TIM1_BASE;
+#define TIM1 (&TIM1_BASE)
+SPI_TypeDef SPI1_BASE;
+#define SPI1 (&SPI1_BASE)
+TIM_TypeDef TIM8_BASE;
+#define TIM8 (&TIM8_BASE)
+USART_TypeDef USART1_BASE;
+#define USART1 (&USART1_BASE)
+ADC_TypeDef ADC3_BASE;
+#define ADC3 (&ADC3_BASE)
+SDIO_TypeDef SDIO_BASE;
+#define SDIO (&SDIO_BASE)
+DMA_TypeDef DMA1_BASE;
+#define DMA1 (&DMA1_BASE)
+DMA_TypeDef DMA2_BASE;
+#define DMA2 (&DMA2_BASE)
+DMA_Channel_TypeDef DMA1_Channel1_BASE;
+#define DMA1_Channel1 (&DMA1_Channel1_BASE)
+DMA_Channel_TypeDef DMA1_Channel2_BASE;
+#define DMA1_Channel2 (&DMA1_Channel2_BASE)
+DMA_Channel_TypeDef DMA1_Channel3_BASE;
+#define DMA1_Channel3 (&DMA1_Channel3_BASE)
+DMA_Channel_TypeDef DMA1_Channel4_BASE;
+#define DMA1_Channel4 (&DMA1_Channel4_BASE)
+DMA_Channel_TypeDef DMA1_Channel5_BASE;
+#define DMA1_Channel5 (&DMA1_Channel5_BASE)
+DMA_Channel_TypeDef DMA1_Channel6_BASE;
+#define DMA1_Channel6 (&DMA1_Channel6_BASE)
+DMA_Channel_TypeDef DMA1_Channel7_BASE;
+#define DMA1_Channel7 (&DMA1_Channel7_BASE)
+DMA_Channel_TypeDef DMA2_Channel1_BASE;
+#define DMA2_Channel1 (&DMA2_Channel1_BASE)
+DMA_Channel_TypeDef DMA2_Channel2_BASE;
+#define DMA2_Channel2 (&DMA2_Channel2_BASE)
+DMA_Channel_TypeDef DMA2_Channel3_BASE;
+#define DMA2_Channel3 (&DMA2_Channel3_BASE)
+DMA_Channel_TypeDef DMA2_Channel4_BASE;
+#define DMA2_Channel4 (&DMA2_Channel4_BASE)
+DMA_Channel_TypeDef DMA2_Channel5_BASE;
+#define DMA2_Channel5 (&DMA2_Channel5_BASE)
+RCC_TypeDef RCC_BASE;
+#define RCC (&RCC_BASE)
+CRC_TypeDef CRC_BASE;
+#define CRC (&CRC_BASE)
+FLASH_TypeDef FLASH_R_BASE;
+#define FLASH (&FLASH_R_BASE)
+OB_TypeDef OB_BASE;
+#define OB (&OB_BASE)
+FSMC_Bank1_TypeDef FSMC_BANK1_R_BASE;
+#define FSMC_Bank1 (&FSMC_BANK1_R_BASE)
+FSMC_Bank1_TypeDef FSMC_BANK1E_R_BASE;
+#define FSMC_Bank1E (&FSMC_BANK1E_R_BASE)
+FSMC_Bank2_3_TypeDef FSMC_BANK2_3_R_BASE;
+#define FSMC_Bank2_3 (&FSMC_BANK2_3_R_BASE)
+FSMC_Bank4_TypeDef FSMC_BANK4_R_BASE;
+#define FSMC_Bank4 (&FSMC_BANK4_R_BASE)
+DBGMCU_TypeDef DBGMCU_BASE;
+#define DBGMCU (&DBGMCU_BASE)
+
+
+/**
+ * @}
+ */
+
+/** @addtogroup Exported_constants
+ * @{
+ */
+
+ /** @addtogroup Peripheral_Registers_Bits_Definition
+ * @{
+ */
+
+/******************************************************************************/
+/* Peripheral Registers_Bits_Definition */
+/******************************************************************************/
+
+/******************************************************************************/
+/* */
+/* CRC calculation unit (CRC) */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for CRC_DR register *********************/
+#define CRC_DR_DR ((uint32_t)0xFFFFFFFF) /*!< Data register bits */
+
+/******************* Bit definition for CRC_IDR register ********************/
+#define CRC_IDR_IDR ((uint32_t)0xFF) /*!< General-purpose 8-bit data register bits */
+
+/******************** Bit definition for CRC_CR register ********************/
+#define CRC_CR_RESET ((uint32_t)0x00000001) /*!< RESET bit */
+
+/******************************************************************************/
+/* */
+/* Power Control */
+/* */
+/******************************************************************************/
+
+/******************** Bit definition for PWR_CR register ********************/
+#define PWR_CR_LPDS ((uint32_t)0x00000001) /*!< Low-Power Deepsleep */
+#define PWR_CR_PDDS ((uint32_t)0x00000002) /*!< Power Down Deepsleep */
+#define PWR_CR_CWUF ((uint32_t)0x00000004) /*!< Clear Wakeup Flag */
+#define PWR_CR_CSBF ((uint32_t)0x00000008) /*!< Clear Standby Flag */
+#define PWR_CR_PVDE ((uint32_t)0x00000010) /*!< Power Voltage Detector Enable */
+
+#define PWR_CR_PLS ((uint32_t)0x000000E0) /*!< PLS[2:0] bits (PVD Level Selection) */
+#define PWR_CR_PLS_0 ((uint32_t)0x00000020) /*!< Bit 0 */
+#define PWR_CR_PLS_1 ((uint32_t)0x00000040) /*!< Bit 1 */
+#define PWR_CR_PLS_2 ((uint32_t)0x00000080) /*!< Bit 2 */
+
+/*!< PVD level configuration */
+#define PWR_CR_PLS_2V2 ((uint32_t)0x00000000) /*!< PVD level 2.2V */
+#define PWR_CR_PLS_2V3 ((uint32_t)0x00000020) /*!< PVD level 2.3V */
+#define PWR_CR_PLS_2V4 ((uint32_t)0x00000040) /*!< PVD level 2.4V */
+#define PWR_CR_PLS_2V5 ((uint32_t)0x00000060) /*!< PVD level 2.5V */
+#define PWR_CR_PLS_2V6 ((uint32_t)0x00000080) /*!< PVD level 2.6V */
+#define PWR_CR_PLS_2V7 ((uint32_t)0x000000A0) /*!< PVD level 2.7V */
+#define PWR_CR_PLS_2V8 ((uint32_t)0x000000C0) /*!< PVD level 2.8V */
+#define PWR_CR_PLS_2V9 ((uint32_t)0x000000E0) /*!< PVD level 2.9V */
+
+#define PWR_CR_DBP ((uint32_t)0x00000100) /*!< Disable Backup Domain write protection */
+
+
+/******************* Bit definition for PWR_CSR register ********************/
+#define PWR_CSR_WUF ((uint32_t)0x00000001) /*!< Wakeup Flag */
+#define PWR_CSR_SBF ((uint32_t)0x00000002) /*!< Standby Flag */
+#define PWR_CSR_PVDO ((uint32_t)0x00000004) /*!< PVD Output */
+#define PWR_CSR_EWUP ((uint32_t)0x00000100) /*!< Enable WKUP pin */
+
+/******************************************************************************/
+/* */
+/* Backup registers */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for BKP_DR1 register ********************/
+#define BKP_DR1_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR2 register ********************/
+#define BKP_DR2_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR3 register ********************/
+#define BKP_DR3_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR4 register ********************/
+#define BKP_DR4_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR5 register ********************/
+#define BKP_DR5_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR6 register ********************/
+#define BKP_DR6_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR7 register ********************/
+#define BKP_DR7_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR8 register ********************/
+#define BKP_DR8_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR9 register ********************/
+#define BKP_DR9_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR10 register *******************/
+#define BKP_DR10_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR11 register *******************/
+#define BKP_DR11_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR12 register *******************/
+#define BKP_DR12_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR13 register *******************/
+#define BKP_DR13_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR14 register *******************/
+#define BKP_DR14_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR15 register *******************/
+#define BKP_DR15_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR16 register *******************/
+#define BKP_DR16_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR17 register *******************/
+#define BKP_DR17_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/****************** Bit definition for BKP_DR18 register ********************/
+#define BKP_DR18_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR19 register *******************/
+#define BKP_DR19_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR20 register *******************/
+#define BKP_DR20_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR21 register *******************/
+#define BKP_DR21_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR22 register *******************/
+#define BKP_DR22_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR23 register *******************/
+#define BKP_DR23_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR24 register *******************/
+#define BKP_DR24_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR25 register *******************/
+#define BKP_DR25_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR26 register *******************/
+#define BKP_DR26_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR27 register *******************/
+#define BKP_DR27_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR28 register *******************/
+#define BKP_DR28_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR29 register *******************/
+#define BKP_DR29_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR30 register *******************/
+#define BKP_DR30_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR31 register *******************/
+#define BKP_DR31_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR32 register *******************/
+#define BKP_DR32_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR33 register *******************/
+#define BKP_DR33_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR34 register *******************/
+#define BKP_DR34_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR35 register *******************/
+#define BKP_DR35_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR36 register *******************/
+#define BKP_DR36_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR37 register *******************/
+#define BKP_DR37_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR38 register *******************/
+#define BKP_DR38_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR39 register *******************/
+#define BKP_DR39_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR40 register *******************/
+#define BKP_DR40_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR41 register *******************/
+#define BKP_DR41_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+/******************* Bit definition for BKP_DR42 register *******************/
+#define BKP_DR42_D ((uint32_t)0x0000FFFF) /*!< Backup data */
+
+#define RTC_BKP_NUMBER 42
+
+/****************** Bit definition for BKP_RTCCR register *******************/
+#define BKP_RTCCR_CAL ((uint32_t)0x0000007F) /*!< Calibration value */
+#define BKP_RTCCR_CCO ((uint32_t)0x00000080) /*!< Calibration Clock Output */
+#define BKP_RTCCR_ASOE ((uint32_t)0x00000100) /*!< Alarm or Second Output Enable */
+#define BKP_RTCCR_ASOS ((uint32_t)0x00000200) /*!< Alarm or Second Output Selection */
+
+/******************** Bit definition for BKP_CR register ********************/
+#define BKP_CR_TPE ((uint32_t)0x00000001) /*!< TAMPER pin enable */
+#define BKP_CR_TPAL ((uint32_t)0x00000002) /*!< TAMPER pin active level */
+
+/******************* Bit definition for BKP_CSR register ********************/
+#define BKP_CSR_CTE ((uint32_t)0x00000001) /*!< Clear Tamper event */
+#define BKP_CSR_CTI ((uint32_t)0x00000002) /*!< Clear Tamper Interrupt */
+#define BKP_CSR_TPIE ((uint32_t)0x00000004) /*!< TAMPER Pin interrupt enable */
+#define BKP_CSR_TEF ((uint32_t)0x00000100) /*!< Tamper Event Flag */
+#define BKP_CSR_TIF ((uint32_t)0x00000200) /*!< Tamper Interrupt Flag */
+
+/******************************************************************************/
+/* */
+/* Reset and Clock Control */
+/* */
+/******************************************************************************/
+
+/******************** Bit definition for RCC_CR register ********************/
+#define RCC_CR_HSION ((uint32_t)0x00000001) /*!< Internal High Speed clock enable */
+#define RCC_CR_HSIRDY ((uint32_t)0x00000002) /*!< Internal High Speed clock ready flag */
+#define RCC_CR_HSITRIM ((uint32_t)0x000000F8) /*!< Internal High Speed clock trimming */
+#define RCC_CR_HSICAL ((uint32_t)0x0000FF00) /*!< Internal High Speed clock Calibration */
+#define RCC_CR_HSEON ((uint32_t)0x00010000) /*!< External High Speed clock enable */
+#define RCC_CR_HSERDY ((uint32_t)0x00020000) /*!< External High Speed clock ready flag */
+#define RCC_CR_HSEBYP ((uint32_t)0x00040000) /*!< External High Speed clock Bypass */
+#define RCC_CR_CSSON ((uint32_t)0x00080000) /*!< Clock Security System enable */
+#define RCC_CR_PLLON ((uint32_t)0x01000000) /*!< PLL enable */
+#define RCC_CR_PLLRDY ((uint32_t)0x02000000) /*!< PLL clock ready flag */
+
+
+/******************* Bit definition for RCC_CFGR register *******************/
+/*!< SW configuration */
+#define RCC_CFGR_SW ((uint32_t)0x00000003) /*!< SW[1:0] bits (System clock Switch) */
+#define RCC_CFGR_SW_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define RCC_CFGR_SW_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+
+#define RCC_CFGR_SW_HSI ((uint32_t)0x00000000) /*!< HSI selected as system clock */
+#define RCC_CFGR_SW_HSE ((uint32_t)0x00000001) /*!< HSE selected as system clock */
+#define RCC_CFGR_SW_PLL ((uint32_t)0x00000002) /*!< PLL selected as system clock */
+
+/*!< SWS configuration */
+#define RCC_CFGR_SWS ((uint32_t)0x0000000C) /*!< SWS[1:0] bits (System Clock Switch Status) */
+#define RCC_CFGR_SWS_0 ((uint32_t)0x00000004) /*!< Bit 0 */
+#define RCC_CFGR_SWS_1 ((uint32_t)0x00000008) /*!< Bit 1 */
+
+#define RCC_CFGR_SWS_HSI ((uint32_t)0x00000000) /*!< HSI oscillator used as system clock */
+#define RCC_CFGR_SWS_HSE ((uint32_t)0x00000004) /*!< HSE oscillator used as system clock */
+#define RCC_CFGR_SWS_PLL ((uint32_t)0x00000008) /*!< PLL used as system clock */
+
+/*!< HPRE configuration */
+#define RCC_CFGR_HPRE ((uint32_t)0x000000F0) /*!< HPRE[3:0] bits (AHB prescaler) */
+#define RCC_CFGR_HPRE_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define RCC_CFGR_HPRE_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+#define RCC_CFGR_HPRE_2 ((uint32_t)0x00000040) /*!< Bit 2 */
+#define RCC_CFGR_HPRE_3 ((uint32_t)0x00000080) /*!< Bit 3 */
+
+#define RCC_CFGR_HPRE_DIV1 ((uint32_t)0x00000000) /*!< SYSCLK not divided */
+#define RCC_CFGR_HPRE_DIV2 ((uint32_t)0x00000080) /*!< SYSCLK divided by 2 */
+#define RCC_CFGR_HPRE_DIV4 ((uint32_t)0x00000090) /*!< SYSCLK divided by 4 */
+#define RCC_CFGR_HPRE_DIV8 ((uint32_t)0x000000A0) /*!< SYSCLK divided by 8 */
+#define RCC_CFGR_HPRE_DIV16 ((uint32_t)0x000000B0) /*!< SYSCLK divided by 16 */
+#define RCC_CFGR_HPRE_DIV64 ((uint32_t)0x000000C0) /*!< SYSCLK divided by 64 */
+#define RCC_CFGR_HPRE_DIV128 ((uint32_t)0x000000D0) /*!< SYSCLK divided by 128 */
+#define RCC_CFGR_HPRE_DIV256 ((uint32_t)0x000000E0) /*!< SYSCLK divided by 256 */
+#define RCC_CFGR_HPRE_DIV512 ((uint32_t)0x000000F0) /*!< SYSCLK divided by 512 */
+
+/*!< PPRE1 configuration */
+#define RCC_CFGR_PPRE1 ((uint32_t)0x00000700) /*!< PRE1[2:0] bits (APB1 prescaler) */
+#define RCC_CFGR_PPRE1_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define RCC_CFGR_PPRE1_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+#define RCC_CFGR_PPRE1_2 ((uint32_t)0x00000400) /*!< Bit 2 */
+
+#define RCC_CFGR_PPRE1_DIV1 ((uint32_t)0x00000000) /*!< HCLK not divided */
+#define RCC_CFGR_PPRE1_DIV2 ((uint32_t)0x00000400) /*!< HCLK divided by 2 */
+#define RCC_CFGR_PPRE1_DIV4 ((uint32_t)0x00000500) /*!< HCLK divided by 4 */
+#define RCC_CFGR_PPRE1_DIV8 ((uint32_t)0x00000600) /*!< HCLK divided by 8 */
+#define RCC_CFGR_PPRE1_DIV16 ((uint32_t)0x00000700) /*!< HCLK divided by 16 */
+
+/*!< PPRE2 configuration */
+#define RCC_CFGR_PPRE2 ((uint32_t)0x00003800) /*!< PRE2[2:0] bits (APB2 prescaler) */
+#define RCC_CFGR_PPRE2_0 ((uint32_t)0x00000800) /*!< Bit 0 */
+#define RCC_CFGR_PPRE2_1 ((uint32_t)0x00001000) /*!< Bit 1 */
+#define RCC_CFGR_PPRE2_2 ((uint32_t)0x00002000) /*!< Bit 2 */
+
+#define RCC_CFGR_PPRE2_DIV1 ((uint32_t)0x00000000) /*!< HCLK not divided */
+#define RCC_CFGR_PPRE2_DIV2 ((uint32_t)0x00002000) /*!< HCLK divided by 2 */
+#define RCC_CFGR_PPRE2_DIV4 ((uint32_t)0x00002800) /*!< HCLK divided by 4 */
+#define RCC_CFGR_PPRE2_DIV8 ((uint32_t)0x00003000) /*!< HCLK divided by 8 */
+#define RCC_CFGR_PPRE2_DIV16 ((uint32_t)0x00003800) /*!< HCLK divided by 16 */
+
+/*!< ADCPPRE configuration */
+#define RCC_CFGR_ADCPRE ((uint32_t)0x0000C000) /*!< ADCPRE[1:0] bits (ADC prescaler) */
+#define RCC_CFGR_ADCPRE_0 ((uint32_t)0x00004000) /*!< Bit 0 */
+#define RCC_CFGR_ADCPRE_1 ((uint32_t)0x00008000) /*!< Bit 1 */
+
+#define RCC_CFGR_ADCPRE_DIV2 ((uint32_t)0x00000000) /*!< PCLK2 divided by 2 */
+#define RCC_CFGR_ADCPRE_DIV4 ((uint32_t)0x00004000) /*!< PCLK2 divided by 4 */
+#define RCC_CFGR_ADCPRE_DIV6 ((uint32_t)0x00008000) /*!< PCLK2 divided by 6 */
+#define RCC_CFGR_ADCPRE_DIV8 ((uint32_t)0x0000C000) /*!< PCLK2 divided by 8 */
+
+#define RCC_CFGR_PLLSRC ((uint32_t)0x00010000) /*!< PLL entry clock source */
+
+#define RCC_CFGR_PLLXTPRE ((uint32_t)0x00020000) /*!< HSE divider for PLL entry */
+
+/*!< PLLMUL configuration */
+#define RCC_CFGR_PLLMULL ((uint32_t)0x003C0000) /*!< PLLMUL[3:0] bits (PLL multiplication factor) */
+#define RCC_CFGR_PLLMULL_0 ((uint32_t)0x00040000) /*!< Bit 0 */
+#define RCC_CFGR_PLLMULL_1 ((uint32_t)0x00080000) /*!< Bit 1 */
+#define RCC_CFGR_PLLMULL_2 ((uint32_t)0x00100000) /*!< Bit 2 */
+#define RCC_CFGR_PLLMULL_3 ((uint32_t)0x00200000) /*!< Bit 3 */
+
+ #define RCC_CFGR_PLLXTPRE_HSE ((uint32_t)0x00000000) /*!< HSE clock not divided for PLL entry */
+ #define RCC_CFGR_PLLXTPRE_HSE_DIV2 ((uint32_t)0x00020000) /*!< HSE clock divided by 2 for PLL entry */
+
+ #define RCC_CFGR_PLLMULL2 ((uint32_t)0x00000000) /*!< PLL input clock*2 */
+ #define RCC_CFGR_PLLMULL3 ((uint32_t)0x00040000) /*!< PLL input clock*3 */
+ #define RCC_CFGR_PLLMULL4 ((uint32_t)0x00080000) /*!< PLL input clock*4 */
+ #define RCC_CFGR_PLLMULL5 ((uint32_t)0x000C0000) /*!< PLL input clock*5 */
+ #define RCC_CFGR_PLLMULL6 ((uint32_t)0x00100000) /*!< PLL input clock*6 */
+ #define RCC_CFGR_PLLMULL7 ((uint32_t)0x00140000) /*!< PLL input clock*7 */
+ #define RCC_CFGR_PLLMULL8 ((uint32_t)0x00180000) /*!< PLL input clock*8 */
+ #define RCC_CFGR_PLLMULL9 ((uint32_t)0x001C0000) /*!< PLL input clock*9 */
+ #define RCC_CFGR_PLLMULL10 ((uint32_t)0x00200000) /*!< PLL input clock10 */
+ #define RCC_CFGR_PLLMULL11 ((uint32_t)0x00240000) /*!< PLL input clock*11 */
+ #define RCC_CFGR_PLLMULL12 ((uint32_t)0x00280000) /*!< PLL input clock*12 */
+ #define RCC_CFGR_PLLMULL13 ((uint32_t)0x002C0000) /*!< PLL input clock*13 */
+ #define RCC_CFGR_PLLMULL14 ((uint32_t)0x00300000) /*!< PLL input clock*14 */
+ #define RCC_CFGR_PLLMULL15 ((uint32_t)0x00340000) /*!< PLL input clock*15 */
+ #define RCC_CFGR_PLLMULL16 ((uint32_t)0x00380000) /*!< PLL input clock*16 */
+ #define RCC_CFGR_USBPRE ((uint32_t)0x00400000) /*!< USB Device prescaler */
+
+/*!< MCO configuration */
+ #define RCC_CFGR_MCO ((uint32_t)0x07000000) /*!< MCO[2:0] bits (Microcontroller Clock Output) */
+ #define RCC_CFGR_MCO_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+ #define RCC_CFGR_MCO_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+ #define RCC_CFGR_MCO_2 ((uint32_t)0x04000000) /*!< Bit 2 */
+
+ #define RCC_CFGR_MCO_NOCLOCK ((uint32_t)0x00000000) /*!< No clock */
+ #define RCC_CFGR_MCO_SYSCLK ((uint32_t)0x04000000) /*!< System clock selected as MCO source */
+ #define RCC_CFGR_MCO_HSI ((uint32_t)0x05000000) /*!< HSI clock selected as MCO source */
+ #define RCC_CFGR_MCO_HSE ((uint32_t)0x06000000) /*!< HSE clock selected as MCO source */
+ #define RCC_CFGR_MCO_PLLCLK_DIV2 ((uint32_t)0x07000000) /*!< PLL clock divided by 2 selected as MCO source */
+
+/*!<****************** Bit definition for RCC_CIR register ********************/
+#define RCC_CIR_LSIRDYF ((uint32_t)0x00000001) /*!< LSI Ready Interrupt flag */
+#define RCC_CIR_LSERDYF ((uint32_t)0x00000002) /*!< LSE Ready Interrupt flag */
+#define RCC_CIR_HSIRDYF ((uint32_t)0x00000004) /*!< HSI Ready Interrupt flag */
+#define RCC_CIR_HSERDYF ((uint32_t)0x00000008) /*!< HSE Ready Interrupt flag */
+#define RCC_CIR_PLLRDYF ((uint32_t)0x00000010) /*!< PLL Ready Interrupt flag */
+#define RCC_CIR_CSSF ((uint32_t)0x00000080) /*!< Clock Security System Interrupt flag */
+#define RCC_CIR_LSIRDYIE ((uint32_t)0x00000100) /*!< LSI Ready Interrupt Enable */
+#define RCC_CIR_LSERDYIE ((uint32_t)0x00000200) /*!< LSE Ready Interrupt Enable */
+#define RCC_CIR_HSIRDYIE ((uint32_t)0x00000400) /*!< HSI Ready Interrupt Enable */
+#define RCC_CIR_HSERDYIE ((uint32_t)0x00000800) /*!< HSE Ready Interrupt Enable */
+#define RCC_CIR_PLLRDYIE ((uint32_t)0x00001000) /*!< PLL Ready Interrupt Enable */
+#define RCC_CIR_LSIRDYC ((uint32_t)0x00010000) /*!< LSI Ready Interrupt Clear */
+#define RCC_CIR_LSERDYC ((uint32_t)0x00020000) /*!< LSE Ready Interrupt Clear */
+#define RCC_CIR_HSIRDYC ((uint32_t)0x00040000) /*!< HSI Ready Interrupt Clear */
+#define RCC_CIR_HSERDYC ((uint32_t)0x00080000) /*!< HSE Ready Interrupt Clear */
+#define RCC_CIR_PLLRDYC ((uint32_t)0x00100000) /*!< PLL Ready Interrupt Clear */
+#define RCC_CIR_CSSC ((uint32_t)0x00800000) /*!< Clock Security System Interrupt Clear */
+
+
+/***************** Bit definition for RCC_APB2RSTR register *****************/
+#define RCC_APB2RSTR_AFIORST ((uint32_t)0x00000001) /*!< Alternate Function I/O reset */
+#define RCC_APB2RSTR_IOPARST ((uint32_t)0x00000004) /*!< I/O port A reset */
+#define RCC_APB2RSTR_IOPBRST ((uint32_t)0x00000008) /*!< I/O port B reset */
+#define RCC_APB2RSTR_IOPCRST ((uint32_t)0x00000010) /*!< I/O port C reset */
+#define RCC_APB2RSTR_IOPDRST ((uint32_t)0x00000020) /*!< I/O port D reset */
+#define RCC_APB2RSTR_ADC1RST ((uint32_t)0x00000200) /*!< ADC 1 interface reset */
+
+#define RCC_APB2RSTR_ADC2RST ((uint32_t)0x00000400) /*!< ADC 2 interface reset */
+
+#define RCC_APB2RSTR_TIM1RST ((uint32_t)0x00000800) /*!< TIM1 Timer reset */
+#define RCC_APB2RSTR_SPI1RST ((uint32_t)0x00001000) /*!< SPI 1 reset */
+#define RCC_APB2RSTR_USART1RST ((uint32_t)0x00004000) /*!< USART1 reset */
+
+
+#define RCC_APB2RSTR_IOPERST ((uint32_t)0x00000040) /*!< I/O port E reset */
+
+#define RCC_APB2RSTR_IOPFRST ((uint32_t)0x00000080) /*!< I/O port F reset */
+#define RCC_APB2RSTR_IOPGRST ((uint32_t)0x00000100) /*!< I/O port G reset */
+#define RCC_APB2RSTR_TIM8RST ((uint32_t)0x00002000) /*!< TIM8 Timer reset */
+#define RCC_APB2RSTR_ADC3RST ((uint32_t)0x00008000) /*!< ADC3 interface reset */
+
+
+
+/***************** Bit definition for RCC_APB1RSTR register *****************/
+#define RCC_APB1RSTR_TIM2RST ((uint32_t)0x00000001) /*!< Timer 2 reset */
+#define RCC_APB1RSTR_TIM3RST ((uint32_t)0x00000002) /*!< Timer 3 reset */
+#define RCC_APB1RSTR_WWDGRST ((uint32_t)0x00000800) /*!< Window Watchdog reset */
+#define RCC_APB1RSTR_USART2RST ((uint32_t)0x00020000) /*!< USART 2 reset */
+#define RCC_APB1RSTR_I2C1RST ((uint32_t)0x00200000) /*!< I2C 1 reset */
+
+#define RCC_APB1RSTR_CAN1RST ((uint32_t)0x02000000) /*!< CAN1 reset */
+
+#define RCC_APB1RSTR_BKPRST ((uint32_t)0x08000000) /*!< Backup interface reset */
+#define RCC_APB1RSTR_PWRRST ((uint32_t)0x10000000) /*!< Power interface reset */
+
+#define RCC_APB1RSTR_TIM4RST ((uint32_t)0x00000004) /*!< Timer 4 reset */
+#define RCC_APB1RSTR_SPI2RST ((uint32_t)0x00004000) /*!< SPI 2 reset */
+#define RCC_APB1RSTR_USART3RST ((uint32_t)0x00040000) /*!< USART 3 reset */
+#define RCC_APB1RSTR_I2C2RST ((uint32_t)0x00400000) /*!< I2C 2 reset */
+
+#define RCC_APB1RSTR_USBRST ((uint32_t)0x00800000) /*!< USB Device reset */
+
+#define RCC_APB1RSTR_TIM5RST ((uint32_t)0x00000008) /*!< Timer 5 reset */
+#define RCC_APB1RSTR_TIM6RST ((uint32_t)0x00000010) /*!< Timer 6 reset */
+#define RCC_APB1RSTR_TIM7RST ((uint32_t)0x00000020) /*!< Timer 7 reset */
+#define RCC_APB1RSTR_SPI3RST ((uint32_t)0x00008000) /*!< SPI 3 reset */
+#define RCC_APB1RSTR_UART4RST ((uint32_t)0x00080000) /*!< UART 4 reset */
+#define RCC_APB1RSTR_UART5RST ((uint32_t)0x00100000) /*!< UART 5 reset */
+
+
+
+
+#define RCC_APB1RSTR_DACRST ((uint32_t)0x20000000) /*!< DAC interface reset */
+
+/****************** Bit definition for RCC_AHBENR register ******************/
+#define RCC_AHBENR_DMA1EN ((uint32_t)0x00000001) /*!< DMA1 clock enable */
+#define RCC_AHBENR_SRAMEN ((uint32_t)0x00000004) /*!< SRAM interface clock enable */
+#define RCC_AHBENR_FLITFEN ((uint32_t)0x00000010) /*!< FLITF clock enable */
+#define RCC_AHBENR_CRCEN ((uint32_t)0x00000040) /*!< CRC clock enable */
+
+ #define RCC_AHBENR_DMA2EN ((uint32_t)0x00000002) /*!< DMA2 clock enable */
+
+ #define RCC_AHBENR_FSMCEN ((uint32_t)0x00000100) /*!< FSMC clock enable */
+ #define RCC_AHBENR_SDIOEN ((uint32_t)0x00000400) /*!< SDIO clock enable */
+
+
+/****************** Bit definition for RCC_APB2ENR register *****************/
+#define RCC_APB2ENR_AFIOEN ((uint32_t)0x00000001) /*!< Alternate Function I/O clock enable */
+#define RCC_APB2ENR_IOPAEN ((uint32_t)0x00000004) /*!< I/O port A clock enable */
+#define RCC_APB2ENR_IOPBEN ((uint32_t)0x00000008) /*!< I/O port B clock enable */
+#define RCC_APB2ENR_IOPCEN ((uint32_t)0x00000010) /*!< I/O port C clock enable */
+#define RCC_APB2ENR_IOPDEN ((uint32_t)0x00000020) /*!< I/O port D clock enable */
+#define RCC_APB2ENR_ADC1EN ((uint32_t)0x00000200) /*!< ADC 1 interface clock enable */
+
+#define RCC_APB2ENR_ADC2EN ((uint32_t)0x00000400) /*!< ADC 2 interface clock enable */
+
+#define RCC_APB2ENR_TIM1EN ((uint32_t)0x00000800) /*!< TIM1 Timer clock enable */
+#define RCC_APB2ENR_SPI1EN ((uint32_t)0x00001000) /*!< SPI 1 clock enable */
+#define RCC_APB2ENR_USART1EN ((uint32_t)0x00004000) /*!< USART1 clock enable */
+
+
+#define RCC_APB2ENR_IOPEEN ((uint32_t)0x00000040) /*!< I/O port E clock enable */
+
+#define RCC_APB2ENR_IOPFEN ((uint32_t)0x00000080) /*!< I/O port F clock enable */
+#define RCC_APB2ENR_IOPGEN ((uint32_t)0x00000100) /*!< I/O port G clock enable */
+#define RCC_APB2ENR_TIM8EN ((uint32_t)0x00002000) /*!< TIM8 Timer clock enable */
+#define RCC_APB2ENR_ADC3EN ((uint32_t)0x00008000) /*!< DMA1 clock enable */
+
+
+
+/***************** Bit definition for RCC_APB1ENR register ******************/
+#define RCC_APB1ENR_TIM2EN ((uint32_t)0x00000001) /*!< Timer 2 clock enabled*/
+#define RCC_APB1ENR_TIM3EN ((uint32_t)0x00000002) /*!< Timer 3 clock enable */
+#define RCC_APB1ENR_WWDGEN ((uint32_t)0x00000800) /*!< Window Watchdog clock enable */
+#define RCC_APB1ENR_USART2EN ((uint32_t)0x00020000) /*!< USART 2 clock enable */
+#define RCC_APB1ENR_I2C1EN ((uint32_t)0x00200000) /*!< I2C 1 clock enable */
+
+#define RCC_APB1ENR_CAN1EN ((uint32_t)0x02000000) /*!< CAN1 clock enable */
+
+#define RCC_APB1ENR_BKPEN ((uint32_t)0x08000000) /*!< Backup interface clock enable */
+#define RCC_APB1ENR_PWREN ((uint32_t)0x10000000) /*!< Power interface clock enable */
+
+#define RCC_APB1ENR_TIM4EN ((uint32_t)0x00000004) /*!< Timer 4 clock enable */
+#define RCC_APB1ENR_SPI2EN ((uint32_t)0x00004000) /*!< SPI 2 clock enable */
+#define RCC_APB1ENR_USART3EN ((uint32_t)0x00040000) /*!< USART 3 clock enable */
+#define RCC_APB1ENR_I2C2EN ((uint32_t)0x00400000) /*!< I2C 2 clock enable */
+
+#define RCC_APB1ENR_USBEN ((uint32_t)0x00800000) /*!< USB Device clock enable */
+
+#define RCC_APB1ENR_TIM5EN ((uint32_t)0x00000008) /*!< Timer 5 clock enable */
+#define RCC_APB1ENR_TIM6EN ((uint32_t)0x00000010) /*!< Timer 6 clock enable */
+#define RCC_APB1ENR_TIM7EN ((uint32_t)0x00000020) /*!< Timer 7 clock enable */
+#define RCC_APB1ENR_SPI3EN ((uint32_t)0x00008000) /*!< SPI 3 clock enable */
+#define RCC_APB1ENR_UART4EN ((uint32_t)0x00080000) /*!< UART 4 clock enable */
+#define RCC_APB1ENR_UART5EN ((uint32_t)0x00100000) /*!< UART 5 clock enable */
+
+
+
+
+#define RCC_APB1ENR_DACEN ((uint32_t)0x20000000) /*!< DAC interface clock enable */
+
+/******************* Bit definition for RCC_BDCR register *******************/
+#define RCC_BDCR_LSEON ((uint32_t)0x00000001) /*!< External Low Speed oscillator enable */
+#define RCC_BDCR_LSERDY ((uint32_t)0x00000002) /*!< External Low Speed oscillator Ready */
+#define RCC_BDCR_LSEBYP ((uint32_t)0x00000004) /*!< External Low Speed oscillator Bypass */
+
+#define RCC_BDCR_RTCSEL ((uint32_t)0x00000300) /*!< RTCSEL[1:0] bits (RTC clock source selection) */
+#define RCC_BDCR_RTCSEL_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define RCC_BDCR_RTCSEL_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+
+/*!< RTC congiguration */
+#define RCC_BDCR_RTCSEL_NOCLOCK ((uint32_t)0x00000000) /*!< No clock */
+#define RCC_BDCR_RTCSEL_LSE ((uint32_t)0x00000100) /*!< LSE oscillator clock used as RTC clock */
+#define RCC_BDCR_RTCSEL_LSI ((uint32_t)0x00000200) /*!< LSI oscillator clock used as RTC clock */
+#define RCC_BDCR_RTCSEL_HSE ((uint32_t)0x00000300) /*!< HSE oscillator clock divided by 128 used as RTC clock */
+
+#define RCC_BDCR_RTCEN ((uint32_t)0x00008000) /*!< RTC clock enable */
+#define RCC_BDCR_BDRST ((uint32_t)0x00010000) /*!< Backup domain software reset */
+
+/******************* Bit definition for RCC_CSR register ********************/
+#define RCC_CSR_LSION ((uint32_t)0x00000001) /*!< Internal Low Speed oscillator enable */
+#define RCC_CSR_LSIRDY ((uint32_t)0x00000002) /*!< Internal Low Speed oscillator Ready */
+#define RCC_CSR_RMVF ((uint32_t)0x01000000) /*!< Remove reset flag */
+#define RCC_CSR_PINRSTF ((uint32_t)0x04000000) /*!< PIN reset flag */
+#define RCC_CSR_PORRSTF ((uint32_t)0x08000000) /*!< POR/PDR reset flag */
+#define RCC_CSR_SFTRSTF ((uint32_t)0x10000000) /*!< Software Reset flag */
+#define RCC_CSR_IWDGRSTF ((uint32_t)0x20000000) /*!< Independent Watchdog reset flag */
+#define RCC_CSR_WWDGRSTF ((uint32_t)0x40000000) /*!< Window watchdog reset flag */
+#define RCC_CSR_LPWRRSTF ((uint32_t)0x80000000) /*!< Low-Power reset flag */
+
+
+
+/******************************************************************************/
+/* */
+/* General Purpose and Alternate Function I/O */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for GPIO_CRL register *******************/
+#define GPIO_CRL_MODE ((uint32_t)0x33333333) /*!< Port x mode bits */
+
+#define GPIO_CRL_MODE0 ((uint32_t)0x00000003) /*!< MODE0[1:0] bits (Port x mode bits, pin 0) */
+#define GPIO_CRL_MODE0_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define GPIO_CRL_MODE0_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+
+#define GPIO_CRL_MODE1 ((uint32_t)0x00000030) /*!< MODE1[1:0] bits (Port x mode bits, pin 1) */
+#define GPIO_CRL_MODE1_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define GPIO_CRL_MODE1_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define GPIO_CRL_MODE2 ((uint32_t)0x00000300) /*!< MODE2[1:0] bits (Port x mode bits, pin 2) */
+#define GPIO_CRL_MODE2_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define GPIO_CRL_MODE2_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+
+#define GPIO_CRL_MODE3 ((uint32_t)0x00003000) /*!< MODE3[1:0] bits (Port x mode bits, pin 3) */
+#define GPIO_CRL_MODE3_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define GPIO_CRL_MODE3_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define GPIO_CRL_MODE4 ((uint32_t)0x00030000) /*!< MODE4[1:0] bits (Port x mode bits, pin 4) */
+#define GPIO_CRL_MODE4_0 ((uint32_t)0x00010000) /*!< Bit 0 */
+#define GPIO_CRL_MODE4_1 ((uint32_t)0x00020000) /*!< Bit 1 */
+
+#define GPIO_CRL_MODE5 ((uint32_t)0x00300000) /*!< MODE5[1:0] bits (Port x mode bits, pin 5) */
+#define GPIO_CRL_MODE5_0 ((uint32_t)0x00100000) /*!< Bit 0 */
+#define GPIO_CRL_MODE5_1 ((uint32_t)0x00200000) /*!< Bit 1 */
+
+#define GPIO_CRL_MODE6 ((uint32_t)0x03000000) /*!< MODE6[1:0] bits (Port x mode bits, pin 6) */
+#define GPIO_CRL_MODE6_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+#define GPIO_CRL_MODE6_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+
+#define GPIO_CRL_MODE7 ((uint32_t)0x30000000) /*!< MODE7[1:0] bits (Port x mode bits, pin 7) */
+#define GPIO_CRL_MODE7_0 ((uint32_t)0x10000000) /*!< Bit 0 */
+#define GPIO_CRL_MODE7_1 ((uint32_t)0x20000000) /*!< Bit 1 */
+
+#define GPIO_CRL_CNF ((uint32_t)0xCCCCCCCC) /*!< Port x configuration bits */
+
+#define GPIO_CRL_CNF0 ((uint32_t)0x0000000C) /*!< CNF0[1:0] bits (Port x configuration bits, pin 0) */
+#define GPIO_CRL_CNF0_0 ((uint32_t)0x00000004) /*!< Bit 0 */
+#define GPIO_CRL_CNF0_1 ((uint32_t)0x00000008) /*!< Bit 1 */
+
+#define GPIO_CRL_CNF1 ((uint32_t)0x000000C0) /*!< CNF1[1:0] bits (Port x configuration bits, pin 1) */
+#define GPIO_CRL_CNF1_0 ((uint32_t)0x00000040) /*!< Bit 0 */
+#define GPIO_CRL_CNF1_1 ((uint32_t)0x00000080) /*!< Bit 1 */
+
+#define GPIO_CRL_CNF2 ((uint32_t)0x00000C00) /*!< CNF2[1:0] bits (Port x configuration bits, pin 2) */
+#define GPIO_CRL_CNF2_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define GPIO_CRL_CNF2_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+
+#define GPIO_CRL_CNF3 ((uint32_t)0x0000C000) /*!< CNF3[1:0] bits (Port x configuration bits, pin 3) */
+#define GPIO_CRL_CNF3_0 ((uint32_t)0x00004000) /*!< Bit 0 */
+#define GPIO_CRL_CNF3_1 ((uint32_t)0x00008000) /*!< Bit 1 */
+
+#define GPIO_CRL_CNF4 ((uint32_t)0x000C0000) /*!< CNF4[1:0] bits (Port x configuration bits, pin 4) */
+#define GPIO_CRL_CNF4_0 ((uint32_t)0x00040000) /*!< Bit 0 */
+#define GPIO_CRL_CNF4_1 ((uint32_t)0x00080000) /*!< Bit 1 */
+
+#define GPIO_CRL_CNF5 ((uint32_t)0x00C00000) /*!< CNF5[1:0] bits (Port x configuration bits, pin 5) */
+#define GPIO_CRL_CNF5_0 ((uint32_t)0x00400000) /*!< Bit 0 */
+#define GPIO_CRL_CNF5_1 ((uint32_t)0x00800000) /*!< Bit 1 */
+
+#define GPIO_CRL_CNF6 ((uint32_t)0x0C000000) /*!< CNF6[1:0] bits (Port x configuration bits, pin 6) */
+#define GPIO_CRL_CNF6_0 ((uint32_t)0x04000000) /*!< Bit 0 */
+#define GPIO_CRL_CNF6_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+
+#define GPIO_CRL_CNF7 ((uint32_t)0xC0000000) /*!< CNF7[1:0] bits (Port x configuration bits, pin 7) */
+#define GPIO_CRL_CNF7_0 ((uint32_t)0x40000000) /*!< Bit 0 */
+#define GPIO_CRL_CNF7_1 ((uint32_t)0x80000000) /*!< Bit 1 */
+
+/******************* Bit definition for GPIO_CRH register *******************/
+#define GPIO_CRH_MODE ((uint32_t)0x33333333) /*!< Port x mode bits */
+
+#define GPIO_CRH_MODE8 ((uint32_t)0x00000003) /*!< MODE8[1:0] bits (Port x mode bits, pin 8) */
+#define GPIO_CRH_MODE8_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define GPIO_CRH_MODE8_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+
+#define GPIO_CRH_MODE9 ((uint32_t)0x00000030) /*!< MODE9[1:0] bits (Port x mode bits, pin 9) */
+#define GPIO_CRH_MODE9_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define GPIO_CRH_MODE9_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define GPIO_CRH_MODE10 ((uint32_t)0x00000300) /*!< MODE10[1:0] bits (Port x mode bits, pin 10) */
+#define GPIO_CRH_MODE10_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define GPIO_CRH_MODE10_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+
+#define GPIO_CRH_MODE11 ((uint32_t)0x00003000) /*!< MODE11[1:0] bits (Port x mode bits, pin 11) */
+#define GPIO_CRH_MODE11_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define GPIO_CRH_MODE11_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define GPIO_CRH_MODE12 ((uint32_t)0x00030000) /*!< MODE12[1:0] bits (Port x mode bits, pin 12) */
+#define GPIO_CRH_MODE12_0 ((uint32_t)0x00010000) /*!< Bit 0 */
+#define GPIO_CRH_MODE12_1 ((uint32_t)0x00020000) /*!< Bit 1 */
+
+#define GPIO_CRH_MODE13 ((uint32_t)0x00300000) /*!< MODE13[1:0] bits (Port x mode bits, pin 13) */
+#define GPIO_CRH_MODE13_0 ((uint32_t)0x00100000) /*!< Bit 0 */
+#define GPIO_CRH_MODE13_1 ((uint32_t)0x00200000) /*!< Bit 1 */
+
+#define GPIO_CRH_MODE14 ((uint32_t)0x03000000) /*!< MODE14[1:0] bits (Port x mode bits, pin 14) */
+#define GPIO_CRH_MODE14_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+#define GPIO_CRH_MODE14_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+
+#define GPIO_CRH_MODE15 ((uint32_t)0x30000000) /*!< MODE15[1:0] bits (Port x mode bits, pin 15) */
+#define GPIO_CRH_MODE15_0 ((uint32_t)0x10000000) /*!< Bit 0 */
+#define GPIO_CRH_MODE15_1 ((uint32_t)0x20000000) /*!< Bit 1 */
+
+#define GPIO_CRH_CNF ((uint32_t)0xCCCCCCCC) /*!< Port x configuration bits */
+
+#define GPIO_CRH_CNF8 ((uint32_t)0x0000000C) /*!< CNF8[1:0] bits (Port x configuration bits, pin 8) */
+#define GPIO_CRH_CNF8_0 ((uint32_t)0x00000004) /*!< Bit 0 */
+#define GPIO_CRH_CNF8_1 ((uint32_t)0x00000008) /*!< Bit 1 */
+
+#define GPIO_CRH_CNF9 ((uint32_t)0x000000C0) /*!< CNF9[1:0] bits (Port x configuration bits, pin 9) */
+#define GPIO_CRH_CNF9_0 ((uint32_t)0x00000040) /*!< Bit 0 */
+#define GPIO_CRH_CNF9_1 ((uint32_t)0x00000080) /*!< Bit 1 */
+
+#define GPIO_CRH_CNF10 ((uint32_t)0x00000C00) /*!< CNF10[1:0] bits (Port x configuration bits, pin 10) */
+#define GPIO_CRH_CNF10_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define GPIO_CRH_CNF10_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+
+#define GPIO_CRH_CNF11 ((uint32_t)0x0000C000) /*!< CNF11[1:0] bits (Port x configuration bits, pin 11) */
+#define GPIO_CRH_CNF11_0 ((uint32_t)0x00004000) /*!< Bit 0 */
+#define GPIO_CRH_CNF11_1 ((uint32_t)0x00008000) /*!< Bit 1 */
+
+#define GPIO_CRH_CNF12 ((uint32_t)0x000C0000) /*!< CNF12[1:0] bits (Port x configuration bits, pin 12) */
+#define GPIO_CRH_CNF12_0 ((uint32_t)0x00040000) /*!< Bit 0 */
+#define GPIO_CRH_CNF12_1 ((uint32_t)0x00080000) /*!< Bit 1 */
+
+#define GPIO_CRH_CNF13 ((uint32_t)0x00C00000) /*!< CNF13[1:0] bits (Port x configuration bits, pin 13) */
+#define GPIO_CRH_CNF13_0 ((uint32_t)0x00400000) /*!< Bit 0 */
+#define GPIO_CRH_CNF13_1 ((uint32_t)0x00800000) /*!< Bit 1 */
+
+#define GPIO_CRH_CNF14 ((uint32_t)0x0C000000) /*!< CNF14[1:0] bits (Port x configuration bits, pin 14) */
+#define GPIO_CRH_CNF14_0 ((uint32_t)0x04000000) /*!< Bit 0 */
+#define GPIO_CRH_CNF14_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+
+#define GPIO_CRH_CNF15 ((uint32_t)0xC0000000) /*!< CNF15[1:0] bits (Port x configuration bits, pin 15) */
+#define GPIO_CRH_CNF15_0 ((uint32_t)0x40000000) /*!< Bit 0 */
+#define GPIO_CRH_CNF15_1 ((uint32_t)0x80000000) /*!< Bit 1 */
+
+/*!<****************** Bit definition for GPIO_IDR register *******************/
+#define GPIO_IDR_IDR0 ((uint32_t)0x0001) /*!< Port input data, bit 0 */
+#define GPIO_IDR_IDR1 ((uint32_t)0x0002) /*!< Port input data, bit 1 */
+#define GPIO_IDR_IDR2 ((uint32_t)0x0004) /*!< Port input data, bit 2 */
+#define GPIO_IDR_IDR3 ((uint32_t)0x0008) /*!< Port input data, bit 3 */
+#define GPIO_IDR_IDR4 ((uint32_t)0x0010) /*!< Port input data, bit 4 */
+#define GPIO_IDR_IDR5 ((uint32_t)0x0020) /*!< Port input data, bit 5 */
+#define GPIO_IDR_IDR6 ((uint32_t)0x0040) /*!< Port input data, bit 6 */
+#define GPIO_IDR_IDR7 ((uint32_t)0x0080) /*!< Port input data, bit 7 */
+#define GPIO_IDR_IDR8 ((uint32_t)0x0100) /*!< Port input data, bit 8 */
+#define GPIO_IDR_IDR9 ((uint32_t)0x0200) /*!< Port input data, bit 9 */
+#define GPIO_IDR_IDR10 ((uint32_t)0x0400) /*!< Port input data, bit 10 */
+#define GPIO_IDR_IDR11 ((uint32_t)0x0800) /*!< Port input data, bit 11 */
+#define GPIO_IDR_IDR12 ((uint32_t)0x1000) /*!< Port input data, bit 12 */
+#define GPIO_IDR_IDR13 ((uint32_t)0x2000) /*!< Port input data, bit 13 */
+#define GPIO_IDR_IDR14 ((uint32_t)0x4000) /*!< Port input data, bit 14 */
+#define GPIO_IDR_IDR15 ((uint32_t)0x8000) /*!< Port input data, bit 15 */
+
+/******************* Bit definition for GPIO_ODR register *******************/
+#define GPIO_ODR_ODR0 ((uint32_t)0x0001) /*!< Port output data, bit 0 */
+#define GPIO_ODR_ODR1 ((uint32_t)0x0002) /*!< Port output data, bit 1 */
+#define GPIO_ODR_ODR2 ((uint32_t)0x0004) /*!< Port output data, bit 2 */
+#define GPIO_ODR_ODR3 ((uint32_t)0x0008) /*!< Port output data, bit 3 */
+#define GPIO_ODR_ODR4 ((uint32_t)0x0010) /*!< Port output data, bit 4 */
+#define GPIO_ODR_ODR5 ((uint32_t)0x0020) /*!< Port output data, bit 5 */
+#define GPIO_ODR_ODR6 ((uint32_t)0x0040) /*!< Port output data, bit 6 */
+#define GPIO_ODR_ODR7 ((uint32_t)0x0080) /*!< Port output data, bit 7 */
+#define GPIO_ODR_ODR8 ((uint32_t)0x0100) /*!< Port output data, bit 8 */
+#define GPIO_ODR_ODR9 ((uint32_t)0x0200) /*!< Port output data, bit 9 */
+#define GPIO_ODR_ODR10 ((uint32_t)0x0400) /*!< Port output data, bit 10 */
+#define GPIO_ODR_ODR11 ((uint32_t)0x0800) /*!< Port output data, bit 11 */
+#define GPIO_ODR_ODR12 ((uint32_t)0x1000) /*!< Port output data, bit 12 */
+#define GPIO_ODR_ODR13 ((uint32_t)0x2000) /*!< Port output data, bit 13 */
+#define GPIO_ODR_ODR14 ((uint32_t)0x4000) /*!< Port output data, bit 14 */
+#define GPIO_ODR_ODR15 ((uint32_t)0x8000) /*!< Port output data, bit 15 */
+
+/****************** Bit definition for GPIO_BSRR register *******************/
+#define GPIO_BSRR_BS0 ((uint32_t)0x00000001) /*!< Port x Set bit 0 */
+#define GPIO_BSRR_BS1 ((uint32_t)0x00000002) /*!< Port x Set bit 1 */
+#define GPIO_BSRR_BS2 ((uint32_t)0x00000004) /*!< Port x Set bit 2 */
+#define GPIO_BSRR_BS3 ((uint32_t)0x00000008) /*!< Port x Set bit 3 */
+#define GPIO_BSRR_BS4 ((uint32_t)0x00000010) /*!< Port x Set bit 4 */
+#define GPIO_BSRR_BS5 ((uint32_t)0x00000020) /*!< Port x Set bit 5 */
+#define GPIO_BSRR_BS6 ((uint32_t)0x00000040) /*!< Port x Set bit 6 */
+#define GPIO_BSRR_BS7 ((uint32_t)0x00000080) /*!< Port x Set bit 7 */
+#define GPIO_BSRR_BS8 ((uint32_t)0x00000100) /*!< Port x Set bit 8 */
+#define GPIO_BSRR_BS9 ((uint32_t)0x00000200) /*!< Port x Set bit 9 */
+#define GPIO_BSRR_BS10 ((uint32_t)0x00000400) /*!< Port x Set bit 10 */
+#define GPIO_BSRR_BS11 ((uint32_t)0x00000800) /*!< Port x Set bit 11 */
+#define GPIO_BSRR_BS12 ((uint32_t)0x00001000) /*!< Port x Set bit 12 */
+#define GPIO_BSRR_BS13 ((uint32_t)0x00002000) /*!< Port x Set bit 13 */
+#define GPIO_BSRR_BS14 ((uint32_t)0x00004000) /*!< Port x Set bit 14 */
+#define GPIO_BSRR_BS15 ((uint32_t)0x00008000) /*!< Port x Set bit 15 */
+
+#define GPIO_BSRR_BR0 ((uint32_t)0x00010000) /*!< Port x Reset bit 0 */
+#define GPIO_BSRR_BR1 ((uint32_t)0x00020000) /*!< Port x Reset bit 1 */
+#define GPIO_BSRR_BR2 ((uint32_t)0x00040000) /*!< Port x Reset bit 2 */
+#define GPIO_BSRR_BR3 ((uint32_t)0x00080000) /*!< Port x Reset bit 3 */
+#define GPIO_BSRR_BR4 ((uint32_t)0x00100000) /*!< Port x Reset bit 4 */
+#define GPIO_BSRR_BR5 ((uint32_t)0x00200000) /*!< Port x Reset bit 5 */
+#define GPIO_BSRR_BR6 ((uint32_t)0x00400000) /*!< Port x Reset bit 6 */
+#define GPIO_BSRR_BR7 ((uint32_t)0x00800000) /*!< Port x Reset bit 7 */
+#define GPIO_BSRR_BR8 ((uint32_t)0x01000000) /*!< Port x Reset bit 8 */
+#define GPIO_BSRR_BR9 ((uint32_t)0x02000000) /*!< Port x Reset bit 9 */
+#define GPIO_BSRR_BR10 ((uint32_t)0x04000000) /*!< Port x Reset bit 10 */
+#define GPIO_BSRR_BR11 ((uint32_t)0x08000000) /*!< Port x Reset bit 11 */
+#define GPIO_BSRR_BR12 ((uint32_t)0x10000000) /*!< Port x Reset bit 12 */
+#define GPIO_BSRR_BR13 ((uint32_t)0x20000000) /*!< Port x Reset bit 13 */
+#define GPIO_BSRR_BR14 ((uint32_t)0x40000000) /*!< Port x Reset bit 14 */
+#define GPIO_BSRR_BR15 ((uint32_t)0x80000000) /*!< Port x Reset bit 15 */
+
+/******************* Bit definition for GPIO_BRR register *******************/
+#define GPIO_BRR_BR0 ((uint32_t)0x0001) /*!< Port x Reset bit 0 */
+#define GPIO_BRR_BR1 ((uint32_t)0x0002) /*!< Port x Reset bit 1 */
+#define GPIO_BRR_BR2 ((uint32_t)0x0004) /*!< Port x Reset bit 2 */
+#define GPIO_BRR_BR3 ((uint32_t)0x0008) /*!< Port x Reset bit 3 */
+#define GPIO_BRR_BR4 ((uint32_t)0x0010) /*!< Port x Reset bit 4 */
+#define GPIO_BRR_BR5 ((uint32_t)0x0020) /*!< Port x Reset bit 5 */
+#define GPIO_BRR_BR6 ((uint32_t)0x0040) /*!< Port x Reset bit 6 */
+#define GPIO_BRR_BR7 ((uint32_t)0x0080) /*!< Port x Reset bit 7 */
+#define GPIO_BRR_BR8 ((uint32_t)0x0100) /*!< Port x Reset bit 8 */
+#define GPIO_BRR_BR9 ((uint32_t)0x0200) /*!< Port x Reset bit 9 */
+#define GPIO_BRR_BR10 ((uint32_t)0x0400) /*!< Port x Reset bit 10 */
+#define GPIO_BRR_BR11 ((uint32_t)0x0800) /*!< Port x Reset bit 11 */
+#define GPIO_BRR_BR12 ((uint32_t)0x1000) /*!< Port x Reset bit 12 */
+#define GPIO_BRR_BR13 ((uint32_t)0x2000) /*!< Port x Reset bit 13 */
+#define GPIO_BRR_BR14 ((uint32_t)0x4000) /*!< Port x Reset bit 14 */
+#define GPIO_BRR_BR15 ((uint32_t)0x8000) /*!< Port x Reset bit 15 */
+
+/****************** Bit definition for GPIO_LCKR register *******************/
+#define GPIO_LCKR_LCK0 ((uint32_t)0x00000001) /*!< Port x Lock bit 0 */
+#define GPIO_LCKR_LCK1 ((uint32_t)0x00000002) /*!< Port x Lock bit 1 */
+#define GPIO_LCKR_LCK2 ((uint32_t)0x00000004) /*!< Port x Lock bit 2 */
+#define GPIO_LCKR_LCK3 ((uint32_t)0x00000008) /*!< Port x Lock bit 3 */
+#define GPIO_LCKR_LCK4 ((uint32_t)0x00000010) /*!< Port x Lock bit 4 */
+#define GPIO_LCKR_LCK5 ((uint32_t)0x00000020) /*!< Port x Lock bit 5 */
+#define GPIO_LCKR_LCK6 ((uint32_t)0x00000040) /*!< Port x Lock bit 6 */
+#define GPIO_LCKR_LCK7 ((uint32_t)0x00000080) /*!< Port x Lock bit 7 */
+#define GPIO_LCKR_LCK8 ((uint32_t)0x00000100) /*!< Port x Lock bit 8 */
+#define GPIO_LCKR_LCK9 ((uint32_t)0x00000200) /*!< Port x Lock bit 9 */
+#define GPIO_LCKR_LCK10 ((uint32_t)0x00000400) /*!< Port x Lock bit 10 */
+#define GPIO_LCKR_LCK11 ((uint32_t)0x00000800) /*!< Port x Lock bit 11 */
+#define GPIO_LCKR_LCK12 ((uint32_t)0x00001000) /*!< Port x Lock bit 12 */
+#define GPIO_LCKR_LCK13 ((uint32_t)0x00002000) /*!< Port x Lock bit 13 */
+#define GPIO_LCKR_LCK14 ((uint32_t)0x00004000) /*!< Port x Lock bit 14 */
+#define GPIO_LCKR_LCK15 ((uint32_t)0x00008000) /*!< Port x Lock bit 15 */
+#define GPIO_LCKR_LCKK ((uint32_t)0x00010000) /*!< Lock key */
+
+/*----------------------------------------------------------------------------*/
+
+/****************** Bit definition for AFIO_EVCR register *******************/
+#define AFIO_EVCR_PIN ((uint32_t)0x0000000F) /*!< PIN[3:0] bits (Pin selection) */
+#define AFIO_EVCR_PIN_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define AFIO_EVCR_PIN_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define AFIO_EVCR_PIN_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define AFIO_EVCR_PIN_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+
+/*!< PIN configuration */
+#define AFIO_EVCR_PIN_PX0 ((uint32_t)0x00000000) /*!< Pin 0 selected */
+#define AFIO_EVCR_PIN_PX1 ((uint32_t)0x00000001) /*!< Pin 1 selected */
+#define AFIO_EVCR_PIN_PX2 ((uint32_t)0x00000002) /*!< Pin 2 selected */
+#define AFIO_EVCR_PIN_PX3 ((uint32_t)0x00000003) /*!< Pin 3 selected */
+#define AFIO_EVCR_PIN_PX4 ((uint32_t)0x00000004) /*!< Pin 4 selected */
+#define AFIO_EVCR_PIN_PX5 ((uint32_t)0x00000005) /*!< Pin 5 selected */
+#define AFIO_EVCR_PIN_PX6 ((uint32_t)0x00000006) /*!< Pin 6 selected */
+#define AFIO_EVCR_PIN_PX7 ((uint32_t)0x00000007) /*!< Pin 7 selected */
+#define AFIO_EVCR_PIN_PX8 ((uint32_t)0x00000008) /*!< Pin 8 selected */
+#define AFIO_EVCR_PIN_PX9 ((uint32_t)0x00000009) /*!< Pin 9 selected */
+#define AFIO_EVCR_PIN_PX10 ((uint32_t)0x0000000A) /*!< Pin 10 selected */
+#define AFIO_EVCR_PIN_PX11 ((uint32_t)0x0000000B) /*!< Pin 11 selected */
+#define AFIO_EVCR_PIN_PX12 ((uint32_t)0x0000000C) /*!< Pin 12 selected */
+#define AFIO_EVCR_PIN_PX13 ((uint32_t)0x0000000D) /*!< Pin 13 selected */
+#define AFIO_EVCR_PIN_PX14 ((uint32_t)0x0000000E) /*!< Pin 14 selected */
+#define AFIO_EVCR_PIN_PX15 ((uint32_t)0x0000000F) /*!< Pin 15 selected */
+
+#define AFIO_EVCR_PORT ((uint32_t)0x00000070) /*!< PORT[2:0] bits (Port selection) */
+#define AFIO_EVCR_PORT_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define AFIO_EVCR_PORT_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+#define AFIO_EVCR_PORT_2 ((uint32_t)0x00000040) /*!< Bit 2 */
+
+/*!< PORT configuration */
+#define AFIO_EVCR_PORT_PA ((uint32_t)0x00000000) /*!< Port A selected */
+#define AFIO_EVCR_PORT_PB ((uint32_t)0x00000010) /*!< Port B selected */
+#define AFIO_EVCR_PORT_PC ((uint32_t)0x00000020) /*!< Port C selected */
+#define AFIO_EVCR_PORT_PD ((uint32_t)0x00000030) /*!< Port D selected */
+#define AFIO_EVCR_PORT_PE ((uint32_t)0x00000040) /*!< Port E selected */
+
+#define AFIO_EVCR_EVOE ((uint32_t)0x00000080) /*!< Event Output Enable */
+
+/****************** Bit definition for AFIO_MAPR register *******************/
+#define AFIO_MAPR_SPI1_REMAP ((uint32_t)0x00000001) /*!< SPI1 remapping */
+#define AFIO_MAPR_I2C1_REMAP ((uint32_t)0x00000002) /*!< I2C1 remapping */
+#define AFIO_MAPR_USART1_REMAP ((uint32_t)0x00000004) /*!< USART1 remapping */
+#define AFIO_MAPR_USART2_REMAP ((uint32_t)0x00000008) /*!< USART2 remapping */
+
+#define AFIO_MAPR_USART3_REMAP ((uint32_t)0x00000030) /*!< USART3_REMAP[1:0] bits (USART3 remapping) */
+#define AFIO_MAPR_USART3_REMAP_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define AFIO_MAPR_USART3_REMAP_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+/* USART3_REMAP configuration */
+#define AFIO_MAPR_USART3_REMAP_NOREMAP ((uint32_t)0x00000000) /*!< No remap (TX/PB10, RX/PB11, CK/PB12, CTS/PB13, RTS/PB14) */
+#define AFIO_MAPR_USART3_REMAP_PARTIALREMAP ((uint32_t)0x00000010) /*!< Partial remap (TX/PC10, RX/PC11, CK/PC12, CTS/PB13, RTS/PB14) */
+#define AFIO_MAPR_USART3_REMAP_FULLREMAP ((uint32_t)0x00000030) /*!< Full remap (TX/PD8, RX/PD9, CK/PD10, CTS/PD11, RTS/PD12) */
+
+#define AFIO_MAPR_TIM1_REMAP ((uint32_t)0x000000C0) /*!< TIM1_REMAP[1:0] bits (TIM1 remapping) */
+#define AFIO_MAPR_TIM1_REMAP_0 ((uint32_t)0x00000040) /*!< Bit 0 */
+#define AFIO_MAPR_TIM1_REMAP_1 ((uint32_t)0x00000080) /*!< Bit 1 */
+
+/*!< TIM1_REMAP configuration */
+#define AFIO_MAPR_TIM1_REMAP_NOREMAP ((uint32_t)0x00000000) /*!< No remap (ETR/PA12, CH1/PA8, CH2/PA9, CH3/PA10, CH4/PA11, BKIN/PB12, CH1N/PB13, CH2N/PB14, CH3N/PB15) */
+#define AFIO_MAPR_TIM1_REMAP_PARTIALREMAP ((uint32_t)0x00000040) /*!< Partial remap (ETR/PA12, CH1/PA8, CH2/PA9, CH3/PA10, CH4/PA11, BKIN/PA6, CH1N/PA7, CH2N/PB0, CH3N/PB1) */
+#define AFIO_MAPR_TIM1_REMAP_FULLREMAP ((uint32_t)0x000000C0) /*!< Full remap (ETR/PE7, CH1/PE9, CH2/PE11, CH3/PE13, CH4/PE14, BKIN/PE15, CH1N/PE8, CH2N/PE10, CH3N/PE12) */
+
+#define AFIO_MAPR_TIM2_REMAP ((uint32_t)0x00000300) /*!< TIM2_REMAP[1:0] bits (TIM2 remapping) */
+#define AFIO_MAPR_TIM2_REMAP_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define AFIO_MAPR_TIM2_REMAP_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+
+/*!< TIM2_REMAP configuration */
+#define AFIO_MAPR_TIM2_REMAP_NOREMAP ((uint32_t)0x00000000) /*!< No remap (CH1/ETR/PA0, CH2/PA1, CH3/PA2, CH4/PA3) */
+#define AFIO_MAPR_TIM2_REMAP_PARTIALREMAP1 ((uint32_t)0x00000100) /*!< Partial remap (CH1/ETR/PA15, CH2/PB3, CH3/PA2, CH4/PA3) */
+#define AFIO_MAPR_TIM2_REMAP_PARTIALREMAP2 ((uint32_t)0x00000200) /*!< Partial remap (CH1/ETR/PA0, CH2/PA1, CH3/PB10, CH4/PB11) */
+#define AFIO_MAPR_TIM2_REMAP_FULLREMAP ((uint32_t)0x00000300) /*!< Full remap (CH1/ETR/PA15, CH2/PB3, CH3/PB10, CH4/PB11) */
+
+#define AFIO_MAPR_TIM3_REMAP ((uint32_t)0x00000C00) /*!< TIM3_REMAP[1:0] bits (TIM3 remapping) */
+#define AFIO_MAPR_TIM3_REMAP_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define AFIO_MAPR_TIM3_REMAP_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+
+/*!< TIM3_REMAP configuration */
+#define AFIO_MAPR_TIM3_REMAP_NOREMAP ((uint32_t)0x00000000) /*!< No remap (CH1/PA6, CH2/PA7, CH3/PB0, CH4/PB1) */
+#define AFIO_MAPR_TIM3_REMAP_PARTIALREMAP ((uint32_t)0x00000800) /*!< Partial remap (CH1/PB4, CH2/PB5, CH3/PB0, CH4/PB1) */
+#define AFIO_MAPR_TIM3_REMAP_FULLREMAP ((uint32_t)0x00000C00) /*!< Full remap (CH1/PC6, CH2/PC7, CH3/PC8, CH4/PC9) */
+
+#define AFIO_MAPR_TIM4_REMAP ((uint32_t)0x00001000) /*!< TIM4_REMAP bit (TIM4 remapping) */
+
+#define AFIO_MAPR_CAN_REMAP ((uint32_t)0x00006000) /*!< CAN_REMAP[1:0] bits (CAN Alternate function remapping) */
+#define AFIO_MAPR_CAN_REMAP_0 ((uint32_t)0x00002000) /*!< Bit 0 */
+#define AFIO_MAPR_CAN_REMAP_1 ((uint32_t)0x00004000) /*!< Bit 1 */
+
+/*!< CAN_REMAP configuration */
+#define AFIO_MAPR_CAN_REMAP_REMAP1 ((uint32_t)0x00000000) /*!< CANRX mapped to PA11, CANTX mapped to PA12 */
+#define AFIO_MAPR_CAN_REMAP_REMAP2 ((uint32_t)0x00004000) /*!< CANRX mapped to PB8, CANTX mapped to PB9 */
+#define AFIO_MAPR_CAN_REMAP_REMAP3 ((uint32_t)0x00006000) /*!< CANRX mapped to PD0, CANTX mapped to PD1 */
+
+#define AFIO_MAPR_PD01_REMAP ((uint32_t)0x00008000) /*!< Port D0/Port D1 mapping on OSC_IN/OSC_OUT */
+#define AFIO_MAPR_TIM5CH4_IREMAP ((uint32_t)0x00010000) /*!< TIM5 Channel4 Internal Remap */
+#define AFIO_MAPR_ADC1_ETRGINJ_REMAP ((uint32_t)0x00020000) /*!< ADC 1 External Trigger Injected Conversion remapping */
+#define AFIO_MAPR_ADC1_ETRGREG_REMAP ((uint32_t)0x00040000) /*!< ADC 1 External Trigger Regular Conversion remapping */
+#define AFIO_MAPR_ADC2_ETRGINJ_REMAP ((uint32_t)0x00080000) /*!< ADC 2 External Trigger Injected Conversion remapping */
+#define AFIO_MAPR_ADC2_ETRGREG_REMAP ((uint32_t)0x00100000) /*!< ADC 2 External Trigger Regular Conversion remapping */
+
+/*!< SWJ_CFG configuration */
+#define AFIO_MAPR_SWJ_CFG ((uint32_t)0x07000000) /*!< SWJ_CFG[2:0] bits (Serial Wire JTAG configuration) */
+#define AFIO_MAPR_SWJ_CFG_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+#define AFIO_MAPR_SWJ_CFG_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+#define AFIO_MAPR_SWJ_CFG_2 ((uint32_t)0x04000000) /*!< Bit 2 */
+
+#define AFIO_MAPR_SWJ_CFG_RESET ((uint32_t)0x00000000) /*!< Full SWJ (JTAG-DP + SW-DP) : Reset State */
+#define AFIO_MAPR_SWJ_CFG_NOJNTRST ((uint32_t)0x01000000) /*!< Full SWJ (JTAG-DP + SW-DP) but without JNTRST */
+#define AFIO_MAPR_SWJ_CFG_JTAGDISABLE ((uint32_t)0x02000000) /*!< JTAG-DP Disabled and SW-DP Enabled */
+#define AFIO_MAPR_SWJ_CFG_DISABLE ((uint32_t)0x04000000) /*!< JTAG-DP Disabled and SW-DP Disabled */
+
+
+/***************** Bit definition for AFIO_EXTICR1 register *****************/
+#define AFIO_EXTICR1_EXTI0 ((uint32_t)0x0000000F) /*!< EXTI 0 configuration */
+#define AFIO_EXTICR1_EXTI1 ((uint32_t)0x000000F0) /*!< EXTI 1 configuration */
+#define AFIO_EXTICR1_EXTI2 ((uint32_t)0x00000F00) /*!< EXTI 2 configuration */
+#define AFIO_EXTICR1_EXTI3 ((uint32_t)0x0000F000) /*!< EXTI 3 configuration */
+
+/*!< EXTI0 configuration */
+#define AFIO_EXTICR1_EXTI0_PA ((uint32_t)0x00000000) /*!< PA[0] pin */
+#define AFIO_EXTICR1_EXTI0_PB ((uint32_t)0x00000001) /*!< PB[0] pin */
+#define AFIO_EXTICR1_EXTI0_PC ((uint32_t)0x00000002) /*!< PC[0] pin */
+#define AFIO_EXTICR1_EXTI0_PD ((uint32_t)0x00000003) /*!< PD[0] pin */
+#define AFIO_EXTICR1_EXTI0_PE ((uint32_t)0x00000004) /*!< PE[0] pin */
+#define AFIO_EXTICR1_EXTI0_PF ((uint32_t)0x00000005) /*!< PF[0] pin */
+#define AFIO_EXTICR1_EXTI0_PG ((uint32_t)0x00000006) /*!< PG[0] pin */
+
+/*!< EXTI1 configuration */
+#define AFIO_EXTICR1_EXTI1_PA ((uint32_t)0x00000000) /*!< PA[1] pin */
+#define AFIO_EXTICR1_EXTI1_PB ((uint32_t)0x00000010) /*!< PB[1] pin */
+#define AFIO_EXTICR1_EXTI1_PC ((uint32_t)0x00000020) /*!< PC[1] pin */
+#define AFIO_EXTICR1_EXTI1_PD ((uint32_t)0x00000030) /*!< PD[1] pin */
+#define AFIO_EXTICR1_EXTI1_PE ((uint32_t)0x00000040) /*!< PE[1] pin */
+#define AFIO_EXTICR1_EXTI1_PF ((uint32_t)0x00000050) /*!< PF[1] pin */
+#define AFIO_EXTICR1_EXTI1_PG ((uint32_t)0x00000060) /*!< PG[1] pin */
+
+/*!< EXTI2 configuration */
+#define AFIO_EXTICR1_EXTI2_PA ((uint32_t)0x00000000) /*!< PA[2] pin */
+#define AFIO_EXTICR1_EXTI2_PB ((uint32_t)0x00000100) /*!< PB[2] pin */
+#define AFIO_EXTICR1_EXTI2_PC ((uint32_t)0x00000200) /*!< PC[2] pin */
+#define AFIO_EXTICR1_EXTI2_PD ((uint32_t)0x00000300) /*!< PD[2] pin */
+#define AFIO_EXTICR1_EXTI2_PE ((uint32_t)0x00000400) /*!< PE[2] pin */
+#define AFIO_EXTICR1_EXTI2_PF ((uint32_t)0x00000500) /*!< PF[2] pin */
+#define AFIO_EXTICR1_EXTI2_PG ((uint32_t)0x00000600) /*!< PG[2] pin */
+
+/*!< EXTI3 configuration */
+#define AFIO_EXTICR1_EXTI3_PA ((uint32_t)0x00000000) /*!< PA[3] pin */
+#define AFIO_EXTICR1_EXTI3_PB ((uint32_t)0x00001000) /*!< PB[3] pin */
+#define AFIO_EXTICR1_EXTI3_PC ((uint32_t)0x00002000) /*!< PC[3] pin */
+#define AFIO_EXTICR1_EXTI3_PD ((uint32_t)0x00003000) /*!< PD[3] pin */
+#define AFIO_EXTICR1_EXTI3_PE ((uint32_t)0x00004000) /*!< PE[3] pin */
+#define AFIO_EXTICR1_EXTI3_PF ((uint32_t)0x00005000) /*!< PF[3] pin */
+#define AFIO_EXTICR1_EXTI3_PG ((uint32_t)0x00006000) /*!< PG[3] pin */
+
+/***************** Bit definition for AFIO_EXTICR2 register *****************/
+#define AFIO_EXTICR2_EXTI4 ((uint32_t)0x0000000F) /*!< EXTI 4 configuration */
+#define AFIO_EXTICR2_EXTI5 ((uint32_t)0x000000F0) /*!< EXTI 5 configuration */
+#define AFIO_EXTICR2_EXTI6 ((uint32_t)0x00000F00) /*!< EXTI 6 configuration */
+#define AFIO_EXTICR2_EXTI7 ((uint32_t)0x0000F000) /*!< EXTI 7 configuration */
+
+/*!< EXTI4 configuration */
+#define AFIO_EXTICR2_EXTI4_PA ((uint32_t)0x00000000) /*!< PA[4] pin */
+#define AFIO_EXTICR2_EXTI4_PB ((uint32_t)0x00000001) /*!< PB[4] pin */
+#define AFIO_EXTICR2_EXTI4_PC ((uint32_t)0x00000002) /*!< PC[4] pin */
+#define AFIO_EXTICR2_EXTI4_PD ((uint32_t)0x00000003) /*!< PD[4] pin */
+#define AFIO_EXTICR2_EXTI4_PE ((uint32_t)0x00000004) /*!< PE[4] pin */
+#define AFIO_EXTICR2_EXTI4_PF ((uint32_t)0x00000005) /*!< PF[4] pin */
+#define AFIO_EXTICR2_EXTI4_PG ((uint32_t)0x00000006) /*!< PG[4] pin */
+
+/* EXTI5 configuration */
+#define AFIO_EXTICR2_EXTI5_PA ((uint32_t)0x00000000) /*!< PA[5] pin */
+#define AFIO_EXTICR2_EXTI5_PB ((uint32_t)0x00000010) /*!< PB[5] pin */
+#define AFIO_EXTICR2_EXTI5_PC ((uint32_t)0x00000020) /*!< PC[5] pin */
+#define AFIO_EXTICR2_EXTI5_PD ((uint32_t)0x00000030) /*!< PD[5] pin */
+#define AFIO_EXTICR2_EXTI5_PE ((uint32_t)0x00000040) /*!< PE[5] pin */
+#define AFIO_EXTICR2_EXTI5_PF ((uint32_t)0x00000050) /*!< PF[5] pin */
+#define AFIO_EXTICR2_EXTI5_PG ((uint32_t)0x00000060) /*!< PG[5] pin */
+
+/*!< EXTI6 configuration */
+#define AFIO_EXTICR2_EXTI6_PA ((uint32_t)0x00000000) /*!< PA[6] pin */
+#define AFIO_EXTICR2_EXTI6_PB ((uint32_t)0x00000100) /*!< PB[6] pin */
+#define AFIO_EXTICR2_EXTI6_PC ((uint32_t)0x00000200) /*!< PC[6] pin */
+#define AFIO_EXTICR2_EXTI6_PD ((uint32_t)0x00000300) /*!< PD[6] pin */
+#define AFIO_EXTICR2_EXTI6_PE ((uint32_t)0x00000400) /*!< PE[6] pin */
+#define AFIO_EXTICR2_EXTI6_PF ((uint32_t)0x00000500) /*!< PF[6] pin */
+#define AFIO_EXTICR2_EXTI6_PG ((uint32_t)0x00000600) /*!< PG[6] pin */
+
+/*!< EXTI7 configuration */
+#define AFIO_EXTICR2_EXTI7_PA ((uint32_t)0x00000000) /*!< PA[7] pin */
+#define AFIO_EXTICR2_EXTI7_PB ((uint32_t)0x00001000) /*!< PB[7] pin */
+#define AFIO_EXTICR2_EXTI7_PC ((uint32_t)0x00002000) /*!< PC[7] pin */
+#define AFIO_EXTICR2_EXTI7_PD ((uint32_t)0x00003000) /*!< PD[7] pin */
+#define AFIO_EXTICR2_EXTI7_PE ((uint32_t)0x00004000) /*!< PE[7] pin */
+#define AFIO_EXTICR2_EXTI7_PF ((uint32_t)0x00005000) /*!< PF[7] pin */
+#define AFIO_EXTICR2_EXTI7_PG ((uint32_t)0x00006000) /*!< PG[7] pin */
+
+/***************** Bit definition for AFIO_EXTICR3 register *****************/
+#define AFIO_EXTICR3_EXTI8 ((uint32_t)0x0000000F) /*!< EXTI 8 configuration */
+#define AFIO_EXTICR3_EXTI9 ((uint32_t)0x000000F0) /*!< EXTI 9 configuration */
+#define AFIO_EXTICR3_EXTI10 ((uint32_t)0x00000F00) /*!< EXTI 10 configuration */
+#define AFIO_EXTICR3_EXTI11 ((uint32_t)0x0000F000) /*!< EXTI 11 configuration */
+
+/*!< EXTI8 configuration */
+#define AFIO_EXTICR3_EXTI8_PA ((uint32_t)0x00000000) /*!< PA[8] pin */
+#define AFIO_EXTICR3_EXTI8_PB ((uint32_t)0x00000001) /*!< PB[8] pin */
+#define AFIO_EXTICR3_EXTI8_PC ((uint32_t)0x00000002) /*!< PC[8] pin */
+#define AFIO_EXTICR3_EXTI8_PD ((uint32_t)0x00000003) /*!< PD[8] pin */
+#define AFIO_EXTICR3_EXTI8_PE ((uint32_t)0x00000004) /*!< PE[8] pin */
+#define AFIO_EXTICR3_EXTI8_PF ((uint32_t)0x00000005) /*!< PF[8] pin */
+#define AFIO_EXTICR3_EXTI8_PG ((uint32_t)0x00000006) /*!< PG[8] pin */
+
+/*!< EXTI9 configuration */
+#define AFIO_EXTICR3_EXTI9_PA ((uint32_t)0x00000000) /*!< PA[9] pin */
+#define AFIO_EXTICR3_EXTI9_PB ((uint32_t)0x00000010) /*!< PB[9] pin */
+#define AFIO_EXTICR3_EXTI9_PC ((uint32_t)0x00000020) /*!< PC[9] pin */
+#define AFIO_EXTICR3_EXTI9_PD ((uint32_t)0x00000030) /*!< PD[9] pin */
+#define AFIO_EXTICR3_EXTI9_PE ((uint32_t)0x00000040) /*!< PE[9] pin */
+#define AFIO_EXTICR3_EXTI9_PF ((uint32_t)0x00000050) /*!< PF[9] pin */
+#define AFIO_EXTICR3_EXTI9_PG ((uint32_t)0x00000060) /*!< PG[9] pin */
+
+/*!< EXTI10 configuration */
+#define AFIO_EXTICR3_EXTI10_PA ((uint32_t)0x00000000) /*!< PA[10] pin */
+#define AFIO_EXTICR3_EXTI10_PB ((uint32_t)0x00000100) /*!< PB[10] pin */
+#define AFIO_EXTICR3_EXTI10_PC ((uint32_t)0x00000200) /*!< PC[10] pin */
+#define AFIO_EXTICR3_EXTI10_PD ((uint32_t)0x00000300) /*!< PD[10] pin */
+#define AFIO_EXTICR3_EXTI10_PE ((uint32_t)0x00000400) /*!< PE[10] pin */
+#define AFIO_EXTICR3_EXTI10_PF ((uint32_t)0x00000500) /*!< PF[10] pin */
+#define AFIO_EXTICR3_EXTI10_PG ((uint32_t)0x00000600) /*!< PG[10] pin */
+
+/*!< EXTI11 configuration */
+#define AFIO_EXTICR3_EXTI11_PA ((uint32_t)0x00000000) /*!< PA[11] pin */
+#define AFIO_EXTICR3_EXTI11_PB ((uint32_t)0x00001000) /*!< PB[11] pin */
+#define AFIO_EXTICR3_EXTI11_PC ((uint32_t)0x00002000) /*!< PC[11] pin */
+#define AFIO_EXTICR3_EXTI11_PD ((uint32_t)0x00003000) /*!< PD[11] pin */
+#define AFIO_EXTICR3_EXTI11_PE ((uint32_t)0x00004000) /*!< PE[11] pin */
+#define AFIO_EXTICR3_EXTI11_PF ((uint32_t)0x00005000) /*!< PF[11] pin */
+#define AFIO_EXTICR3_EXTI11_PG ((uint32_t)0x00006000) /*!< PG[11] pin */
+
+/***************** Bit definition for AFIO_EXTICR4 register *****************/
+#define AFIO_EXTICR4_EXTI12 ((uint32_t)0x0000000F) /*!< EXTI 12 configuration */
+#define AFIO_EXTICR4_EXTI13 ((uint32_t)0x000000F0) /*!< EXTI 13 configuration */
+#define AFIO_EXTICR4_EXTI14 ((uint32_t)0x00000F00) /*!< EXTI 14 configuration */
+#define AFIO_EXTICR4_EXTI15 ((uint32_t)0x0000F000) /*!< EXTI 15 configuration */
+
+/* EXTI12 configuration */
+#define AFIO_EXTICR4_EXTI12_PA ((uint32_t)0x00000000) /*!< PA[12] pin */
+#define AFIO_EXTICR4_EXTI12_PB ((uint32_t)0x00000001) /*!< PB[12] pin */
+#define AFIO_EXTICR4_EXTI12_PC ((uint32_t)0x00000002) /*!< PC[12] pin */
+#define AFIO_EXTICR4_EXTI12_PD ((uint32_t)0x00000003) /*!< PD[12] pin */
+#define AFIO_EXTICR4_EXTI12_PE ((uint32_t)0x00000004) /*!< PE[12] pin */
+#define AFIO_EXTICR4_EXTI12_PF ((uint32_t)0x00000005) /*!< PF[12] pin */
+#define AFIO_EXTICR4_EXTI12_PG ((uint32_t)0x00000006) /*!< PG[12] pin */
+
+/* EXTI13 configuration */
+#define AFIO_EXTICR4_EXTI13_PA ((uint32_t)0x00000000) /*!< PA[13] pin */
+#define AFIO_EXTICR4_EXTI13_PB ((uint32_t)0x00000010) /*!< PB[13] pin */
+#define AFIO_EXTICR4_EXTI13_PC ((uint32_t)0x00000020) /*!< PC[13] pin */
+#define AFIO_EXTICR4_EXTI13_PD ((uint32_t)0x00000030) /*!< PD[13] pin */
+#define AFIO_EXTICR4_EXTI13_PE ((uint32_t)0x00000040) /*!< PE[13] pin */
+#define AFIO_EXTICR4_EXTI13_PF ((uint32_t)0x00000050) /*!< PF[13] pin */
+#define AFIO_EXTICR4_EXTI13_PG ((uint32_t)0x00000060) /*!< PG[13] pin */
+
+/*!< EXTI14 configuration */
+#define AFIO_EXTICR4_EXTI14_PA ((uint32_t)0x00000000) /*!< PA[14] pin */
+#define AFIO_EXTICR4_EXTI14_PB ((uint32_t)0x00000100) /*!< PB[14] pin */
+#define AFIO_EXTICR4_EXTI14_PC ((uint32_t)0x00000200) /*!< PC[14] pin */
+#define AFIO_EXTICR4_EXTI14_PD ((uint32_t)0x00000300) /*!< PD[14] pin */
+#define AFIO_EXTICR4_EXTI14_PE ((uint32_t)0x00000400) /*!< PE[14] pin */
+#define AFIO_EXTICR4_EXTI14_PF ((uint32_t)0x00000500) /*!< PF[14] pin */
+#define AFIO_EXTICR4_EXTI14_PG ((uint32_t)0x00000600) /*!< PG[14] pin */
+
+/*!< EXTI15 configuration */
+#define AFIO_EXTICR4_EXTI15_PA ((uint32_t)0x00000000) /*!< PA[15] pin */
+#define AFIO_EXTICR4_EXTI15_PB ((uint32_t)0x00001000) /*!< PB[15] pin */
+#define AFIO_EXTICR4_EXTI15_PC ((uint32_t)0x00002000) /*!< PC[15] pin */
+#define AFIO_EXTICR4_EXTI15_PD ((uint32_t)0x00003000) /*!< PD[15] pin */
+#define AFIO_EXTICR4_EXTI15_PE ((uint32_t)0x00004000) /*!< PE[15] pin */
+#define AFIO_EXTICR4_EXTI15_PF ((uint32_t)0x00005000) /*!< PF[15] pin */
+#define AFIO_EXTICR4_EXTI15_PG ((uint32_t)0x00006000) /*!< PG[15] pin */
+
+/****************** Bit definition for AFIO_MAPR2 register ******************/
+
+
+#define AFIO_MAPR2_FSMC_NADV_REMAP ((uint32_t)0x00000400) /*!< FSMC NADV remapping */
+
+/******************************************************************************/
+/* */
+/* SystemTick */
+/* */
+/******************************************************************************/
+
+/***************** Bit definition for SysTick_CTRL register *****************/
+#define SysTick_CTRL_ENABLE ((uint32_t)0x00000001) /*!< Counter enable */
+#define SysTick_CTRL_TICKINT ((uint32_t)0x00000002) /*!< Counting down to 0 pends the SysTick handler */
+#define SysTick_CTRL_CLKSOURCE ((uint32_t)0x00000004) /*!< Clock source */
+#define SysTick_CTRL_COUNTFLAG ((uint32_t)0x00010000) /*!< Count Flag */
+
+/***************** Bit definition for SysTick_LOAD register *****************/
+#define SysTick_LOAD_RELOAD ((uint32_t)0x00FFFFFF) /*!< Value to load into the SysTick Current Value Register when the counter reaches 0 */
+
+/***************** Bit definition for SysTick_VAL register ******************/
+#define SysTick_VAL_CURRENT ((uint32_t)0x00FFFFFF) /*!< Current value at the time the register is accessed */
+
+/***************** Bit definition for SysTick_CALIB register ****************/
+#define SysTick_CALIB_TENMS ((uint32_t)0x00FFFFFF) /*!< Reload value to use for 10ms timing */
+#define SysTick_CALIB_SKEW ((uint32_t)0x40000000) /*!< Calibration value is not exactly 10 ms */
+#define SysTick_CALIB_NOREF ((uint32_t)0x80000000) /*!< The reference clock is not provided */
+
+/******************************************************************************/
+/* */
+/* Nested Vectored Interrupt Controller */
+/* */
+/******************************************************************************/
+
+/****************** Bit definition for NVIC_ISER register *******************/
+#define NVIC_ISER_SETENA ((uint32_t)0xFFFFFFFF) /*!< Interrupt set enable bits */
+#define NVIC_ISER_SETENA_0 ((uint32_t)0x00000001) /*!< bit 0 */
+#define NVIC_ISER_SETENA_1 ((uint32_t)0x00000002) /*!< bit 1 */
+#define NVIC_ISER_SETENA_2 ((uint32_t)0x00000004) /*!< bit 2 */
+#define NVIC_ISER_SETENA_3 ((uint32_t)0x00000008) /*!< bit 3 */
+#define NVIC_ISER_SETENA_4 ((uint32_t)0x00000010) /*!< bit 4 */
+#define NVIC_ISER_SETENA_5 ((uint32_t)0x00000020) /*!< bit 5 */
+#define NVIC_ISER_SETENA_6 ((uint32_t)0x00000040) /*!< bit 6 */
+#define NVIC_ISER_SETENA_7 ((uint32_t)0x00000080) /*!< bit 7 */
+#define NVIC_ISER_SETENA_8 ((uint32_t)0x00000100) /*!< bit 8 */
+#define NVIC_ISER_SETENA_9 ((uint32_t)0x00000200) /*!< bit 9 */
+#define NVIC_ISER_SETENA_10 ((uint32_t)0x00000400) /*!< bit 10 */
+#define NVIC_ISER_SETENA_11 ((uint32_t)0x00000800) /*!< bit 11 */
+#define NVIC_ISER_SETENA_12 ((uint32_t)0x00001000) /*!< bit 12 */
+#define NVIC_ISER_SETENA_13 ((uint32_t)0x00002000) /*!< bit 13 */
+#define NVIC_ISER_SETENA_14 ((uint32_t)0x00004000) /*!< bit 14 */
+#define NVIC_ISER_SETENA_15 ((uint32_t)0x00008000) /*!< bit 15 */
+#define NVIC_ISER_SETENA_16 ((uint32_t)0x00010000) /*!< bit 16 */
+#define NVIC_ISER_SETENA_17 ((uint32_t)0x00020000) /*!< bit 17 */
+#define NVIC_ISER_SETENA_18 ((uint32_t)0x00040000) /*!< bit 18 */
+#define NVIC_ISER_SETENA_19 ((uint32_t)0x00080000) /*!< bit 19 */
+#define NVIC_ISER_SETENA_20 ((uint32_t)0x00100000) /*!< bit 20 */
+#define NVIC_ISER_SETENA_21 ((uint32_t)0x00200000) /*!< bit 21 */
+#define NVIC_ISER_SETENA_22 ((uint32_t)0x00400000) /*!< bit 22 */
+#define NVIC_ISER_SETENA_23 ((uint32_t)0x00800000) /*!< bit 23 */
+#define NVIC_ISER_SETENA_24 ((uint32_t)0x01000000) /*!< bit 24 */
+#define NVIC_ISER_SETENA_25 ((uint32_t)0x02000000) /*!< bit 25 */
+#define NVIC_ISER_SETENA_26 ((uint32_t)0x04000000) /*!< bit 26 */
+#define NVIC_ISER_SETENA_27 ((uint32_t)0x08000000) /*!< bit 27 */
+#define NVIC_ISER_SETENA_28 ((uint32_t)0x10000000) /*!< bit 28 */
+#define NVIC_ISER_SETENA_29 ((uint32_t)0x20000000) /*!< bit 29 */
+#define NVIC_ISER_SETENA_30 ((uint32_t)0x40000000) /*!< bit 30 */
+#define NVIC_ISER_SETENA_31 ((uint32_t)0x80000000) /*!< bit 31 */
+
+/****************** Bit definition for NVIC_ICER register *******************/
+#define NVIC_ICER_CLRENA ((uint32_t)0xFFFFFFFF) /*!< Interrupt clear-enable bits */
+#define NVIC_ICER_CLRENA_0 ((uint32_t)0x00000001) /*!< bit 0 */
+#define NVIC_ICER_CLRENA_1 ((uint32_t)0x00000002) /*!< bit 1 */
+#define NVIC_ICER_CLRENA_2 ((uint32_t)0x00000004) /*!< bit 2 */
+#define NVIC_ICER_CLRENA_3 ((uint32_t)0x00000008) /*!< bit 3 */
+#define NVIC_ICER_CLRENA_4 ((uint32_t)0x00000010) /*!< bit 4 */
+#define NVIC_ICER_CLRENA_5 ((uint32_t)0x00000020) /*!< bit 5 */
+#define NVIC_ICER_CLRENA_6 ((uint32_t)0x00000040) /*!< bit 6 */
+#define NVIC_ICER_CLRENA_7 ((uint32_t)0x00000080) /*!< bit 7 */
+#define NVIC_ICER_CLRENA_8 ((uint32_t)0x00000100) /*!< bit 8 */
+#define NVIC_ICER_CLRENA_9 ((uint32_t)0x00000200) /*!< bit 9 */
+#define NVIC_ICER_CLRENA_10 ((uint32_t)0x00000400) /*!< bit 10 */
+#define NVIC_ICER_CLRENA_11 ((uint32_t)0x00000800) /*!< bit 11 */
+#define NVIC_ICER_CLRENA_12 ((uint32_t)0x00001000) /*!< bit 12 */
+#define NVIC_ICER_CLRENA_13 ((uint32_t)0x00002000) /*!< bit 13 */
+#define NVIC_ICER_CLRENA_14 ((uint32_t)0x00004000) /*!< bit 14 */
+#define NVIC_ICER_CLRENA_15 ((uint32_t)0x00008000) /*!< bit 15 */
+#define NVIC_ICER_CLRENA_16 ((uint32_t)0x00010000) /*!< bit 16 */
+#define NVIC_ICER_CLRENA_17 ((uint32_t)0x00020000) /*!< bit 17 */
+#define NVIC_ICER_CLRENA_18 ((uint32_t)0x00040000) /*!< bit 18 */
+#define NVIC_ICER_CLRENA_19 ((uint32_t)0x00080000) /*!< bit 19 */
+#define NVIC_ICER_CLRENA_20 ((uint32_t)0x00100000) /*!< bit 20 */
+#define NVIC_ICER_CLRENA_21 ((uint32_t)0x00200000) /*!< bit 21 */
+#define NVIC_ICER_CLRENA_22 ((uint32_t)0x00400000) /*!< bit 22 */
+#define NVIC_ICER_CLRENA_23 ((uint32_t)0x00800000) /*!< bit 23 */
+#define NVIC_ICER_CLRENA_24 ((uint32_t)0x01000000) /*!< bit 24 */
+#define NVIC_ICER_CLRENA_25 ((uint32_t)0x02000000) /*!< bit 25 */
+#define NVIC_ICER_CLRENA_26 ((uint32_t)0x04000000) /*!< bit 26 */
+#define NVIC_ICER_CLRENA_27 ((uint32_t)0x08000000) /*!< bit 27 */
+#define NVIC_ICER_CLRENA_28 ((uint32_t)0x10000000) /*!< bit 28 */
+#define NVIC_ICER_CLRENA_29 ((uint32_t)0x20000000) /*!< bit 29 */
+#define NVIC_ICER_CLRENA_30 ((uint32_t)0x40000000) /*!< bit 30 */
+#define NVIC_ICER_CLRENA_31 ((uint32_t)0x80000000) /*!< bit 31 */
+
+/****************** Bit definition for NVIC_ISPR register *******************/
+#define NVIC_ISPR_SETPEND ((uint32_t)0xFFFFFFFF) /*!< Interrupt set-pending bits */
+#define NVIC_ISPR_SETPEND_0 ((uint32_t)0x00000001) /*!< bit 0 */
+#define NVIC_ISPR_SETPEND_1 ((uint32_t)0x00000002) /*!< bit 1 */
+#define NVIC_ISPR_SETPEND_2 ((uint32_t)0x00000004) /*!< bit 2 */
+#define NVIC_ISPR_SETPEND_3 ((uint32_t)0x00000008) /*!< bit 3 */
+#define NVIC_ISPR_SETPEND_4 ((uint32_t)0x00000010) /*!< bit 4 */
+#define NVIC_ISPR_SETPEND_5 ((uint32_t)0x00000020) /*!< bit 5 */
+#define NVIC_ISPR_SETPEND_6 ((uint32_t)0x00000040) /*!< bit 6 */
+#define NVIC_ISPR_SETPEND_7 ((uint32_t)0x00000080) /*!< bit 7 */
+#define NVIC_ISPR_SETPEND_8 ((uint32_t)0x00000100) /*!< bit 8 */
+#define NVIC_ISPR_SETPEND_9 ((uint32_t)0x00000200) /*!< bit 9 */
+#define NVIC_ISPR_SETPEND_10 ((uint32_t)0x00000400) /*!< bit 10 */
+#define NVIC_ISPR_SETPEND_11 ((uint32_t)0x00000800) /*!< bit 11 */
+#define NVIC_ISPR_SETPEND_12 ((uint32_t)0x00001000) /*!< bit 12 */
+#define NVIC_ISPR_SETPEND_13 ((uint32_t)0x00002000) /*!< bit 13 */
+#define NVIC_ISPR_SETPEND_14 ((uint32_t)0x00004000) /*!< bit 14 */
+#define NVIC_ISPR_SETPEND_15 ((uint32_t)0x00008000) /*!< bit 15 */
+#define NVIC_ISPR_SETPEND_16 ((uint32_t)0x00010000) /*!< bit 16 */
+#define NVIC_ISPR_SETPEND_17 ((uint32_t)0x00020000) /*!< bit 17 */
+#define NVIC_ISPR_SETPEND_18 ((uint32_t)0x00040000) /*!< bit 18 */
+#define NVIC_ISPR_SETPEND_19 ((uint32_t)0x00080000) /*!< bit 19 */
+#define NVIC_ISPR_SETPEND_20 ((uint32_t)0x00100000) /*!< bit 20 */
+#define NVIC_ISPR_SETPEND_21 ((uint32_t)0x00200000) /*!< bit 21 */
+#define NVIC_ISPR_SETPEND_22 ((uint32_t)0x00400000) /*!< bit 22 */
+#define NVIC_ISPR_SETPEND_23 ((uint32_t)0x00800000) /*!< bit 23 */
+#define NVIC_ISPR_SETPEND_24 ((uint32_t)0x01000000) /*!< bit 24 */
+#define NVIC_ISPR_SETPEND_25 ((uint32_t)0x02000000) /*!< bit 25 */
+#define NVIC_ISPR_SETPEND_26 ((uint32_t)0x04000000) /*!< bit 26 */
+#define NVIC_ISPR_SETPEND_27 ((uint32_t)0x08000000) /*!< bit 27 */
+#define NVIC_ISPR_SETPEND_28 ((uint32_t)0x10000000) /*!< bit 28 */
+#define NVIC_ISPR_SETPEND_29 ((uint32_t)0x20000000) /*!< bit 29 */
+#define NVIC_ISPR_SETPEND_30 ((uint32_t)0x40000000) /*!< bit 30 */
+#define NVIC_ISPR_SETPEND_31 ((uint32_t)0x80000000) /*!< bit 31 */
+
+/****************** Bit definition for NVIC_ICPR register *******************/
+#define NVIC_ICPR_CLRPEND ((uint32_t)0xFFFFFFFF) /*!< Interrupt clear-pending bits */
+#define NVIC_ICPR_CLRPEND_0 ((uint32_t)0x00000001) /*!< bit 0 */
+#define NVIC_ICPR_CLRPEND_1 ((uint32_t)0x00000002) /*!< bit 1 */
+#define NVIC_ICPR_CLRPEND_2 ((uint32_t)0x00000004) /*!< bit 2 */
+#define NVIC_ICPR_CLRPEND_3 ((uint32_t)0x00000008) /*!< bit 3 */
+#define NVIC_ICPR_CLRPEND_4 ((uint32_t)0x00000010) /*!< bit 4 */
+#define NVIC_ICPR_CLRPEND_5 ((uint32_t)0x00000020) /*!< bit 5 */
+#define NVIC_ICPR_CLRPEND_6 ((uint32_t)0x00000040) /*!< bit 6 */
+#define NVIC_ICPR_CLRPEND_7 ((uint32_t)0x00000080) /*!< bit 7 */
+#define NVIC_ICPR_CLRPEND_8 ((uint32_t)0x00000100) /*!< bit 8 */
+#define NVIC_ICPR_CLRPEND_9 ((uint32_t)0x00000200) /*!< bit 9 */
+#define NVIC_ICPR_CLRPEND_10 ((uint32_t)0x00000400) /*!< bit 10 */
+#define NVIC_ICPR_CLRPEND_11 ((uint32_t)0x00000800) /*!< bit 11 */
+#define NVIC_ICPR_CLRPEND_12 ((uint32_t)0x00001000) /*!< bit 12 */
+#define NVIC_ICPR_CLRPEND_13 ((uint32_t)0x00002000) /*!< bit 13 */
+#define NVIC_ICPR_CLRPEND_14 ((uint32_t)0x00004000) /*!< bit 14 */
+#define NVIC_ICPR_CLRPEND_15 ((uint32_t)0x00008000) /*!< bit 15 */
+#define NVIC_ICPR_CLRPEND_16 ((uint32_t)0x00010000) /*!< bit 16 */
+#define NVIC_ICPR_CLRPEND_17 ((uint32_t)0x00020000) /*!< bit 17 */
+#define NVIC_ICPR_CLRPEND_18 ((uint32_t)0x00040000) /*!< bit 18 */
+#define NVIC_ICPR_CLRPEND_19 ((uint32_t)0x00080000) /*!< bit 19 */
+#define NVIC_ICPR_CLRPEND_20 ((uint32_t)0x00100000) /*!< bit 20 */
+#define NVIC_ICPR_CLRPEND_21 ((uint32_t)0x00200000) /*!< bit 21 */
+#define NVIC_ICPR_CLRPEND_22 ((uint32_t)0x00400000) /*!< bit 22 */
+#define NVIC_ICPR_CLRPEND_23 ((uint32_t)0x00800000) /*!< bit 23 */
+#define NVIC_ICPR_CLRPEND_24 ((uint32_t)0x01000000) /*!< bit 24 */
+#define NVIC_ICPR_CLRPEND_25 ((uint32_t)0x02000000) /*!< bit 25 */
+#define NVIC_ICPR_CLRPEND_26 ((uint32_t)0x04000000) /*!< bit 26 */
+#define NVIC_ICPR_CLRPEND_27 ((uint32_t)0x08000000) /*!< bit 27 */
+#define NVIC_ICPR_CLRPEND_28 ((uint32_t)0x10000000) /*!< bit 28 */
+#define NVIC_ICPR_CLRPEND_29 ((uint32_t)0x20000000) /*!< bit 29 */
+#define NVIC_ICPR_CLRPEND_30 ((uint32_t)0x40000000) /*!< bit 30 */
+#define NVIC_ICPR_CLRPEND_31 ((uint32_t)0x80000000) /*!< bit 31 */
+
+/****************** Bit definition for NVIC_IABR register *******************/
+#define NVIC_IABR_ACTIVE ((uint32_t)0xFFFFFFFF) /*!< Interrupt active flags */
+#define NVIC_IABR_ACTIVE_0 ((uint32_t)0x00000001) /*!< bit 0 */
+#define NVIC_IABR_ACTIVE_1 ((uint32_t)0x00000002) /*!< bit 1 */
+#define NVIC_IABR_ACTIVE_2 ((uint32_t)0x00000004) /*!< bit 2 */
+#define NVIC_IABR_ACTIVE_3 ((uint32_t)0x00000008) /*!< bit 3 */
+#define NVIC_IABR_ACTIVE_4 ((uint32_t)0x00000010) /*!< bit 4 */
+#define NVIC_IABR_ACTIVE_5 ((uint32_t)0x00000020) /*!< bit 5 */
+#define NVIC_IABR_ACTIVE_6 ((uint32_t)0x00000040) /*!< bit 6 */
+#define NVIC_IABR_ACTIVE_7 ((uint32_t)0x00000080) /*!< bit 7 */
+#define NVIC_IABR_ACTIVE_8 ((uint32_t)0x00000100) /*!< bit 8 */
+#define NVIC_IABR_ACTIVE_9 ((uint32_t)0x00000200) /*!< bit 9 */
+#define NVIC_IABR_ACTIVE_10 ((uint32_t)0x00000400) /*!< bit 10 */
+#define NVIC_IABR_ACTIVE_11 ((uint32_t)0x00000800) /*!< bit 11 */
+#define NVIC_IABR_ACTIVE_12 ((uint32_t)0x00001000) /*!< bit 12 */
+#define NVIC_IABR_ACTIVE_13 ((uint32_t)0x00002000) /*!< bit 13 */
+#define NVIC_IABR_ACTIVE_14 ((uint32_t)0x00004000) /*!< bit 14 */
+#define NVIC_IABR_ACTIVE_15 ((uint32_t)0x00008000) /*!< bit 15 */
+#define NVIC_IABR_ACTIVE_16 ((uint32_t)0x00010000) /*!< bit 16 */
+#define NVIC_IABR_ACTIVE_17 ((uint32_t)0x00020000) /*!< bit 17 */
+#define NVIC_IABR_ACTIVE_18 ((uint32_t)0x00040000) /*!< bit 18 */
+#define NVIC_IABR_ACTIVE_19 ((uint32_t)0x00080000) /*!< bit 19 */
+#define NVIC_IABR_ACTIVE_20 ((uint32_t)0x00100000) /*!< bit 20 */
+#define NVIC_IABR_ACTIVE_21 ((uint32_t)0x00200000) /*!< bit 21 */
+#define NVIC_IABR_ACTIVE_22 ((uint32_t)0x00400000) /*!< bit 22 */
+#define NVIC_IABR_ACTIVE_23 ((uint32_t)0x00800000) /*!< bit 23 */
+#define NVIC_IABR_ACTIVE_24 ((uint32_t)0x01000000) /*!< bit 24 */
+#define NVIC_IABR_ACTIVE_25 ((uint32_t)0x02000000) /*!< bit 25 */
+#define NVIC_IABR_ACTIVE_26 ((uint32_t)0x04000000) /*!< bit 26 */
+#define NVIC_IABR_ACTIVE_27 ((uint32_t)0x08000000) /*!< bit 27 */
+#define NVIC_IABR_ACTIVE_28 ((uint32_t)0x10000000) /*!< bit 28 */
+#define NVIC_IABR_ACTIVE_29 ((uint32_t)0x20000000) /*!< bit 29 */
+#define NVIC_IABR_ACTIVE_30 ((uint32_t)0x40000000) /*!< bit 30 */
+#define NVIC_IABR_ACTIVE_31 ((uint32_t)0x80000000) /*!< bit 31 */
+
+/****************** Bit definition for NVIC_PRI0 register *******************/
+#define NVIC_IPR0_PRI_0 ((uint32_t)0x000000FF) /*!< Priority of interrupt 0 */
+#define NVIC_IPR0_PRI_1 ((uint32_t)0x0000FF00) /*!< Priority of interrupt 1 */
+#define NVIC_IPR0_PRI_2 ((uint32_t)0x00FF0000) /*!< Priority of interrupt 2 */
+#define NVIC_IPR0_PRI_3 ((uint32_t)0xFF000000) /*!< Priority of interrupt 3 */
+
+/****************** Bit definition for NVIC_PRI1 register *******************/
+#define NVIC_IPR1_PRI_4 ((uint32_t)0x000000FF) /*!< Priority of interrupt 4 */
+#define NVIC_IPR1_PRI_5 ((uint32_t)0x0000FF00) /*!< Priority of interrupt 5 */
+#define NVIC_IPR1_PRI_6 ((uint32_t)0x00FF0000) /*!< Priority of interrupt 6 */
+#define NVIC_IPR1_PRI_7 ((uint32_t)0xFF000000) /*!< Priority of interrupt 7 */
+
+/****************** Bit definition for NVIC_PRI2 register *******************/
+#define NVIC_IPR2_PRI_8 ((uint32_t)0x000000FF) /*!< Priority of interrupt 8 */
+#define NVIC_IPR2_PRI_9 ((uint32_t)0x0000FF00) /*!< Priority of interrupt 9 */
+#define NVIC_IPR2_PRI_10 ((uint32_t)0x00FF0000) /*!< Priority of interrupt 10 */
+#define NVIC_IPR2_PRI_11 ((uint32_t)0xFF000000) /*!< Priority of interrupt 11 */
+
+/****************** Bit definition for NVIC_PRI3 register *******************/
+#define NVIC_IPR3_PRI_12 ((uint32_t)0x000000FF) /*!< Priority of interrupt 12 */
+#define NVIC_IPR3_PRI_13 ((uint32_t)0x0000FF00) /*!< Priority of interrupt 13 */
+#define NVIC_IPR3_PRI_14 ((uint32_t)0x00FF0000) /*!< Priority of interrupt 14 */
+#define NVIC_IPR3_PRI_15 ((uint32_t)0xFF000000) /*!< Priority of interrupt 15 */
+
+/****************** Bit definition for NVIC_PRI4 register *******************/
+#define NVIC_IPR4_PRI_16 ((uint32_t)0x000000FF) /*!< Priority of interrupt 16 */
+#define NVIC_IPR4_PRI_17 ((uint32_t)0x0000FF00) /*!< Priority of interrupt 17 */
+#define NVIC_IPR4_PRI_18 ((uint32_t)0x00FF0000) /*!< Priority of interrupt 18 */
+#define NVIC_IPR4_PRI_19 ((uint32_t)0xFF000000) /*!< Priority of interrupt 19 */
+
+/****************** Bit definition for NVIC_PRI5 register *******************/
+#define NVIC_IPR5_PRI_20 ((uint32_t)0x000000FF) /*!< Priority of interrupt 20 */
+#define NVIC_IPR5_PRI_21 ((uint32_t)0x0000FF00) /*!< Priority of interrupt 21 */
+#define NVIC_IPR5_PRI_22 ((uint32_t)0x00FF0000) /*!< Priority of interrupt 22 */
+#define NVIC_IPR5_PRI_23 ((uint32_t)0xFF000000) /*!< Priority of interrupt 23 */
+
+/****************** Bit definition for NVIC_PRI6 register *******************/
+#define NVIC_IPR6_PRI_24 ((uint32_t)0x000000FF) /*!< Priority of interrupt 24 */
+#define NVIC_IPR6_PRI_25 ((uint32_t)0x0000FF00) /*!< Priority of interrupt 25 */
+#define NVIC_IPR6_PRI_26 ((uint32_t)0x00FF0000) /*!< Priority of interrupt 26 */
+#define NVIC_IPR6_PRI_27 ((uint32_t)0xFF000000) /*!< Priority of interrupt 27 */
+
+/****************** Bit definition for NVIC_PRI7 register *******************/
+#define NVIC_IPR7_PRI_28 ((uint32_t)0x000000FF) /*!< Priority of interrupt 28 */
+#define NVIC_IPR7_PRI_29 ((uint32_t)0x0000FF00) /*!< Priority of interrupt 29 */
+#define NVIC_IPR7_PRI_30 ((uint32_t)0x00FF0000) /*!< Priority of interrupt 30 */
+#define NVIC_IPR7_PRI_31 ((uint32_t)0xFF000000) /*!< Priority of interrupt 31 */
+
+/****************** Bit definition for SCB_CPUID register *******************/
+#define SCB_CPUID_REVISION ((uint32_t)0x0000000F) /*!< Implementation defined revision number */
+#define SCB_CPUID_PARTNO ((uint32_t)0x0000FFF0) /*!< Number of processor within family */
+#define SCB_CPUID_Constant ((uint32_t)0x000F0000) /*!< Reads as 0x0F */
+#define SCB_CPUID_VARIANT ((uint32_t)0x00F00000) /*!< Implementation defined variant number */
+#define SCB_CPUID_IMPLEMENTER ((uint32_t)0xFF000000) /*!< Implementer code. ARM is 0x41 */
+
+/******************* Bit definition for SCB_ICSR register *******************/
+#define SCB_ICSR_VECTACTIVE ((uint32_t)0x000001FF) /*!< Active ISR number field */
+#define SCB_ICSR_RETTOBASE ((uint32_t)0x00000800) /*!< All active exceptions minus the IPSR_current_exception yields the empty set */
+#define SCB_ICSR_VECTPENDING ((uint32_t)0x003FF000) /*!< Pending ISR number field */
+#define SCB_ICSR_ISRPENDING ((uint32_t)0x00400000) /*!< Interrupt pending flag */
+#define SCB_ICSR_ISRPREEMPT ((uint32_t)0x00800000) /*!< It indicates that a pending interrupt becomes active in the next running cycle */
+#define SCB_ICSR_PENDSTCLR ((uint32_t)0x02000000) /*!< Clear pending SysTick bit */
+#define SCB_ICSR_PENDSTSET ((uint32_t)0x04000000) /*!< Set pending SysTick bit */
+#define SCB_ICSR_PENDSVCLR ((uint32_t)0x08000000) /*!< Clear pending pendSV bit */
+#define SCB_ICSR_PENDSVSET ((uint32_t)0x10000000) /*!< Set pending pendSV bit */
+#define SCB_ICSR_NMIPENDSET ((uint32_t)0x80000000) /*!< Set pending NMI bit */
+
+/******************* Bit definition for SCB_VTOR register *******************/
+#define SCB_VTOR_TBLOFF ((uint32_t)0x1FFFFF80) /*!< Vector table base offset field */
+#define SCB_VTOR_TBLBASE ((uint32_t)0x20000000) /*!< Table base in code(0) or RAM(1) */
+
+/*!<***************** Bit definition for SCB_AIRCR register *******************/
+#define SCB_AIRCR_VECTRESET ((uint32_t)0x00000001) /*!< System Reset bit */
+#define SCB_AIRCR_VECTCLRACTIVE ((uint32_t)0x00000002) /*!< Clear active vector bit */
+#define SCB_AIRCR_SYSRESETREQ ((uint32_t)0x00000004) /*!< Requests chip control logic to generate a reset */
+
+#define SCB_AIRCR_PRIGROUP ((uint32_t)0x00000700) /*!< PRIGROUP[2:0] bits (Priority group) */
+#define SCB_AIRCR_PRIGROUP_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define SCB_AIRCR_PRIGROUP_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+#define SCB_AIRCR_PRIGROUP_2 ((uint32_t)0x00000400) /*!< Bit 2 */
+
+/* prority group configuration */
+#define SCB_AIRCR_PRIGROUP0 ((uint32_t)0x00000000) /*!< Priority group=0 (7 bits of pre-emption priority, 1 bit of subpriority) */
+#define SCB_AIRCR_PRIGROUP1 ((uint32_t)0x00000100) /*!< Priority group=1 (6 bits of pre-emption priority, 2 bits of subpriority) */
+#define SCB_AIRCR_PRIGROUP2 ((uint32_t)0x00000200) /*!< Priority group=2 (5 bits of pre-emption priority, 3 bits of subpriority) */
+#define SCB_AIRCR_PRIGROUP3 ((uint32_t)0x00000300) /*!< Priority group=3 (4 bits of pre-emption priority, 4 bits of subpriority) */
+#define SCB_AIRCR_PRIGROUP4 ((uint32_t)0x00000400) /*!< Priority group=4 (3 bits of pre-emption priority, 5 bits of subpriority) */
+#define SCB_AIRCR_PRIGROUP5 ((uint32_t)0x00000500) /*!< Priority group=5 (2 bits of pre-emption priority, 6 bits of subpriority) */
+#define SCB_AIRCR_PRIGROUP6 ((uint32_t)0x00000600) /*!< Priority group=6 (1 bit of pre-emption priority, 7 bits of subpriority) */
+#define SCB_AIRCR_PRIGROUP7 ((uint32_t)0x00000700) /*!< Priority group=7 (no pre-emption priority, 8 bits of subpriority) */
+
+#define SCB_AIRCR_ENDIANESS ((uint32_t)0x00008000) /*!< Data endianness bit */
+#define SCB_AIRCR_VECTKEY ((uint32_t)0xFFFF0000) /*!< Register key (VECTKEY) - Reads as 0xFA05 (VECTKEYSTAT) */
+
+/******************* Bit definition for SCB_SCR register ********************/
+#define SCB_SCR_SLEEPONEXIT ((uint32_t)0x00000002) /*!< Sleep on exit bit */
+#define SCB_SCR_SLEEPDEEP ((uint32_t)0x00000004) /*!< Sleep deep bit */
+#define SCB_SCR_SEVONPEND ((uint32_t)0x00000010) /*!< Wake up from WFE */
+
+/******************** Bit definition for SCB_CCR register *******************/
+#define SCB_CCR_NONBASETHRDENA ((uint32_t)0x00000001) /*!< Thread mode can be entered from any level in Handler mode by controlled return value */
+#define SCB_CCR_USERSETMPEND ((uint32_t)0x00000002) /*!< Enables user code to write the Software Trigger Interrupt register to trigger (pend) a Main exception */
+#define SCB_CCR_UNALIGN_TRP ((uint32_t)0x00000008) /*!< Trap for unaligned access */
+#define SCB_CCR_DIV_0_TRP ((uint32_t)0x00000010) /*!< Trap on Divide by 0 */
+#define SCB_CCR_BFHFNMIGN ((uint32_t)0x00000100) /*!< Handlers running at priority -1 and -2 */
+#define SCB_CCR_STKALIGN ((uint32_t)0x00000200) /*!< On exception entry, the SP used prior to the exception is adjusted to be 8-byte aligned */
+
+/******************* Bit definition for SCB_SHPR register ********************/
+#define SCB_SHPR_PRI_N ((uint32_t)0x000000FF) /*!< Priority of system handler 4,8, and 12. Mem Manage, reserved and Debug Monitor */
+#define SCB_SHPR_PRI_N1 ((uint32_t)0x0000FF00) /*!< Priority of system handler 5,9, and 13. Bus Fault, reserved and reserved */
+#define SCB_SHPR_PRI_N2 ((uint32_t)0x00FF0000) /*!< Priority of system handler 6,10, and 14. Usage Fault, reserved and PendSV */
+#define SCB_SHPR_PRI_N3 ((uint32_t)0xFF000000) /*!< Priority of system handler 7,11, and 15. Reserved, SVCall and SysTick */
+
+/****************** Bit definition for SCB_SHCSR register *******************/
+#define SCB_SHCSR_MEMFAULTACT ((uint32_t)0x00000001) /*!< MemManage is active */
+#define SCB_SHCSR_BUSFAULTACT ((uint32_t)0x00000002) /*!< BusFault is active */
+#define SCB_SHCSR_USGFAULTACT ((uint32_t)0x00000008) /*!< UsageFault is active */
+#define SCB_SHCSR_SVCALLACT ((uint32_t)0x00000080) /*!< SVCall is active */
+#define SCB_SHCSR_MONITORACT ((uint32_t)0x00000100) /*!< Monitor is active */
+#define SCB_SHCSR_PENDSVACT ((uint32_t)0x00000400) /*!< PendSV is active */
+#define SCB_SHCSR_SYSTICKACT ((uint32_t)0x00000800) /*!< SysTick is active */
+#define SCB_SHCSR_USGFAULTPENDED ((uint32_t)0x00001000) /*!< Usage Fault is pended */
+#define SCB_SHCSR_MEMFAULTPENDED ((uint32_t)0x00002000) /*!< MemManage is pended */
+#define SCB_SHCSR_BUSFAULTPENDED ((uint32_t)0x00004000) /*!< Bus Fault is pended */
+#define SCB_SHCSR_SVCALLPENDED ((uint32_t)0x00008000) /*!< SVCall is pended */
+#define SCB_SHCSR_MEMFAULTENA ((uint32_t)0x00010000) /*!< MemManage enable */
+#define SCB_SHCSR_BUSFAULTENA ((uint32_t)0x00020000) /*!< Bus Fault enable */
+#define SCB_SHCSR_USGFAULTENA ((uint32_t)0x00040000) /*!< UsageFault enable */
+
+/******************* Bit definition for SCB_CFSR register *******************/
+/*!< MFSR */
+#define SCB_CFSR_IACCVIOL ((uint32_t)0x00000001) /*!< Instruction access violation */
+#define SCB_CFSR_DACCVIOL ((uint32_t)0x00000002) /*!< Data access violation */
+#define SCB_CFSR_MUNSTKERR ((uint32_t)0x00000008) /*!< Unstacking error */
+#define SCB_CFSR_MSTKERR ((uint32_t)0x00000010) /*!< Stacking error */
+#define SCB_CFSR_MMARVALID ((uint32_t)0x00000080) /*!< Memory Manage Address Register address valid flag */
+/*!< BFSR */
+#define SCB_CFSR_IBUSERR ((uint32_t)0x00000100) /*!< Instruction bus error flag */
+#define SCB_CFSR_PRECISERR ((uint32_t)0x00000200) /*!< Precise data bus error */
+#define SCB_CFSR_IMPRECISERR ((uint32_t)0x00000400) /*!< Imprecise data bus error */
+#define SCB_CFSR_UNSTKERR ((uint32_t)0x00000800) /*!< Unstacking error */
+#define SCB_CFSR_STKERR ((uint32_t)0x00001000) /*!< Stacking error */
+#define SCB_CFSR_BFARVALID ((uint32_t)0x00008000) /*!< Bus Fault Address Register address valid flag */
+/*!< UFSR */
+#define SCB_CFSR_UNDEFINSTR ((uint32_t)0x00010000) /*!< The processor attempt to execute an undefined instruction */
+#define SCB_CFSR_INVSTATE ((uint32_t)0x00020000) /*!< Invalid combination of EPSR and instruction */
+#define SCB_CFSR_INVPC ((uint32_t)0x00040000) /*!< Attempt to load EXC_RETURN into pc illegally */
+#define SCB_CFSR_NOCP ((uint32_t)0x00080000) /*!< Attempt to use a coprocessor instruction */
+#define SCB_CFSR_UNALIGNED ((uint32_t)0x01000000) /*!< Fault occurs when there is an attempt to make an unaligned memory access */
+#define SCB_CFSR_DIVBYZERO ((uint32_t)0x02000000) /*!< Fault occurs when SDIV or DIV instruction is used with a divisor of 0 */
+
+/******************* Bit definition for SCB_HFSR register *******************/
+#define SCB_HFSR_VECTTBL ((uint32_t)0x00000002) /*!< Fault occurs because of vector table read on exception processing */
+#define SCB_HFSR_FORCED ((uint32_t)0x40000000) /*!< Hard Fault activated when a configurable Fault was received and cannot activate */
+#define SCB_HFSR_DEBUGEVT ((uint32_t)0x80000000) /*!< Fault related to debug */
+
+/******************* Bit definition for SCB_DFSR register *******************/
+#define SCB_DFSR_HALTED ((uint32_t)0x00000001) /*!< Halt request flag */
+#define SCB_DFSR_BKPT ((uint32_t)0x00000002) /*!< BKPT flag */
+#define SCB_DFSR_DWTTRAP ((uint32_t)0x00000004) /*!< Data Watchpoint and Trace (DWT) flag */
+#define SCB_DFSR_VCATCH ((uint32_t)0x00000008) /*!< Vector catch flag */
+#define SCB_DFSR_EXTERNAL ((uint32_t)0x00000010) /*!< External debug request flag */
+
+/******************* Bit definition for SCB_MMFAR register ******************/
+#define SCB_MMFAR_ADDRESS ((uint32_t)0xFFFFFFFF) /*!< Mem Manage fault address field */
+
+/******************* Bit definition for SCB_BFAR register *******************/
+#define SCB_BFAR_ADDRESS ((uint32_t)0xFFFFFFFF) /*!< Bus fault address field */
+
+/******************* Bit definition for SCB_afsr register *******************/
+#define SCB_AFSR_IMPDEF ((uint32_t)0xFFFFFFFF) /*!< Implementation defined */
+
+/******************************************************************************/
+/* */
+/* External Interrupt/Event Controller */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for EXTI_IMR register *******************/
+#define EXTI_IMR_MR0 ((uint32_t)0x00000001) /*!< Interrupt Mask on line 0 */
+#define EXTI_IMR_MR1 ((uint32_t)0x00000002) /*!< Interrupt Mask on line 1 */
+#define EXTI_IMR_MR2 ((uint32_t)0x00000004) /*!< Interrupt Mask on line 2 */
+#define EXTI_IMR_MR3 ((uint32_t)0x00000008) /*!< Interrupt Mask on line 3 */
+#define EXTI_IMR_MR4 ((uint32_t)0x00000010) /*!< Interrupt Mask on line 4 */
+#define EXTI_IMR_MR5 ((uint32_t)0x00000020) /*!< Interrupt Mask on line 5 */
+#define EXTI_IMR_MR6 ((uint32_t)0x00000040) /*!< Interrupt Mask on line 6 */
+#define EXTI_IMR_MR7 ((uint32_t)0x00000080) /*!< Interrupt Mask on line 7 */
+#define EXTI_IMR_MR8 ((uint32_t)0x00000100) /*!< Interrupt Mask on line 8 */
+#define EXTI_IMR_MR9 ((uint32_t)0x00000200) /*!< Interrupt Mask on line 9 */
+#define EXTI_IMR_MR10 ((uint32_t)0x00000400) /*!< Interrupt Mask on line 10 */
+#define EXTI_IMR_MR11 ((uint32_t)0x00000800) /*!< Interrupt Mask on line 11 */
+#define EXTI_IMR_MR12 ((uint32_t)0x00001000) /*!< Interrupt Mask on line 12 */
+#define EXTI_IMR_MR13 ((uint32_t)0x00002000) /*!< Interrupt Mask on line 13 */
+#define EXTI_IMR_MR14 ((uint32_t)0x00004000) /*!< Interrupt Mask on line 14 */
+#define EXTI_IMR_MR15 ((uint32_t)0x00008000) /*!< Interrupt Mask on line 15 */
+#define EXTI_IMR_MR16 ((uint32_t)0x00010000) /*!< Interrupt Mask on line 16 */
+#define EXTI_IMR_MR17 ((uint32_t)0x00020000) /*!< Interrupt Mask on line 17 */
+#define EXTI_IMR_MR18 ((uint32_t)0x00040000) /*!< Interrupt Mask on line 18 */
+#define EXTI_IMR_MR19 ((uint32_t)0x00080000) /*!< Interrupt Mask on line 19 */
+
+/******************* Bit definition for EXTI_EMR register *******************/
+#define EXTI_EMR_MR0 ((uint32_t)0x00000001) /*!< Event Mask on line 0 */
+#define EXTI_EMR_MR1 ((uint32_t)0x00000002) /*!< Event Mask on line 1 */
+#define EXTI_EMR_MR2 ((uint32_t)0x00000004) /*!< Event Mask on line 2 */
+#define EXTI_EMR_MR3 ((uint32_t)0x00000008) /*!< Event Mask on line 3 */
+#define EXTI_EMR_MR4 ((uint32_t)0x00000010) /*!< Event Mask on line 4 */
+#define EXTI_EMR_MR5 ((uint32_t)0x00000020) /*!< Event Mask on line 5 */
+#define EXTI_EMR_MR6 ((uint32_t)0x00000040) /*!< Event Mask on line 6 */
+#define EXTI_EMR_MR7 ((uint32_t)0x00000080) /*!< Event Mask on line 7 */
+#define EXTI_EMR_MR8 ((uint32_t)0x00000100) /*!< Event Mask on line 8 */
+#define EXTI_EMR_MR9 ((uint32_t)0x00000200) /*!< Event Mask on line 9 */
+#define EXTI_EMR_MR10 ((uint32_t)0x00000400) /*!< Event Mask on line 10 */
+#define EXTI_EMR_MR11 ((uint32_t)0x00000800) /*!< Event Mask on line 11 */
+#define EXTI_EMR_MR12 ((uint32_t)0x00001000) /*!< Event Mask on line 12 */
+#define EXTI_EMR_MR13 ((uint32_t)0x00002000) /*!< Event Mask on line 13 */
+#define EXTI_EMR_MR14 ((uint32_t)0x00004000) /*!< Event Mask on line 14 */
+#define EXTI_EMR_MR15 ((uint32_t)0x00008000) /*!< Event Mask on line 15 */
+#define EXTI_EMR_MR16 ((uint32_t)0x00010000) /*!< Event Mask on line 16 */
+#define EXTI_EMR_MR17 ((uint32_t)0x00020000) /*!< Event Mask on line 17 */
+#define EXTI_EMR_MR18 ((uint32_t)0x00040000) /*!< Event Mask on line 18 */
+#define EXTI_EMR_MR19 ((uint32_t)0x00080000) /*!< Event Mask on line 19 */
+
+/****************** Bit definition for EXTI_RTSR register *******************/
+#define EXTI_RTSR_TR0 ((uint32_t)0x00000001) /*!< Rising trigger event configuration bit of line 0 */
+#define EXTI_RTSR_TR1 ((uint32_t)0x00000002) /*!< Rising trigger event configuration bit of line 1 */
+#define EXTI_RTSR_TR2 ((uint32_t)0x00000004) /*!< Rising trigger event configuration bit of line 2 */
+#define EXTI_RTSR_TR3 ((uint32_t)0x00000008) /*!< Rising trigger event configuration bit of line 3 */
+#define EXTI_RTSR_TR4 ((uint32_t)0x00000010) /*!< Rising trigger event configuration bit of line 4 */
+#define EXTI_RTSR_TR5 ((uint32_t)0x00000020) /*!< Rising trigger event configuration bit of line 5 */
+#define EXTI_RTSR_TR6 ((uint32_t)0x00000040) /*!< Rising trigger event configuration bit of line 6 */
+#define EXTI_RTSR_TR7 ((uint32_t)0x00000080) /*!< Rising trigger event configuration bit of line 7 */
+#define EXTI_RTSR_TR8 ((uint32_t)0x00000100) /*!< Rising trigger event configuration bit of line 8 */
+#define EXTI_RTSR_TR9 ((uint32_t)0x00000200) /*!< Rising trigger event configuration bit of line 9 */
+#define EXTI_RTSR_TR10 ((uint32_t)0x00000400) /*!< Rising trigger event configuration bit of line 10 */
+#define EXTI_RTSR_TR11 ((uint32_t)0x00000800) /*!< Rising trigger event configuration bit of line 11 */
+#define EXTI_RTSR_TR12 ((uint32_t)0x00001000) /*!< Rising trigger event configuration bit of line 12 */
+#define EXTI_RTSR_TR13 ((uint32_t)0x00002000) /*!< Rising trigger event configuration bit of line 13 */
+#define EXTI_RTSR_TR14 ((uint32_t)0x00004000) /*!< Rising trigger event configuration bit of line 14 */
+#define EXTI_RTSR_TR15 ((uint32_t)0x00008000) /*!< Rising trigger event configuration bit of line 15 */
+#define EXTI_RTSR_TR16 ((uint32_t)0x00010000) /*!< Rising trigger event configuration bit of line 16 */
+#define EXTI_RTSR_TR17 ((uint32_t)0x00020000) /*!< Rising trigger event configuration bit of line 17 */
+#define EXTI_RTSR_TR18 ((uint32_t)0x00040000) /*!< Rising trigger event configuration bit of line 18 */
+#define EXTI_RTSR_TR19 ((uint32_t)0x00080000) /*!< Rising trigger event configuration bit of line 19 */
+
+/****************** Bit definition for EXTI_FTSR register *******************/
+#define EXTI_FTSR_TR0 ((uint32_t)0x00000001) /*!< Falling trigger event configuration bit of line 0 */
+#define EXTI_FTSR_TR1 ((uint32_t)0x00000002) /*!< Falling trigger event configuration bit of line 1 */
+#define EXTI_FTSR_TR2 ((uint32_t)0x00000004) /*!< Falling trigger event configuration bit of line 2 */
+#define EXTI_FTSR_TR3 ((uint32_t)0x00000008) /*!< Falling trigger event configuration bit of line 3 */
+#define EXTI_FTSR_TR4 ((uint32_t)0x00000010) /*!< Falling trigger event configuration bit of line 4 */
+#define EXTI_FTSR_TR5 ((uint32_t)0x00000020) /*!< Falling trigger event configuration bit of line 5 */
+#define EXTI_FTSR_TR6 ((uint32_t)0x00000040) /*!< Falling trigger event configuration bit of line 6 */
+#define EXTI_FTSR_TR7 ((uint32_t)0x00000080) /*!< Falling trigger event configuration bit of line 7 */
+#define EXTI_FTSR_TR8 ((uint32_t)0x00000100) /*!< Falling trigger event configuration bit of line 8 */
+#define EXTI_FTSR_TR9 ((uint32_t)0x00000200) /*!< Falling trigger event configuration bit of line 9 */
+#define EXTI_FTSR_TR10 ((uint32_t)0x00000400) /*!< Falling trigger event configuration bit of line 10 */
+#define EXTI_FTSR_TR11 ((uint32_t)0x00000800) /*!< Falling trigger event configuration bit of line 11 */
+#define EXTI_FTSR_TR12 ((uint32_t)0x00001000) /*!< Falling trigger event configuration bit of line 12 */
+#define EXTI_FTSR_TR13 ((uint32_t)0x00002000) /*!< Falling trigger event configuration bit of line 13 */
+#define EXTI_FTSR_TR14 ((uint32_t)0x00004000) /*!< Falling trigger event configuration bit of line 14 */
+#define EXTI_FTSR_TR15 ((uint32_t)0x00008000) /*!< Falling trigger event configuration bit of line 15 */
+#define EXTI_FTSR_TR16 ((uint32_t)0x00010000) /*!< Falling trigger event configuration bit of line 16 */
+#define EXTI_FTSR_TR17 ((uint32_t)0x00020000) /*!< Falling trigger event configuration bit of line 17 */
+#define EXTI_FTSR_TR18 ((uint32_t)0x00040000) /*!< Falling trigger event configuration bit of line 18 */
+#define EXTI_FTSR_TR19 ((uint32_t)0x00080000) /*!< Falling trigger event configuration bit of line 19 */
+
+/****************** Bit definition for EXTI_SWIER register ******************/
+#define EXTI_SWIER_SWIER0 ((uint32_t)0x00000001) /*!< Software Interrupt on line 0 */
+#define EXTI_SWIER_SWIER1 ((uint32_t)0x00000002) /*!< Software Interrupt on line 1 */
+#define EXTI_SWIER_SWIER2 ((uint32_t)0x00000004) /*!< Software Interrupt on line 2 */
+#define EXTI_SWIER_SWIER3 ((uint32_t)0x00000008) /*!< Software Interrupt on line 3 */
+#define EXTI_SWIER_SWIER4 ((uint32_t)0x00000010) /*!< Software Interrupt on line 4 */
+#define EXTI_SWIER_SWIER5 ((uint32_t)0x00000020) /*!< Software Interrupt on line 5 */
+#define EXTI_SWIER_SWIER6 ((uint32_t)0x00000040) /*!< Software Interrupt on line 6 */
+#define EXTI_SWIER_SWIER7 ((uint32_t)0x00000080) /*!< Software Interrupt on line 7 */
+#define EXTI_SWIER_SWIER8 ((uint32_t)0x00000100) /*!< Software Interrupt on line 8 */
+#define EXTI_SWIER_SWIER9 ((uint32_t)0x00000200) /*!< Software Interrupt on line 9 */
+#define EXTI_SWIER_SWIER10 ((uint32_t)0x00000400) /*!< Software Interrupt on line 10 */
+#define EXTI_SWIER_SWIER11 ((uint32_t)0x00000800) /*!< Software Interrupt on line 11 */
+#define EXTI_SWIER_SWIER12 ((uint32_t)0x00001000) /*!< Software Interrupt on line 12 */
+#define EXTI_SWIER_SWIER13 ((uint32_t)0x00002000) /*!< Software Interrupt on line 13 */
+#define EXTI_SWIER_SWIER14 ((uint32_t)0x00004000) /*!< Software Interrupt on line 14 */
+#define EXTI_SWIER_SWIER15 ((uint32_t)0x00008000) /*!< Software Interrupt on line 15 */
+#define EXTI_SWIER_SWIER16 ((uint32_t)0x00010000) /*!< Software Interrupt on line 16 */
+#define EXTI_SWIER_SWIER17 ((uint32_t)0x00020000) /*!< Software Interrupt on line 17 */
+#define EXTI_SWIER_SWIER18 ((uint32_t)0x00040000) /*!< Software Interrupt on line 18 */
+#define EXTI_SWIER_SWIER19 ((uint32_t)0x00080000) /*!< Software Interrupt on line 19 */
+
+/******************* Bit definition for EXTI_PR register ********************/
+#define EXTI_PR_PR0 ((uint32_t)0x00000001) /*!< Pending bit for line 0 */
+#define EXTI_PR_PR1 ((uint32_t)0x00000002) /*!< Pending bit for line 1 */
+#define EXTI_PR_PR2 ((uint32_t)0x00000004) /*!< Pending bit for line 2 */
+#define EXTI_PR_PR3 ((uint32_t)0x00000008) /*!< Pending bit for line 3 */
+#define EXTI_PR_PR4 ((uint32_t)0x00000010) /*!< Pending bit for line 4 */
+#define EXTI_PR_PR5 ((uint32_t)0x00000020) /*!< Pending bit for line 5 */
+#define EXTI_PR_PR6 ((uint32_t)0x00000040) /*!< Pending bit for line 6 */
+#define EXTI_PR_PR7 ((uint32_t)0x00000080) /*!< Pending bit for line 7 */
+#define EXTI_PR_PR8 ((uint32_t)0x00000100) /*!< Pending bit for line 8 */
+#define EXTI_PR_PR9 ((uint32_t)0x00000200) /*!< Pending bit for line 9 */
+#define EXTI_PR_PR10 ((uint32_t)0x00000400) /*!< Pending bit for line 10 */
+#define EXTI_PR_PR11 ((uint32_t)0x00000800) /*!< Pending bit for line 11 */
+#define EXTI_PR_PR12 ((uint32_t)0x00001000) /*!< Pending bit for line 12 */
+#define EXTI_PR_PR13 ((uint32_t)0x00002000) /*!< Pending bit for line 13 */
+#define EXTI_PR_PR14 ((uint32_t)0x00004000) /*!< Pending bit for line 14 */
+#define EXTI_PR_PR15 ((uint32_t)0x00008000) /*!< Pending bit for line 15 */
+#define EXTI_PR_PR16 ((uint32_t)0x00010000) /*!< Pending bit for line 16 */
+#define EXTI_PR_PR17 ((uint32_t)0x00020000) /*!< Pending bit for line 17 */
+#define EXTI_PR_PR18 ((uint32_t)0x00040000) /*!< Pending bit for line 18 */
+#define EXTI_PR_PR19 ((uint32_t)0x00080000) /*!< Pending bit for line 19 */
+
+/******************************************************************************/
+/* */
+/* DMA Controller */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for DMA_ISR register ********************/
+#define DMA_ISR_GIF1 ((uint32_t)0x00000001) /*!< Channel 1 Global interrupt flag */
+#define DMA_ISR_TCIF1 ((uint32_t)0x00000002) /*!< Channel 1 Transfer Complete flag */
+#define DMA_ISR_HTIF1 ((uint32_t)0x00000004) /*!< Channel 1 Half Transfer flag */
+#define DMA_ISR_TEIF1 ((uint32_t)0x00000008) /*!< Channel 1 Transfer Error flag */
+#define DMA_ISR_GIF2 ((uint32_t)0x00000010) /*!< Channel 2 Global interrupt flag */
+#define DMA_ISR_TCIF2 ((uint32_t)0x00000020) /*!< Channel 2 Transfer Complete flag */
+#define DMA_ISR_HTIF2 ((uint32_t)0x00000040) /*!< Channel 2 Half Transfer flag */
+#define DMA_ISR_TEIF2 ((uint32_t)0x00000080) /*!< Channel 2 Transfer Error flag */
+#define DMA_ISR_GIF3 ((uint32_t)0x00000100) /*!< Channel 3 Global interrupt flag */
+#define DMA_ISR_TCIF3 ((uint32_t)0x00000200) /*!< Channel 3 Transfer Complete flag */
+#define DMA_ISR_HTIF3 ((uint32_t)0x00000400) /*!< Channel 3 Half Transfer flag */
+#define DMA_ISR_TEIF3 ((uint32_t)0x00000800) /*!< Channel 3 Transfer Error flag */
+#define DMA_ISR_GIF4 ((uint32_t)0x00001000) /*!< Channel 4 Global interrupt flag */
+#define DMA_ISR_TCIF4 ((uint32_t)0x00002000) /*!< Channel 4 Transfer Complete flag */
+#define DMA_ISR_HTIF4 ((uint32_t)0x00004000) /*!< Channel 4 Half Transfer flag */
+#define DMA_ISR_TEIF4 ((uint32_t)0x00008000) /*!< Channel 4 Transfer Error flag */
+#define DMA_ISR_GIF5 ((uint32_t)0x00010000) /*!< Channel 5 Global interrupt flag */
+#define DMA_ISR_TCIF5 ((uint32_t)0x00020000) /*!< Channel 5 Transfer Complete flag */
+#define DMA_ISR_HTIF5 ((uint32_t)0x00040000) /*!< Channel 5 Half Transfer flag */
+#define DMA_ISR_TEIF5 ((uint32_t)0x00080000) /*!< Channel 5 Transfer Error flag */
+#define DMA_ISR_GIF6 ((uint32_t)0x00100000) /*!< Channel 6 Global interrupt flag */
+#define DMA_ISR_TCIF6 ((uint32_t)0x00200000) /*!< Channel 6 Transfer Complete flag */
+#define DMA_ISR_HTIF6 ((uint32_t)0x00400000) /*!< Channel 6 Half Transfer flag */
+#define DMA_ISR_TEIF6 ((uint32_t)0x00800000) /*!< Channel 6 Transfer Error flag */
+#define DMA_ISR_GIF7 ((uint32_t)0x01000000) /*!< Channel 7 Global interrupt flag */
+#define DMA_ISR_TCIF7 ((uint32_t)0x02000000) /*!< Channel 7 Transfer Complete flag */
+#define DMA_ISR_HTIF7 ((uint32_t)0x04000000) /*!< Channel 7 Half Transfer flag */
+#define DMA_ISR_TEIF7 ((uint32_t)0x08000000) /*!< Channel 7 Transfer Error flag */
+
+/******************* Bit definition for DMA_IFCR register *******************/
+#define DMA_IFCR_CGIF1 ((uint32_t)0x00000001) /*!< Channel 1 Global interrupt clear */
+#define DMA_IFCR_CTCIF1 ((uint32_t)0x00000002) /*!< Channel 1 Transfer Complete clear */
+#define DMA_IFCR_CHTIF1 ((uint32_t)0x00000004) /*!< Channel 1 Half Transfer clear */
+#define DMA_IFCR_CTEIF1 ((uint32_t)0x00000008) /*!< Channel 1 Transfer Error clear */
+#define DMA_IFCR_CGIF2 ((uint32_t)0x00000010) /*!< Channel 2 Global interrupt clear */
+#define DMA_IFCR_CTCIF2 ((uint32_t)0x00000020) /*!< Channel 2 Transfer Complete clear */
+#define DMA_IFCR_CHTIF2 ((uint32_t)0x00000040) /*!< Channel 2 Half Transfer clear */
+#define DMA_IFCR_CTEIF2 ((uint32_t)0x00000080) /*!< Channel 2 Transfer Error clear */
+#define DMA_IFCR_CGIF3 ((uint32_t)0x00000100) /*!< Channel 3 Global interrupt clear */
+#define DMA_IFCR_CTCIF3 ((uint32_t)0x00000200) /*!< Channel 3 Transfer Complete clear */
+#define DMA_IFCR_CHTIF3 ((uint32_t)0x00000400) /*!< Channel 3 Half Transfer clear */
+#define DMA_IFCR_CTEIF3 ((uint32_t)0x00000800) /*!< Channel 3 Transfer Error clear */
+#define DMA_IFCR_CGIF4 ((uint32_t)0x00001000) /*!< Channel 4 Global interrupt clear */
+#define DMA_IFCR_CTCIF4 ((uint32_t)0x00002000) /*!< Channel 4 Transfer Complete clear */
+#define DMA_IFCR_CHTIF4 ((uint32_t)0x00004000) /*!< Channel 4 Half Transfer clear */
+#define DMA_IFCR_CTEIF4 ((uint32_t)0x00008000) /*!< Channel 4 Transfer Error clear */
+#define DMA_IFCR_CGIF5 ((uint32_t)0x00010000) /*!< Channel 5 Global interrupt clear */
+#define DMA_IFCR_CTCIF5 ((uint32_t)0x00020000) /*!< Channel 5 Transfer Complete clear */
+#define DMA_IFCR_CHTIF5 ((uint32_t)0x00040000) /*!< Channel 5 Half Transfer clear */
+#define DMA_IFCR_CTEIF5 ((uint32_t)0x00080000) /*!< Channel 5 Transfer Error clear */
+#define DMA_IFCR_CGIF6 ((uint32_t)0x00100000) /*!< Channel 6 Global interrupt clear */
+#define DMA_IFCR_CTCIF6 ((uint32_t)0x00200000) /*!< Channel 6 Transfer Complete clear */
+#define DMA_IFCR_CHTIF6 ((uint32_t)0x00400000) /*!< Channel 6 Half Transfer clear */
+#define DMA_IFCR_CTEIF6 ((uint32_t)0x00800000) /*!< Channel 6 Transfer Error clear */
+#define DMA_IFCR_CGIF7 ((uint32_t)0x01000000) /*!< Channel 7 Global interrupt clear */
+#define DMA_IFCR_CTCIF7 ((uint32_t)0x02000000) /*!< Channel 7 Transfer Complete clear */
+#define DMA_IFCR_CHTIF7 ((uint32_t)0x04000000) /*!< Channel 7 Half Transfer clear */
+#define DMA_IFCR_CTEIF7 ((uint32_t)0x08000000) /*!< Channel 7 Transfer Error clear */
+
+/******************* Bit definition for DMA_CCR register *******************/
+#define DMA_CCR_EN ((uint32_t)0x00000001) /*!< Channel enable */
+#define DMA_CCR_TCIE ((uint32_t)0x00000002) /*!< Transfer complete interrupt enable */
+#define DMA_CCR_HTIE ((uint32_t)0x00000004) /*!< Half Transfer interrupt enable */
+#define DMA_CCR_TEIE ((uint32_t)0x00000008) /*!< Transfer error interrupt enable */
+#define DMA_CCR_DIR ((uint32_t)0x00000010) /*!< Data transfer direction */
+#define DMA_CCR_CIRC ((uint32_t)0x00000020) /*!< Circular mode */
+#define DMA_CCR_PINC ((uint32_t)0x00000040) /*!< Peripheral increment mode */
+#define DMA_CCR_MINC ((uint32_t)0x00000080) /*!< Memory increment mode */
+
+#define DMA_CCR_PSIZE ((uint32_t)0x00000300) /*!< PSIZE[1:0] bits (Peripheral size) */
+#define DMA_CCR_PSIZE_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define DMA_CCR_PSIZE_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+
+#define DMA_CCR_MSIZE ((uint32_t)0x00000C00) /*!< MSIZE[1:0] bits (Memory size) */
+#define DMA_CCR_MSIZE_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define DMA_CCR_MSIZE_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+
+#define DMA_CCR_PL ((uint32_t)0x00003000) /*!< PL[1:0] bits(Channel Priority level) */
+#define DMA_CCR_PL_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define DMA_CCR_PL_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define DMA_CCR_MEM2MEM ((uint32_t)0x00004000) /*!< Memory to memory mode */
+
+/****************** Bit definition for DMA_CNDTR register ******************/
+#define DMA_CNDTR_NDT ((uint32_t)0x0000FFFF) /*!< Number of data to Transfer */
+
+/****************** Bit definition for DMA_CPAR register *******************/
+#define DMA_CPAR_PA ((uint32_t)0xFFFFFFFF) /*!< Peripheral Address */
+
+/****************** Bit definition for DMA_CMAR register *******************/
+#define DMA_CMAR_MA ((uint32_t)0xFFFFFFFF) /*!< Memory Address */
+
+/******************************************************************************/
+/* */
+/* Analog to Digital Converter */
+/* */
+/******************************************************************************/
+
+/******************** Bit definition for ADC_SR register ********************/
+#define ADC_SR_AWD ((uint32_t)0x00000001) /*!< Analog watchdog flag */
+#define ADC_SR_EOC ((uint32_t)0x00000002) /*!< End of conversion */
+#define ADC_SR_JEOC ((uint32_t)0x00000004) /*!< Injected channel end of conversion */
+#define ADC_SR_JSTRT ((uint32_t)0x00000008) /*!< Injected channel Start flag */
+#define ADC_SR_STRT ((uint32_t)0x00000010) /*!< Regular channel Start flag */
+
+/******************* Bit definition for ADC_CR1 register ********************/
+#define ADC_CR1_AWDCH ((uint32_t)0x0000001F) /*!< AWDCH[4:0] bits (Analog watchdog channel select bits) */
+#define ADC_CR1_AWDCH_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define ADC_CR1_AWDCH_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define ADC_CR1_AWDCH_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define ADC_CR1_AWDCH_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define ADC_CR1_AWDCH_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+
+#define ADC_CR1_EOCIE ((uint32_t)0x00000020) /*!< Interrupt enable for EOC */
+#define ADC_CR1_AWDIE ((uint32_t)0x00000040) /*!< Analog Watchdog interrupt enable */
+#define ADC_CR1_JEOCIE ((uint32_t)0x00000080) /*!< Interrupt enable for injected channels */
+#define ADC_CR1_SCAN ((uint32_t)0x00000100) /*!< Scan mode */
+#define ADC_CR1_AWDSGL ((uint32_t)0x00000200) /*!< Enable the watchdog on a single channel in scan mode */
+#define ADC_CR1_JAUTO ((uint32_t)0x00000400) /*!< Automatic injected group conversion */
+#define ADC_CR1_DISCEN ((uint32_t)0x00000800) /*!< Discontinuous mode on regular channels */
+#define ADC_CR1_JDISCEN ((uint32_t)0x00001000) /*!< Discontinuous mode on injected channels */
+
+#define ADC_CR1_DISCNUM ((uint32_t)0x0000E000) /*!< DISCNUM[2:0] bits (Discontinuous mode channel count) */
+#define ADC_CR1_DISCNUM_0 ((uint32_t)0x00002000) /*!< Bit 0 */
+#define ADC_CR1_DISCNUM_1 ((uint32_t)0x00004000) /*!< Bit 1 */
+#define ADC_CR1_DISCNUM_2 ((uint32_t)0x00008000) /*!< Bit 2 */
+
+#define ADC_CR1_DUALMOD ((uint32_t)0x000F0000) /*!< DUALMOD[3:0] bits (Dual mode selection) */
+#define ADC_CR1_DUALMOD_0 ((uint32_t)0x00010000) /*!< Bit 0 */
+#define ADC_CR1_DUALMOD_1 ((uint32_t)0x00020000) /*!< Bit 1 */
+#define ADC_CR1_DUALMOD_2 ((uint32_t)0x00040000) /*!< Bit 2 */
+#define ADC_CR1_DUALMOD_3 ((uint32_t)0x00080000) /*!< Bit 3 */
+
+#define ADC_CR1_JAWDEN ((uint32_t)0x00400000) /*!< Analog watchdog enable on injected channels */
+#define ADC_CR1_AWDEN ((uint32_t)0x00800000) /*!< Analog watchdog enable on regular channels */
+
+
+/******************* Bit definition for ADC_CR2 register ********************/
+#define ADC_CR2_ADON ((uint32_t)0x00000001) /*!< A/D Converter ON / OFF */
+#define ADC_CR2_CONT ((uint32_t)0x00000002) /*!< Continuous Conversion */
+#define ADC_CR2_CAL ((uint32_t)0x00000004) /*!< A/D Calibration */
+#define ADC_CR2_RSTCAL ((uint32_t)0x00000008) /*!< Reset Calibration */
+#define ADC_CR2_DMA ((uint32_t)0x00000100) /*!< Direct Memory access mode */
+#define ADC_CR2_ALIGN ((uint32_t)0x00000800) /*!< Data Alignment */
+
+#define ADC_CR2_JEXTSEL ((uint32_t)0x00007000) /*!< JEXTSEL[2:0] bits (External event select for injected group) */
+#define ADC_CR2_JEXTSEL_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define ADC_CR2_JEXTSEL_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+#define ADC_CR2_JEXTSEL_2 ((uint32_t)0x00004000) /*!< Bit 2 */
+
+#define ADC_CR2_JEXTTRIG ((uint32_t)0x00008000) /*!< External Trigger Conversion mode for injected channels */
+
+#define ADC_CR2_EXTSEL ((uint32_t)0x000E0000) /*!< EXTSEL[2:0] bits (External Event Select for regular group) */
+#define ADC_CR2_EXTSEL_0 ((uint32_t)0x00020000) /*!< Bit 0 */
+#define ADC_CR2_EXTSEL_1 ((uint32_t)0x00040000) /*!< Bit 1 */
+#define ADC_CR2_EXTSEL_2 ((uint32_t)0x00080000) /*!< Bit 2 */
+
+#define ADC_CR2_EXTTRIG ((uint32_t)0x00100000) /*!< External Trigger Conversion mode for regular channels */
+#define ADC_CR2_JSWSTART ((uint32_t)0x00200000) /*!< Start Conversion of injected channels */
+#define ADC_CR2_SWSTART ((uint32_t)0x00400000) /*!< Start Conversion of regular channels */
+#define ADC_CR2_TSVREFE ((uint32_t)0x00800000) /*!< Temperature Sensor and VREFINT Enable */
+
+/****************** Bit definition for ADC_SMPR1 register *******************/
+#define ADC_SMPR1_SMP10 ((uint32_t)0x00000007) /*!< SMP10[2:0] bits (Channel 10 Sample time selection) */
+#define ADC_SMPR1_SMP10_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define ADC_SMPR1_SMP10_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define ADC_SMPR1_SMP10_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+
+#define ADC_SMPR1_SMP11 ((uint32_t)0x00000038) /*!< SMP11[2:0] bits (Channel 11 Sample time selection) */
+#define ADC_SMPR1_SMP11_0 ((uint32_t)0x00000008) /*!< Bit 0 */
+#define ADC_SMPR1_SMP11_1 ((uint32_t)0x00000010) /*!< Bit 1 */
+#define ADC_SMPR1_SMP11_2 ((uint32_t)0x00000020) /*!< Bit 2 */
+
+#define ADC_SMPR1_SMP12 ((uint32_t)0x000001C0) /*!< SMP12[2:0] bits (Channel 12 Sample time selection) */
+#define ADC_SMPR1_SMP12_0 ((uint32_t)0x00000040) /*!< Bit 0 */
+#define ADC_SMPR1_SMP12_1 ((uint32_t)0x00000080) /*!< Bit 1 */
+#define ADC_SMPR1_SMP12_2 ((uint32_t)0x00000100) /*!< Bit 2 */
+
+#define ADC_SMPR1_SMP13 ((uint32_t)0x00000E00) /*!< SMP13[2:0] bits (Channel 13 Sample time selection) */
+#define ADC_SMPR1_SMP13_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define ADC_SMPR1_SMP13_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+#define ADC_SMPR1_SMP13_2 ((uint32_t)0x00000800) /*!< Bit 2 */
+
+#define ADC_SMPR1_SMP14 ((uint32_t)0x00007000) /*!< SMP14[2:0] bits (Channel 14 Sample time selection) */
+#define ADC_SMPR1_SMP14_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define ADC_SMPR1_SMP14_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+#define ADC_SMPR1_SMP14_2 ((uint32_t)0x00004000) /*!< Bit 2 */
+
+#define ADC_SMPR1_SMP15 ((uint32_t)0x00038000) /*!< SMP15[2:0] bits (Channel 15 Sample time selection) */
+#define ADC_SMPR1_SMP15_0 ((uint32_t)0x00008000) /*!< Bit 0 */
+#define ADC_SMPR1_SMP15_1 ((uint32_t)0x00010000) /*!< Bit 1 */
+#define ADC_SMPR1_SMP15_2 ((uint32_t)0x00020000) /*!< Bit 2 */
+
+#define ADC_SMPR1_SMP16 ((uint32_t)0x001C0000) /*!< SMP16[2:0] bits (Channel 16 Sample time selection) */
+#define ADC_SMPR1_SMP16_0 ((uint32_t)0x00040000) /*!< Bit 0 */
+#define ADC_SMPR1_SMP16_1 ((uint32_t)0x00080000) /*!< Bit 1 */
+#define ADC_SMPR1_SMP16_2 ((uint32_t)0x00100000) /*!< Bit 2 */
+
+#define ADC_SMPR1_SMP17 ((uint32_t)0x00E00000) /*!< SMP17[2:0] bits (Channel 17 Sample time selection) */
+#define ADC_SMPR1_SMP17_0 ((uint32_t)0x00200000) /*!< Bit 0 */
+#define ADC_SMPR1_SMP17_1 ((uint32_t)0x00400000) /*!< Bit 1 */
+#define ADC_SMPR1_SMP17_2 ((uint32_t)0x00800000) /*!< Bit 2 */
+
+/****************** Bit definition for ADC_SMPR2 register *******************/
+#define ADC_SMPR2_SMP0 ((uint32_t)0x00000007) /*!< SMP0[2:0] bits (Channel 0 Sample time selection) */
+#define ADC_SMPR2_SMP0_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define ADC_SMPR2_SMP0_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define ADC_SMPR2_SMP0_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+
+#define ADC_SMPR2_SMP1 ((uint32_t)0x00000038) /*!< SMP1[2:0] bits (Channel 1 Sample time selection) */
+#define ADC_SMPR2_SMP1_0 ((uint32_t)0x00000008) /*!< Bit 0 */
+#define ADC_SMPR2_SMP1_1 ((uint32_t)0x00000010) /*!< Bit 1 */
+#define ADC_SMPR2_SMP1_2 ((uint32_t)0x00000020) /*!< Bit 2 */
+
+#define ADC_SMPR2_SMP2 ((uint32_t)0x000001C0) /*!< SMP2[2:0] bits (Channel 2 Sample time selection) */
+#define ADC_SMPR2_SMP2_0 ((uint32_t)0x00000040) /*!< Bit 0 */
+#define ADC_SMPR2_SMP2_1 ((uint32_t)0x00000080) /*!< Bit 1 */
+#define ADC_SMPR2_SMP2_2 ((uint32_t)0x00000100) /*!< Bit 2 */
+
+#define ADC_SMPR2_SMP3 ((uint32_t)0x00000E00) /*!< SMP3[2:0] bits (Channel 3 Sample time selection) */
+#define ADC_SMPR2_SMP3_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define ADC_SMPR2_SMP3_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+#define ADC_SMPR2_SMP3_2 ((uint32_t)0x00000800) /*!< Bit 2 */
+
+#define ADC_SMPR2_SMP4 ((uint32_t)0x00007000) /*!< SMP4[2:0] bits (Channel 4 Sample time selection) */
+#define ADC_SMPR2_SMP4_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define ADC_SMPR2_SMP4_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+#define ADC_SMPR2_SMP4_2 ((uint32_t)0x00004000) /*!< Bit 2 */
+
+#define ADC_SMPR2_SMP5 ((uint32_t)0x00038000) /*!< SMP5[2:0] bits (Channel 5 Sample time selection) */
+#define ADC_SMPR2_SMP5_0 ((uint32_t)0x00008000) /*!< Bit 0 */
+#define ADC_SMPR2_SMP5_1 ((uint32_t)0x00010000) /*!< Bit 1 */
+#define ADC_SMPR2_SMP5_2 ((uint32_t)0x00020000) /*!< Bit 2 */
+
+#define ADC_SMPR2_SMP6 ((uint32_t)0x001C0000) /*!< SMP6[2:0] bits (Channel 6 Sample time selection) */
+#define ADC_SMPR2_SMP6_0 ((uint32_t)0x00040000) /*!< Bit 0 */
+#define ADC_SMPR2_SMP6_1 ((uint32_t)0x00080000) /*!< Bit 1 */
+#define ADC_SMPR2_SMP6_2 ((uint32_t)0x00100000) /*!< Bit 2 */
+
+#define ADC_SMPR2_SMP7 ((uint32_t)0x00E00000) /*!< SMP7[2:0] bits (Channel 7 Sample time selection) */
+#define ADC_SMPR2_SMP7_0 ((uint32_t)0x00200000) /*!< Bit 0 */
+#define ADC_SMPR2_SMP7_1 ((uint32_t)0x00400000) /*!< Bit 1 */
+#define ADC_SMPR2_SMP7_2 ((uint32_t)0x00800000) /*!< Bit 2 */
+
+#define ADC_SMPR2_SMP8 ((uint32_t)0x07000000) /*!< SMP8[2:0] bits (Channel 8 Sample time selection) */
+#define ADC_SMPR2_SMP8_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+#define ADC_SMPR2_SMP8_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+#define ADC_SMPR2_SMP8_2 ((uint32_t)0x04000000) /*!< Bit 2 */
+
+#define ADC_SMPR2_SMP9 ((uint32_t)0x38000000) /*!< SMP9[2:0] bits (Channel 9 Sample time selection) */
+#define ADC_SMPR2_SMP9_0 ((uint32_t)0x08000000) /*!< Bit 0 */
+#define ADC_SMPR2_SMP9_1 ((uint32_t)0x10000000) /*!< Bit 1 */
+#define ADC_SMPR2_SMP9_2 ((uint32_t)0x20000000) /*!< Bit 2 */
+
+/****************** Bit definition for ADC_JOFR1 register *******************/
+#define ADC_JOFR1_JOFFSET1 ((uint32_t)0x00000FFF) /*!< Data offset for injected channel 1 */
+
+/****************** Bit definition for ADC_JOFR2 register *******************/
+#define ADC_JOFR2_JOFFSET2 ((uint32_t)0x00000FFF) /*!< Data offset for injected channel 2 */
+
+/****************** Bit definition for ADC_JOFR3 register *******************/
+#define ADC_JOFR3_JOFFSET3 ((uint32_t)0x00000FFF) /*!< Data offset for injected channel 3 */
+
+/****************** Bit definition for ADC_JOFR4 register *******************/
+#define ADC_JOFR4_JOFFSET4 ((uint32_t)0x00000FFF) /*!< Data offset for injected channel 4 */
+
+/******************* Bit definition for ADC_HTR register ********************/
+#define ADC_HTR_HT ((uint32_t)0x00000FFF) /*!< Analog watchdog high threshold */
+
+/******************* Bit definition for ADC_LTR register ********************/
+#define ADC_LTR_LT ((uint32_t)0x00000FFF) /*!< Analog watchdog low threshold */
+
+/******************* Bit definition for ADC_SQR1 register *******************/
+#define ADC_SQR1_SQ13 ((uint32_t)0x0000001F) /*!< SQ13[4:0] bits (13th conversion in regular sequence) */
+#define ADC_SQR1_SQ13_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define ADC_SQR1_SQ13_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define ADC_SQR1_SQ13_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define ADC_SQR1_SQ13_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define ADC_SQR1_SQ13_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+
+#define ADC_SQR1_SQ14 ((uint32_t)0x000003E0) /*!< SQ14[4:0] bits (14th conversion in regular sequence) */
+#define ADC_SQR1_SQ14_0 ((uint32_t)0x00000020) /*!< Bit 0 */
+#define ADC_SQR1_SQ14_1 ((uint32_t)0x00000040) /*!< Bit 1 */
+#define ADC_SQR1_SQ14_2 ((uint32_t)0x00000080) /*!< Bit 2 */
+#define ADC_SQR1_SQ14_3 ((uint32_t)0x00000100) /*!< Bit 3 */
+#define ADC_SQR1_SQ14_4 ((uint32_t)0x00000200) /*!< Bit 4 */
+
+#define ADC_SQR1_SQ15 ((uint32_t)0x00007C00) /*!< SQ15[4:0] bits (15th conversion in regular sequence) */
+#define ADC_SQR1_SQ15_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define ADC_SQR1_SQ15_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define ADC_SQR1_SQ15_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define ADC_SQR1_SQ15_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define ADC_SQR1_SQ15_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define ADC_SQR1_SQ16 ((uint32_t)0x000F8000) /*!< SQ16[4:0] bits (16th conversion in regular sequence) */
+#define ADC_SQR1_SQ16_0 ((uint32_t)0x00008000) /*!< Bit 0 */
+#define ADC_SQR1_SQ16_1 ((uint32_t)0x00010000) /*!< Bit 1 */
+#define ADC_SQR1_SQ16_2 ((uint32_t)0x00020000) /*!< Bit 2 */
+#define ADC_SQR1_SQ16_3 ((uint32_t)0x00040000) /*!< Bit 3 */
+#define ADC_SQR1_SQ16_4 ((uint32_t)0x00080000) /*!< Bit 4 */
+
+#define ADC_SQR1_L ((uint32_t)0x00F00000) /*!< L[3:0] bits (Regular channel sequence length) */
+#define ADC_SQR1_L_0 ((uint32_t)0x00100000) /*!< Bit 0 */
+#define ADC_SQR1_L_1 ((uint32_t)0x00200000) /*!< Bit 1 */
+#define ADC_SQR1_L_2 ((uint32_t)0x00400000) /*!< Bit 2 */
+#define ADC_SQR1_L_3 ((uint32_t)0x00800000) /*!< Bit 3 */
+
+/******************* Bit definition for ADC_SQR2 register *******************/
+#define ADC_SQR2_SQ7 ((uint32_t)0x0000001F) /*!< SQ7[4:0] bits (7th conversion in regular sequence) */
+#define ADC_SQR2_SQ7_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define ADC_SQR2_SQ7_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define ADC_SQR2_SQ7_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define ADC_SQR2_SQ7_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define ADC_SQR2_SQ7_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+
+#define ADC_SQR2_SQ8 ((uint32_t)0x000003E0) /*!< SQ8[4:0] bits (8th conversion in regular sequence) */
+#define ADC_SQR2_SQ8_0 ((uint32_t)0x00000020) /*!< Bit 0 */
+#define ADC_SQR2_SQ8_1 ((uint32_t)0x00000040) /*!< Bit 1 */
+#define ADC_SQR2_SQ8_2 ((uint32_t)0x00000080) /*!< Bit 2 */
+#define ADC_SQR2_SQ8_3 ((uint32_t)0x00000100) /*!< Bit 3 */
+#define ADC_SQR2_SQ8_4 ((uint32_t)0x00000200) /*!< Bit 4 */
+
+#define ADC_SQR2_SQ9 ((uint32_t)0x00007C00) /*!< SQ9[4:0] bits (9th conversion in regular sequence) */
+#define ADC_SQR2_SQ9_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define ADC_SQR2_SQ9_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define ADC_SQR2_SQ9_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define ADC_SQR2_SQ9_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define ADC_SQR2_SQ9_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define ADC_SQR2_SQ10 ((uint32_t)0x000F8000) /*!< SQ10[4:0] bits (10th conversion in regular sequence) */
+#define ADC_SQR2_SQ10_0 ((uint32_t)0x00008000) /*!< Bit 0 */
+#define ADC_SQR2_SQ10_1 ((uint32_t)0x00010000) /*!< Bit 1 */
+#define ADC_SQR2_SQ10_2 ((uint32_t)0x00020000) /*!< Bit 2 */
+#define ADC_SQR2_SQ10_3 ((uint32_t)0x00040000) /*!< Bit 3 */
+#define ADC_SQR2_SQ10_4 ((uint32_t)0x00080000) /*!< Bit 4 */
+
+#define ADC_SQR2_SQ11 ((uint32_t)0x01F00000) /*!< SQ11[4:0] bits (11th conversion in regular sequence) */
+#define ADC_SQR2_SQ11_0 ((uint32_t)0x00100000) /*!< Bit 0 */
+#define ADC_SQR2_SQ11_1 ((uint32_t)0x00200000) /*!< Bit 1 */
+#define ADC_SQR2_SQ11_2 ((uint32_t)0x00400000) /*!< Bit 2 */
+#define ADC_SQR2_SQ11_3 ((uint32_t)0x00800000) /*!< Bit 3 */
+#define ADC_SQR2_SQ11_4 ((uint32_t)0x01000000) /*!< Bit 4 */
+
+#define ADC_SQR2_SQ12 ((uint32_t)0x3E000000) /*!< SQ12[4:0] bits (12th conversion in regular sequence) */
+#define ADC_SQR2_SQ12_0 ((uint32_t)0x02000000) /*!< Bit 0 */
+#define ADC_SQR2_SQ12_1 ((uint32_t)0x04000000) /*!< Bit 1 */
+#define ADC_SQR2_SQ12_2 ((uint32_t)0x08000000) /*!< Bit 2 */
+#define ADC_SQR2_SQ12_3 ((uint32_t)0x10000000) /*!< Bit 3 */
+#define ADC_SQR2_SQ12_4 ((uint32_t)0x20000000) /*!< Bit 4 */
+
+/******************* Bit definition for ADC_SQR3 register *******************/
+#define ADC_SQR3_SQ1 ((uint32_t)0x0000001F) /*!< SQ1[4:0] bits (1st conversion in regular sequence) */
+#define ADC_SQR3_SQ1_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define ADC_SQR3_SQ1_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define ADC_SQR3_SQ1_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define ADC_SQR3_SQ1_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define ADC_SQR3_SQ1_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+
+#define ADC_SQR3_SQ2 ((uint32_t)0x000003E0) /*!< SQ2[4:0] bits (2nd conversion in regular sequence) */
+#define ADC_SQR3_SQ2_0 ((uint32_t)0x00000020) /*!< Bit 0 */
+#define ADC_SQR3_SQ2_1 ((uint32_t)0x00000040) /*!< Bit 1 */
+#define ADC_SQR3_SQ2_2 ((uint32_t)0x00000080) /*!< Bit 2 */
+#define ADC_SQR3_SQ2_3 ((uint32_t)0x00000100) /*!< Bit 3 */
+#define ADC_SQR3_SQ2_4 ((uint32_t)0x00000200) /*!< Bit 4 */
+
+#define ADC_SQR3_SQ3 ((uint32_t)0x00007C00) /*!< SQ3[4:0] bits (3rd conversion in regular sequence) */
+#define ADC_SQR3_SQ3_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define ADC_SQR3_SQ3_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define ADC_SQR3_SQ3_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define ADC_SQR3_SQ3_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define ADC_SQR3_SQ3_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define ADC_SQR3_SQ4 ((uint32_t)0x000F8000) /*!< SQ4[4:0] bits (4th conversion in regular sequence) */
+#define ADC_SQR3_SQ4_0 ((uint32_t)0x00008000) /*!< Bit 0 */
+#define ADC_SQR3_SQ4_1 ((uint32_t)0x00010000) /*!< Bit 1 */
+#define ADC_SQR3_SQ4_2 ((uint32_t)0x00020000) /*!< Bit 2 */
+#define ADC_SQR3_SQ4_3 ((uint32_t)0x00040000) /*!< Bit 3 */
+#define ADC_SQR3_SQ4_4 ((uint32_t)0x00080000) /*!< Bit 4 */
+
+#define ADC_SQR3_SQ5 ((uint32_t)0x01F00000) /*!< SQ5[4:0] bits (5th conversion in regular sequence) */
+#define ADC_SQR3_SQ5_0 ((uint32_t)0x00100000) /*!< Bit 0 */
+#define ADC_SQR3_SQ5_1 ((uint32_t)0x00200000) /*!< Bit 1 */
+#define ADC_SQR3_SQ5_2 ((uint32_t)0x00400000) /*!< Bit 2 */
+#define ADC_SQR3_SQ5_3 ((uint32_t)0x00800000) /*!< Bit 3 */
+#define ADC_SQR3_SQ5_4 ((uint32_t)0x01000000) /*!< Bit 4 */
+
+#define ADC_SQR3_SQ6 ((uint32_t)0x3E000000) /*!< SQ6[4:0] bits (6th conversion in regular sequence) */
+#define ADC_SQR3_SQ6_0 ((uint32_t)0x02000000) /*!< Bit 0 */
+#define ADC_SQR3_SQ6_1 ((uint32_t)0x04000000) /*!< Bit 1 */
+#define ADC_SQR3_SQ6_2 ((uint32_t)0x08000000) /*!< Bit 2 */
+#define ADC_SQR3_SQ6_3 ((uint32_t)0x10000000) /*!< Bit 3 */
+#define ADC_SQR3_SQ6_4 ((uint32_t)0x20000000) /*!< Bit 4 */
+
+/******************* Bit definition for ADC_JSQR register *******************/
+#define ADC_JSQR_JSQ1 ((uint32_t)0x0000001F) /*!< JSQ1[4:0] bits (1st conversion in injected sequence) */
+#define ADC_JSQR_JSQ1_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define ADC_JSQR_JSQ1_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define ADC_JSQR_JSQ1_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define ADC_JSQR_JSQ1_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define ADC_JSQR_JSQ1_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+
+#define ADC_JSQR_JSQ2 ((uint32_t)0x000003E0) /*!< JSQ2[4:0] bits (2nd conversion in injected sequence) */
+#define ADC_JSQR_JSQ2_0 ((uint32_t)0x00000020) /*!< Bit 0 */
+#define ADC_JSQR_JSQ2_1 ((uint32_t)0x00000040) /*!< Bit 1 */
+#define ADC_JSQR_JSQ2_2 ((uint32_t)0x00000080) /*!< Bit 2 */
+#define ADC_JSQR_JSQ2_3 ((uint32_t)0x00000100) /*!< Bit 3 */
+#define ADC_JSQR_JSQ2_4 ((uint32_t)0x00000200) /*!< Bit 4 */
+
+#define ADC_JSQR_JSQ3 ((uint32_t)0x00007C00) /*!< JSQ3[4:0] bits (3rd conversion in injected sequence) */
+#define ADC_JSQR_JSQ3_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define ADC_JSQR_JSQ3_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define ADC_JSQR_JSQ3_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define ADC_JSQR_JSQ3_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define ADC_JSQR_JSQ3_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define ADC_JSQR_JSQ4 ((uint32_t)0x000F8000) /*!< JSQ4[4:0] bits (4th conversion in injected sequence) */
+#define ADC_JSQR_JSQ4_0 ((uint32_t)0x00008000) /*!< Bit 0 */
+#define ADC_JSQR_JSQ4_1 ((uint32_t)0x00010000) /*!< Bit 1 */
+#define ADC_JSQR_JSQ4_2 ((uint32_t)0x00020000) /*!< Bit 2 */
+#define ADC_JSQR_JSQ4_3 ((uint32_t)0x00040000) /*!< Bit 3 */
+#define ADC_JSQR_JSQ4_4 ((uint32_t)0x00080000) /*!< Bit 4 */
+
+#define ADC_JSQR_JL ((uint32_t)0x00300000) /*!< JL[1:0] bits (Injected Sequence length) */
+#define ADC_JSQR_JL_0 ((uint32_t)0x00100000) /*!< Bit 0 */
+#define ADC_JSQR_JL_1 ((uint32_t)0x00200000) /*!< Bit 1 */
+
+/******************* Bit definition for ADC_JDR1 register *******************/
+#define ADC_JDR1_JDATA ((uint32_t)0x0000FFFF) /*!< Injected data */
+
+/******************* Bit definition for ADC_JDR2 register *******************/
+#define ADC_JDR2_JDATA ((uint32_t)0x0000FFFF) /*!< Injected data */
+
+/******************* Bit definition for ADC_JDR3 register *******************/
+#define ADC_JDR3_JDATA ((uint32_t)0x0000FFFF) /*!< Injected data */
+
+/******************* Bit definition for ADC_JDR4 register *******************/
+#define ADC_JDR4_JDATA ((uint32_t)0x0000FFFF) /*!< Injected data */
+
+/******************** Bit definition for ADC_DR register ********************/
+#define ADC_DR_DATA ((uint32_t)0x0000FFFF) /*!< Regular data */
+#define ADC_DR_ADC2DATA ((uint32_t)0xFFFF0000) /*!< ADC2 data */
+/******************************************************************************/
+/* */
+/* Digital to Analog Converter */
+/* */
+/******************************************************************************/
+
+/******************** Bit definition for DAC_CR register ********************/
+#define DAC_CR_EN1 ((uint32_t)0x00000001) /*!< DAC channel1 enable */
+#define DAC_CR_BOFF1 ((uint32_t)0x00000002) /*!< DAC channel1 output buffer disable */
+#define DAC_CR_TEN1 ((uint32_t)0x00000004) /*!< DAC channel1 Trigger enable */
+
+#define DAC_CR_TSEL1 ((uint32_t)0x00000038) /*!< TSEL1[2:0] (DAC channel1 Trigger selection) */
+#define DAC_CR_TSEL1_0 ((uint32_t)0x00000008) /*!< Bit 0 */
+#define DAC_CR_TSEL1_1 ((uint32_t)0x00000010) /*!< Bit 1 */
+#define DAC_CR_TSEL1_2 ((uint32_t)0x00000020) /*!< Bit 2 */
+
+#define DAC_CR_WAVE1 ((uint32_t)0x000000C0) /*!< WAVE1[1:0] (DAC channel1 noise/triangle wave generation enable) */
+#define DAC_CR_WAVE1_0 ((uint32_t)0x00000040) /*!< Bit 0 */
+#define DAC_CR_WAVE1_1 ((uint32_t)0x00000080) /*!< Bit 1 */
+
+#define DAC_CR_MAMP1 ((uint32_t)0x00000F00) /*!< MAMP1[3:0] (DAC channel1 Mask/Amplitude selector) */
+#define DAC_CR_MAMP1_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define DAC_CR_MAMP1_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+#define DAC_CR_MAMP1_2 ((uint32_t)0x00000400) /*!< Bit 2 */
+#define DAC_CR_MAMP1_3 ((uint32_t)0x00000800) /*!< Bit 3 */
+
+#define DAC_CR_DMAEN1 ((uint32_t)0x00001000) /*!< DAC channel1 DMA enable */
+#define DAC_CR_EN2 ((uint32_t)0x00010000) /*!< DAC channel2 enable */
+#define DAC_CR_BOFF2 ((uint32_t)0x00020000) /*!< DAC channel2 output buffer disable */
+#define DAC_CR_TEN2 ((uint32_t)0x00040000) /*!< DAC channel2 Trigger enable */
+
+#define DAC_CR_TSEL2 ((uint32_t)0x00380000) /*!< TSEL2[2:0] (DAC channel2 Trigger selection) */
+#define DAC_CR_TSEL2_0 ((uint32_t)0x00080000) /*!< Bit 0 */
+#define DAC_CR_TSEL2_1 ((uint32_t)0x00100000) /*!< Bit 1 */
+#define DAC_CR_TSEL2_2 ((uint32_t)0x00200000) /*!< Bit 2 */
+
+#define DAC_CR_WAVE2 ((uint32_t)0x00C00000) /*!< WAVE2[1:0] (DAC channel2 noise/triangle wave generation enable) */
+#define DAC_CR_WAVE2_0 ((uint32_t)0x00400000) /*!< Bit 0 */
+#define DAC_CR_WAVE2_1 ((uint32_t)0x00800000) /*!< Bit 1 */
+
+#define DAC_CR_MAMP2 ((uint32_t)0x0F000000) /*!< MAMP2[3:0] (DAC channel2 Mask/Amplitude selector) */
+#define DAC_CR_MAMP2_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+#define DAC_CR_MAMP2_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+#define DAC_CR_MAMP2_2 ((uint32_t)0x04000000) /*!< Bit 2 */
+#define DAC_CR_MAMP2_3 ((uint32_t)0x08000000) /*!< Bit 3 */
+
+#define DAC_CR_DMAEN2 ((uint32_t)0x10000000) /*!< DAC channel2 DMA enabled */
+
+
+/***************** Bit definition for DAC_SWTRIGR register ******************/
+#define DAC_SWTRIGR_SWTRIG1 ((uint32_t)0x00000001) /*!< DAC channel1 software trigger */
+#define DAC_SWTRIGR_SWTRIG2 ((uint32_t)0x00000002) /*!< DAC channel2 software trigger */
+
+/***************** Bit definition for DAC_DHR12R1 register ******************/
+#define DAC_DHR12R1_DACC1DHR ((uint32_t)0x00000FFF) /*!< DAC channel1 12-bit Right aligned data */
+
+/***************** Bit definition for DAC_DHR12L1 register ******************/
+#define DAC_DHR12L1_DACC1DHR ((uint32_t)0x0000FFF0) /*!< DAC channel1 12-bit Left aligned data */
+
+/****************** Bit definition for DAC_DHR8R1 register ******************/
+#define DAC_DHR8R1_DACC1DHR ((uint32_t)0x000000FF) /*!< DAC channel1 8-bit Right aligned data */
+
+/***************** Bit definition for DAC_DHR12R2 register ******************/
+#define DAC_DHR12R2_DACC2DHR ((uint32_t)0x00000FFF) /*!< DAC channel2 12-bit Right aligned data */
+
+/***************** Bit definition for DAC_DHR12L2 register ******************/
+#define DAC_DHR12L2_DACC2DHR ((uint32_t)0x0000FFF0) /*!< DAC channel2 12-bit Left aligned data */
+
+/****************** Bit definition for DAC_DHR8R2 register ******************/
+#define DAC_DHR8R2_DACC2DHR ((uint32_t)0x000000FF) /*!< DAC channel2 8-bit Right aligned data */
+
+/***************** Bit definition for DAC_DHR12RD register ******************/
+#define DAC_DHR12RD_DACC1DHR ((uint32_t)0x00000FFF) /*!< DAC channel1 12-bit Right aligned data */
+#define DAC_DHR12RD_DACC2DHR ((uint32_t)0x0FFF0000) /*!< DAC channel2 12-bit Right aligned data */
+
+/***************** Bit definition for DAC_DHR12LD register ******************/
+#define DAC_DHR12LD_DACC1DHR ((uint32_t)0x0000FFF0) /*!< DAC channel1 12-bit Left aligned data */
+#define DAC_DHR12LD_DACC2DHR ((uint32_t)0xFFF00000) /*!< DAC channel2 12-bit Left aligned data */
+
+/****************** Bit definition for DAC_DHR8RD register ******************/
+#define DAC_DHR8RD_DACC1DHR ((uint32_t)0x000000FF) /*!< DAC channel1 8-bit Right aligned data */
+#define DAC_DHR8RD_DACC2DHR ((uint32_t)0x0000FF00) /*!< DAC channel2 8-bit Right aligned data */
+
+/******************* Bit definition for DAC_DOR1 register *******************/
+#define DAC_DOR1_DACC1DOR ((uint32_t)0x00000FFF) /*!< DAC channel1 data output */
+
+/******************* Bit definition for DAC_DOR2 register *******************/
+#define DAC_DOR2_DACC2DOR ((uint32_t)0x00000FFF) /*!< DAC channel2 data output */
+
+
+
+/*****************************************************************************/
+/* */
+/* Timers (TIM) */
+/* */
+/*****************************************************************************/
+/******************* Bit definition for TIM_CR1 register *******************/
+#define TIM_CR1_CEN ((uint32_t)0x00000001) /*!<Counter enable */
+#define TIM_CR1_UDIS ((uint32_t)0x00000002) /*!<Update disable */
+#define TIM_CR1_URS ((uint32_t)0x00000004) /*!<Update request source */
+#define TIM_CR1_OPM ((uint32_t)0x00000008) /*!<One pulse mode */
+#define TIM_CR1_DIR ((uint32_t)0x00000010) /*!<Direction */
+
+#define TIM_CR1_CMS ((uint32_t)0x00000060) /*!<CMS[1:0] bits (Center-aligned mode selection) */
+#define TIM_CR1_CMS_0 ((uint32_t)0x00000020) /*!<Bit 0 */
+#define TIM_CR1_CMS_1 ((uint32_t)0x00000040) /*!<Bit 1 */
+
+#define TIM_CR1_ARPE ((uint32_t)0x00000080) /*!<Auto-reload preload enable */
+
+#define TIM_CR1_CKD ((uint32_t)0x00000300) /*!<CKD[1:0] bits (clock division) */
+#define TIM_CR1_CKD_0 ((uint32_t)0x00000100) /*!<Bit 0 */
+#define TIM_CR1_CKD_1 ((uint32_t)0x00000200) /*!<Bit 1 */
+
+/******************* Bit definition for TIM_CR2 register *******************/
+#define TIM_CR2_CCPC ((uint32_t)0x00000001) /*!<Capture/Compare Preloaded Control */
+#define TIM_CR2_CCUS ((uint32_t)0x00000004) /*!<Capture/Compare Control Update Selection */
+#define TIM_CR2_CCDS ((uint32_t)0x00000008) /*!<Capture/Compare DMA Selection */
+
+#define TIM_CR2_MMS ((uint32_t)0x00000070) /*!<MMS[2:0] bits (Master Mode Selection) */
+#define TIM_CR2_MMS_0 ((uint32_t)0x00000010) /*!<Bit 0 */
+#define TIM_CR2_MMS_1 ((uint32_t)0x00000020) /*!<Bit 1 */
+#define TIM_CR2_MMS_2 ((uint32_t)0x00000040) /*!<Bit 2 */
+
+#define TIM_CR2_TI1S ((uint32_t)0x00000080) /*!<TI1 Selection */
+#define TIM_CR2_OIS1 ((uint32_t)0x00000100) /*!<Output Idle state 1 (OC1 output) */
+#define TIM_CR2_OIS1N ((uint32_t)0x00000200) /*!<Output Idle state 1 (OC1N output) */
+#define TIM_CR2_OIS2 ((uint32_t)0x00000400) /*!<Output Idle state 2 (OC2 output) */
+#define TIM_CR2_OIS2N ((uint32_t)0x00000800) /*!<Output Idle state 2 (OC2N output) */
+#define TIM_CR2_OIS3 ((uint32_t)0x00001000) /*!<Output Idle state 3 (OC3 output) */
+#define TIM_CR2_OIS3N ((uint32_t)0x00002000) /*!<Output Idle state 3 (OC3N output) */
+#define TIM_CR2_OIS4 ((uint32_t)0x00004000) /*!<Output Idle state 4 (OC4 output) */
+
+/******************* Bit definition for TIM_SMCR register ******************/
+#define TIM_SMCR_SMS ((uint32_t)0x00000007) /*!<SMS[2:0] bits (Slave mode selection) */
+#define TIM_SMCR_SMS_0 ((uint32_t)0x00000001) /*!<Bit 0 */
+#define TIM_SMCR_SMS_1 ((uint32_t)0x00000002) /*!<Bit 1 */
+#define TIM_SMCR_SMS_2 ((uint32_t)0x00000004) /*!<Bit 2 */
+
+#define TIM_SMCR_OCCS ((uint32_t)0x00000008) /*!< OCREF clear selection */
+
+#define TIM_SMCR_TS ((uint32_t)0x00000070) /*!<TS[2:0] bits (Trigger selection) */
+#define TIM_SMCR_TS_0 ((uint32_t)0x00000010) /*!<Bit 0 */
+#define TIM_SMCR_TS_1 ((uint32_t)0x00000020) /*!<Bit 1 */
+#define TIM_SMCR_TS_2 ((uint32_t)0x00000040) /*!<Bit 2 */
+
+#define TIM_SMCR_MSM ((uint32_t)0x00000080) /*!<Master/slave mode */
+
+#define TIM_SMCR_ETF ((uint32_t)0x00000F00) /*!<ETF[3:0] bits (External trigger filter) */
+#define TIM_SMCR_ETF_0 ((uint32_t)0x00000100) /*!<Bit 0 */
+#define TIM_SMCR_ETF_1 ((uint32_t)0x00000200) /*!<Bit 1 */
+#define TIM_SMCR_ETF_2 ((uint32_t)0x00000400) /*!<Bit 2 */
+#define TIM_SMCR_ETF_3 ((uint32_t)0x00000800) /*!<Bit 3 */
+
+#define TIM_SMCR_ETPS ((uint32_t)0x00003000) /*!<ETPS[1:0] bits (External trigger prescaler) */
+#define TIM_SMCR_ETPS_0 ((uint32_t)0x00001000) /*!<Bit 0 */
+#define TIM_SMCR_ETPS_1 ((uint32_t)0x00002000) /*!<Bit 1 */
+
+#define TIM_SMCR_ECE ((uint32_t)0x00004000) /*!<External clock enable */
+#define TIM_SMCR_ETP ((uint32_t)0x00008000) /*!<External trigger polarity */
+
+/******************* Bit definition for TIM_DIER register ******************/
+#define TIM_DIER_UIE ((uint32_t)0x00000001) /*!<Update interrupt enable */
+#define TIM_DIER_CC1IE ((uint32_t)0x00000002) /*!<Capture/Compare 1 interrupt enable */
+#define TIM_DIER_CC2IE ((uint32_t)0x00000004) /*!<Capture/Compare 2 interrupt enable */
+#define TIM_DIER_CC3IE ((uint32_t)0x00000008) /*!<Capture/Compare 3 interrupt enable */
+#define TIM_DIER_CC4IE ((uint32_t)0x00000010) /*!<Capture/Compare 4 interrupt enable */
+#define TIM_DIER_COMIE ((uint32_t)0x00000020) /*!<COM interrupt enable */
+#define TIM_DIER_TIE ((uint32_t)0x00000040) /*!<Trigger interrupt enable */
+#define TIM_DIER_BIE ((uint32_t)0x00000080) /*!<Break interrupt enable */
+#define TIM_DIER_UDE ((uint32_t)0x00000100) /*!<Update DMA request enable */
+#define TIM_DIER_CC1DE ((uint32_t)0x00000200) /*!<Capture/Compare 1 DMA request enable */
+#define TIM_DIER_CC2DE ((uint32_t)0x00000400) /*!<Capture/Compare 2 DMA request enable */
+#define TIM_DIER_CC3DE ((uint32_t)0x00000800) /*!<Capture/Compare 3 DMA request enable */
+#define TIM_DIER_CC4DE ((uint32_t)0x00001000) /*!<Capture/Compare 4 DMA request enable */
+#define TIM_DIER_COMDE ((uint32_t)0x00002000) /*!<COM DMA request enable */
+#define TIM_DIER_TDE ((uint32_t)0x00004000) /*!<Trigger DMA request enable */
+
+/******************** Bit definition for TIM_SR register *******************/
+#define TIM_SR_UIF ((uint32_t)0x00000001) /*!<Update interrupt Flag */
+#define TIM_SR_CC1IF ((uint32_t)0x00000002) /*!<Capture/Compare 1 interrupt Flag */
+#define TIM_SR_CC2IF ((uint32_t)0x00000004) /*!<Capture/Compare 2 interrupt Flag */
+#define TIM_SR_CC3IF ((uint32_t)0x00000008) /*!<Capture/Compare 3 interrupt Flag */
+#define TIM_SR_CC4IF ((uint32_t)0x00000010) /*!<Capture/Compare 4 interrupt Flag */
+#define TIM_SR_COMIF ((uint32_t)0x00000020) /*!<COM interrupt Flag */
+#define TIM_SR_TIF ((uint32_t)0x00000040) /*!<Trigger interrupt Flag */
+#define TIM_SR_BIF ((uint32_t)0x00000080) /*!<Break interrupt Flag */
+#define TIM_SR_CC1OF ((uint32_t)0x00000200) /*!<Capture/Compare 1 Overcapture Flag */
+#define TIM_SR_CC2OF ((uint32_t)0x00000400) /*!<Capture/Compare 2 Overcapture Flag */
+#define TIM_SR_CC3OF ((uint32_t)0x00000800) /*!<Capture/Compare 3 Overcapture Flag */
+#define TIM_SR_CC4OF ((uint32_t)0x00001000) /*!<Capture/Compare 4 Overcapture Flag */
+
+/******************* Bit definition for TIM_EGR register *******************/
+#define TIM_EGR_UG ((uint32_t)0x00000001) /*!<Update Generation */
+#define TIM_EGR_CC1G ((uint32_t)0x00000002) /*!<Capture/Compare 1 Generation */
+#define TIM_EGR_CC2G ((uint32_t)0x00000004) /*!<Capture/Compare 2 Generation */
+#define TIM_EGR_CC3G ((uint32_t)0x00000008) /*!<Capture/Compare 3 Generation */
+#define TIM_EGR_CC4G ((uint32_t)0x00000010) /*!<Capture/Compare 4 Generation */
+#define TIM_EGR_COMG ((uint32_t)0x00000020) /*!<Capture/Compare Control Update Generation */
+#define TIM_EGR_TG ((uint32_t)0x00000040) /*!<Trigger Generation */
+#define TIM_EGR_BG ((uint32_t)0x00000080) /*!<Break Generation */
+
+/****************** Bit definition for TIM_CCMR1 register ******************/
+#define TIM_CCMR1_CC1S ((uint32_t)0x00000003) /*!<CC1S[1:0] bits (Capture/Compare 1 Selection) */
+#define TIM_CCMR1_CC1S_0 ((uint32_t)0x00000001) /*!<Bit 0 */
+#define TIM_CCMR1_CC1S_1 ((uint32_t)0x00000002) /*!<Bit 1 */
+
+#define TIM_CCMR1_OC1FE ((uint32_t)0x00000004) /*!<Output Compare 1 Fast enable */
+#define TIM_CCMR1_OC1PE ((uint32_t)0x00000008) /*!<Output Compare 1 Preload enable */
+
+#define TIM_CCMR1_OC1M ((uint32_t)0x00000070) /*!<OC1M[2:0] bits (Output Compare 1 Mode) */
+#define TIM_CCMR1_OC1M_0 ((uint32_t)0x00000010) /*!<Bit 0 */
+#define TIM_CCMR1_OC1M_1 ((uint32_t)0x00000020) /*!<Bit 1 */
+#define TIM_CCMR1_OC1M_2 ((uint32_t)0x00000040) /*!<Bit 2 */
+
+#define TIM_CCMR1_OC1CE ((uint32_t)0x00000080) /*!<Output Compare 1Clear Enable */
+
+#define TIM_CCMR1_CC2S ((uint32_t)0x00000300) /*!<CC2S[1:0] bits (Capture/Compare 2 Selection) */
+#define TIM_CCMR1_CC2S_0 ((uint32_t)0x00000100) /*!<Bit 0 */
+#define TIM_CCMR1_CC2S_1 ((uint32_t)0x00000200) /*!<Bit 1 */
+
+#define TIM_CCMR1_OC2FE ((uint32_t)0x00000400) /*!<Output Compare 2 Fast enable */
+#define TIM_CCMR1_OC2PE ((uint32_t)0x00000800) /*!<Output Compare 2 Preload enable */
+
+#define TIM_CCMR1_OC2M ((uint32_t)0x00007000) /*!<OC2M[2:0] bits (Output Compare 2 Mode) */
+#define TIM_CCMR1_OC2M_0 ((uint32_t)0x00001000) /*!<Bit 0 */
+#define TIM_CCMR1_OC2M_1 ((uint32_t)0x00002000) /*!<Bit 1 */
+#define TIM_CCMR1_OC2M_2 ((uint32_t)0x00004000) /*!<Bit 2 */
+
+#define TIM_CCMR1_OC2CE ((uint32_t)0x00008000) /*!<Output Compare 2 Clear Enable */
+
+/*---------------------------------------------------------------------------*/
+
+#define TIM_CCMR1_IC1PSC ((uint32_t)0x0000000C) /*!<IC1PSC[1:0] bits (Input Capture 1 Prescaler) */
+#define TIM_CCMR1_IC1PSC_0 ((uint32_t)0x00000004) /*!<Bit 0 */
+#define TIM_CCMR1_IC1PSC_1 ((uint32_t)0x00000008) /*!<Bit 1 */
+
+#define TIM_CCMR1_IC1F ((uint32_t)0x000000F0) /*!<IC1F[3:0] bits (Input Capture 1 Filter) */
+#define TIM_CCMR1_IC1F_0 ((uint32_t)0x00000010) /*!<Bit 0 */
+#define TIM_CCMR1_IC1F_1 ((uint32_t)0x00000020) /*!<Bit 1 */
+#define TIM_CCMR1_IC1F_2 ((uint32_t)0x00000040) /*!<Bit 2 */
+#define TIM_CCMR1_IC1F_3 ((uint32_t)0x00000080) /*!<Bit 3 */
+
+#define TIM_CCMR1_IC2PSC ((uint32_t)0x00000C00) /*!<IC2PSC[1:0] bits (Input Capture 2 Prescaler) */
+#define TIM_CCMR1_IC2PSC_0 ((uint32_t)0x00000400) /*!<Bit 0 */
+#define TIM_CCMR1_IC2PSC_1 ((uint32_t)0x00000800) /*!<Bit 1 */
+
+#define TIM_CCMR1_IC2F ((uint32_t)0x0000F000) /*!<IC2F[3:0] bits (Input Capture 2 Filter) */
+#define TIM_CCMR1_IC2F_0 ((uint32_t)0x00001000) /*!<Bit 0 */
+#define TIM_CCMR1_IC2F_1 ((uint32_t)0x00002000) /*!<Bit 1 */
+#define TIM_CCMR1_IC2F_2 ((uint32_t)0x00004000) /*!<Bit 2 */
+#define TIM_CCMR1_IC2F_3 ((uint32_t)0x00008000) /*!<Bit 3 */
+
+/****************** Bit definition for TIM_CCMR2 register ******************/
+#define TIM_CCMR2_CC3S ((uint32_t)0x00000003) /*!<CC3S[1:0] bits (Capture/Compare 3 Selection) */
+#define TIM_CCMR2_CC3S_0 ((uint32_t)0x00000001) /*!<Bit 0 */
+#define TIM_CCMR2_CC3S_1 ((uint32_t)0x00000002) /*!<Bit 1 */
+
+#define TIM_CCMR2_OC3FE ((uint32_t)0x00000004) /*!<Output Compare 3 Fast enable */
+#define TIM_CCMR2_OC3PE ((uint32_t)0x00000008) /*!<Output Compare 3 Preload enable */
+
+#define TIM_CCMR2_OC3M ((uint32_t)0x00000070) /*!<OC3M[2:0] bits (Output Compare 3 Mode) */
+#define TIM_CCMR2_OC3M_0 ((uint32_t)0x00000010) /*!<Bit 0 */
+#define TIM_CCMR2_OC3M_1 ((uint32_t)0x00000020) /*!<Bit 1 */
+#define TIM_CCMR2_OC3M_2 ((uint32_t)0x00000040) /*!<Bit 2 */
+
+#define TIM_CCMR2_OC3CE ((uint32_t)0x00000080) /*!<Output Compare 3 Clear Enable */
+
+#define TIM_CCMR2_CC4S ((uint32_t)0x00000300) /*!<CC4S[1:0] bits (Capture/Compare 4 Selection) */
+#define TIM_CCMR2_CC4S_0 ((uint32_t)0x00000100) /*!<Bit 0 */
+#define TIM_CCMR2_CC4S_1 ((uint32_t)0x00000200) /*!<Bit 1 */
+
+#define TIM_CCMR2_OC4FE ((uint32_t)0x00000400) /*!<Output Compare 4 Fast enable */
+#define TIM_CCMR2_OC4PE ((uint32_t)0x00000800) /*!<Output Compare 4 Preload enable */
+
+#define TIM_CCMR2_OC4M ((uint32_t)0x00007000) /*!<OC4M[2:0] bits (Output Compare 4 Mode) */
+#define TIM_CCMR2_OC4M_0 ((uint32_t)0x00001000) /*!<Bit 0 */
+#define TIM_CCMR2_OC4M_1 ((uint32_t)0x00002000) /*!<Bit 1 */
+#define TIM_CCMR2_OC4M_2 ((uint32_t)0x00004000) /*!<Bit 2 */
+
+#define TIM_CCMR2_OC4CE ((uint32_t)0x00008000) /*!<Output Compare 4 Clear Enable */
+
+/*---------------------------------------------------------------------------*/
+
+#define TIM_CCMR2_IC3PSC ((uint32_t)0x0000000C) /*!<IC3PSC[1:0] bits (Input Capture 3 Prescaler) */
+#define TIM_CCMR2_IC3PSC_0 ((uint32_t)0x00000004) /*!<Bit 0 */
+#define TIM_CCMR2_IC3PSC_1 ((uint32_t)0x00000008) /*!<Bit 1 */
+
+#define TIM_CCMR2_IC3F ((uint32_t)0x000000F0) /*!<IC3F[3:0] bits (Input Capture 3 Filter) */
+#define TIM_CCMR2_IC3F_0 ((uint32_t)0x00000010) /*!<Bit 0 */
+#define TIM_CCMR2_IC3F_1 ((uint32_t)0x00000020) /*!<Bit 1 */
+#define TIM_CCMR2_IC3F_2 ((uint32_t)0x00000040) /*!<Bit 2 */
+#define TIM_CCMR2_IC3F_3 ((uint32_t)0x00000080) /*!<Bit 3 */
+
+#define TIM_CCMR2_IC4PSC ((uint32_t)0x00000C00) /*!<IC4PSC[1:0] bits (Input Capture 4 Prescaler) */
+#define TIM_CCMR2_IC4PSC_0 ((uint32_t)0x00000400) /*!<Bit 0 */
+#define TIM_CCMR2_IC4PSC_1 ((uint32_t)0x00000800) /*!<Bit 1 */
+
+#define TIM_CCMR2_IC4F ((uint32_t)0x0000F000) /*!<IC4F[3:0] bits (Input Capture 4 Filter) */
+#define TIM_CCMR2_IC4F_0 ((uint32_t)0x00001000) /*!<Bit 0 */
+#define TIM_CCMR2_IC4F_1 ((uint32_t)0x00002000) /*!<Bit 1 */
+#define TIM_CCMR2_IC4F_2 ((uint32_t)0x00004000) /*!<Bit 2 */
+#define TIM_CCMR2_IC4F_3 ((uint32_t)0x00008000) /*!<Bit 3 */
+
+/******************* Bit definition for TIM_CCER register ******************/
+#define TIM_CCER_CC1E ((uint32_t)0x00000001) /*!<Capture/Compare 1 output enable */
+#define TIM_CCER_CC1P ((uint32_t)0x00000002) /*!<Capture/Compare 1 output Polarity */
+#define TIM_CCER_CC1NE ((uint32_t)0x00000004) /*!<Capture/Compare 1 Complementary output enable */
+#define TIM_CCER_CC1NP ((uint32_t)0x00000008) /*!<Capture/Compare 1 Complementary output Polarity */
+#define TIM_CCER_CC2E ((uint32_t)0x00000010) /*!<Capture/Compare 2 output enable */
+#define TIM_CCER_CC2P ((uint32_t)0x00000020) /*!<Capture/Compare 2 output Polarity */
+#define TIM_CCER_CC2NE ((uint32_t)0x00000040) /*!<Capture/Compare 2 Complementary output enable */
+#define TIM_CCER_CC2NP ((uint32_t)0x00000080) /*!<Capture/Compare 2 Complementary output Polarity */
+#define TIM_CCER_CC3E ((uint32_t)0x00000100) /*!<Capture/Compare 3 output enable */
+#define TIM_CCER_CC3P ((uint32_t)0x00000200) /*!<Capture/Compare 3 output Polarity */
+#define TIM_CCER_CC3NE ((uint32_t)0x00000400) /*!<Capture/Compare 3 Complementary output enable */
+#define TIM_CCER_CC3NP ((uint32_t)0x00000800) /*!<Capture/Compare 3 Complementary output Polarity */
+#define TIM_CCER_CC4E ((uint32_t)0x00001000) /*!<Capture/Compare 4 output enable */
+#define TIM_CCER_CC4P ((uint32_t)0x00002000) /*!<Capture/Compare 4 output Polarity */
+#define TIM_CCER_CC4NP ((uint32_t)0x00008000) /*!<Capture/Compare 4 Complementary output Polarity */
+
+/******************* Bit definition for TIM_CNT register *******************/
+#define TIM_CNT_CNT ((uint32_t)0xFFFFFFFF) /*!<Counter Value */
+
+/******************* Bit definition for TIM_PSC register *******************/
+#define TIM_PSC_PSC ((uint32_t)0x0000FFFF) /*!<Prescaler Value */
+
+/******************* Bit definition for TIM_ARR register *******************/
+#define TIM_ARR_ARR ((uint32_t)0xFFFFFFFF) /*!<actual auto-reload Value */
+
+/******************* Bit definition for TIM_RCR register *******************/
+#define TIM_RCR_REP ((uint32_t)0x000000FF) /*!<Repetition Counter Value */
+
+/******************* Bit definition for TIM_CCR1 register ******************/
+#define TIM_CCR1_CCR1 ((uint32_t)0x0000FFFF) /*!<Capture/Compare 1 Value */
+
+/******************* Bit definition for TIM_CCR2 register ******************/
+#define TIM_CCR2_CCR2 ((uint32_t)0x0000FFFF) /*!<Capture/Compare 2 Value */
+
+/******************* Bit definition for TIM_CCR3 register ******************/
+#define TIM_CCR3_CCR3 ((uint32_t)0x0000FFFF) /*!<Capture/Compare 3 Value */
+
+/******************* Bit definition for TIM_CCR4 register ******************/
+#define TIM_CCR4_CCR4 ((uint32_t)0x0000FFFF) /*!<Capture/Compare 4 Value */
+
+/******************* Bit definition for TIM_BDTR register ******************/
+#define TIM_BDTR_DTG ((uint32_t)0x000000FF) /*!<DTG[0:7] bits (Dead-Time Generator set-up) */
+#define TIM_BDTR_DTG_0 ((uint32_t)0x00000001) /*!<Bit 0 */
+#define TIM_BDTR_DTG_1 ((uint32_t)0x00000002) /*!<Bit 1 */
+#define TIM_BDTR_DTG_2 ((uint32_t)0x00000004) /*!<Bit 2 */
+#define TIM_BDTR_DTG_3 ((uint32_t)0x00000008) /*!<Bit 3 */
+#define TIM_BDTR_DTG_4 ((uint32_t)0x00000010) /*!<Bit 4 */
+#define TIM_BDTR_DTG_5 ((uint32_t)0x00000020) /*!<Bit 5 */
+#define TIM_BDTR_DTG_6 ((uint32_t)0x00000040) /*!<Bit 6 */
+#define TIM_BDTR_DTG_7 ((uint32_t)0x00000080) /*!<Bit 7 */
+
+#define TIM_BDTR_LOCK ((uint32_t)0x00000300) /*!<LOCK[1:0] bits (Lock Configuration) */
+#define TIM_BDTR_LOCK_0 ((uint32_t)0x00000100) /*!<Bit 0 */
+#define TIM_BDTR_LOCK_1 ((uint32_t)0x00000200) /*!<Bit 1 */
+
+#define TIM_BDTR_OSSI ((uint32_t)0x00000400) /*!<Off-State Selection for Idle mode */
+#define TIM_BDTR_OSSR ((uint32_t)0x00000800) /*!<Off-State Selection for Run mode */
+#define TIM_BDTR_BKE ((uint32_t)0x00001000) /*!<Break enable */
+#define TIM_BDTR_BKP ((uint32_t)0x00002000) /*!<Break Polarity */
+#define TIM_BDTR_AOE ((uint32_t)0x00004000) /*!<Automatic Output enable */
+#define TIM_BDTR_MOE ((uint32_t)0x00008000) /*!<Main Output enable */
+
+/******************* Bit definition for TIM_DCR register *******************/
+#define TIM_DCR_DBA ((uint32_t)0x0000001F) /*!<DBA[4:0] bits (DMA Base Address) */
+#define TIM_DCR_DBA_0 ((uint32_t)0x00000001) /*!<Bit 0 */
+#define TIM_DCR_DBA_1 ((uint32_t)0x00000002) /*!<Bit 1 */
+#define TIM_DCR_DBA_2 ((uint32_t)0x00000004) /*!<Bit 2 */
+#define TIM_DCR_DBA_3 ((uint32_t)0x00000008) /*!<Bit 3 */
+#define TIM_DCR_DBA_4 ((uint32_t)0x00000010) /*!<Bit 4 */
+
+#define TIM_DCR_DBL ((uint32_t)0x00001F00) /*!<DBL[4:0] bits (DMA Burst Length) */
+#define TIM_DCR_DBL_0 ((uint32_t)0x00000100) /*!<Bit 0 */
+#define TIM_DCR_DBL_1 ((uint32_t)0x00000200) /*!<Bit 1 */
+#define TIM_DCR_DBL_2 ((uint32_t)0x00000400) /*!<Bit 2 */
+#define TIM_DCR_DBL_3 ((uint32_t)0x00000800) /*!<Bit 3 */
+#define TIM_DCR_DBL_4 ((uint32_t)0x00001000) /*!<Bit 4 */
+
+/******************* Bit definition for TIM_DMAR register ******************/
+#define TIM_DMAR_DMAB ((uint32_t)0x0000FFFF) /*!<DMA register for burst accesses */
+
+/******************* Bit definition for TIM_OR register ********************/
+
+/******************************************************************************/
+/* */
+/* Real-Time Clock */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for RTC_CRH register ********************/
+#define RTC_CRH_SECIE ((uint32_t)0x00000001) /*!< Second Interrupt Enable */
+#define RTC_CRH_ALRIE ((uint32_t)0x00000002) /*!< Alarm Interrupt Enable */
+#define RTC_CRH_OWIE ((uint32_t)0x00000004) /*!< OverfloW Interrupt Enable */
+
+/******************* Bit definition for RTC_CRL register ********************/
+#define RTC_CRL_SECF ((uint32_t)0x00000001) /*!< Second Flag */
+#define RTC_CRL_ALRF ((uint32_t)0x00000002) /*!< Alarm Flag */
+#define RTC_CRL_OWF ((uint32_t)0x00000004) /*!< OverfloW Flag */
+#define RTC_CRL_RSF ((uint32_t)0x00000008) /*!< Registers Synchronized Flag */
+#define RTC_CRL_CNF ((uint32_t)0x00000010) /*!< Configuration Flag */
+#define RTC_CRL_RTOFF ((uint32_t)0x00000020) /*!< RTC operation OFF */
+
+/******************* Bit definition for RTC_PRLH register *******************/
+#define RTC_PRLH_PRL ((uint32_t)0x0000000F) /*!< RTC Prescaler Reload Value High */
+
+/******************* Bit definition for RTC_PRLL register *******************/
+#define RTC_PRLL_PRL ((uint32_t)0x0000FFFF) /*!< RTC Prescaler Reload Value Low */
+
+/******************* Bit definition for RTC_DIVH register *******************/
+#define RTC_DIVH_RTC_DIV ((uint32_t)0x0000000F) /*!< RTC Clock Divider High */
+
+/******************* Bit definition for RTC_DIVL register *******************/
+#define RTC_DIVL_RTC_DIV ((uint32_t)0x0000FFFF) /*!< RTC Clock Divider Low */
+
+/******************* Bit definition for RTC_CNTH register *******************/
+#define RTC_CNTH_RTC_CNT ((uint32_t)0x0000FFFF) /*!< RTC Counter High */
+
+/******************* Bit definition for RTC_CNTL register *******************/
+#define RTC_CNTL_RTC_CNT ((uint32_t)0x0000FFFF) /*!< RTC Counter Low */
+
+/******************* Bit definition for RTC_ALRH register *******************/
+#define RTC_ALRH_RTC_ALR ((uint32_t)0x0000FFFF) /*!< RTC Alarm High */
+
+/******************* Bit definition for RTC_ALRL register *******************/
+#define RTC_ALRL_RTC_ALR ((uint32_t)0x0000FFFF) /*!< RTC Alarm Low */
+
+/******************************************************************************/
+/* */
+/* Independent WATCHDOG (IWDG) */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for IWDG_KR register ********************/
+#define IWDG_KR_KEY ((uint32_t)0x0000FFFF) /*!< Key value (write only, read 0000h) */
+
+/******************* Bit definition for IWDG_PR register ********************/
+#define IWDG_PR_PR ((uint32_t)0x00000007) /*!< PR[2:0] (Prescaler divider) */
+#define IWDG_PR_PR_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define IWDG_PR_PR_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define IWDG_PR_PR_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+
+/******************* Bit definition for IWDG_RLR register *******************/
+#define IWDG_RLR_RL ((uint32_t)0x00000FFF) /*!< Watchdog counter reload value */
+
+/******************* Bit definition for IWDG_SR register ********************/
+#define IWDG_SR_PVU ((uint32_t)0x00000001) /*!< Watchdog prescaler value update */
+#define IWDG_SR_RVU ((uint32_t)0x00000002) /*!< Watchdog counter reload value update */
+
+/******************************************************************************/
+/* */
+/* Window WATCHDOG */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for WWDG_CR register ********************/
+#define WWDG_CR_T ((uint32_t)0x0000007F) /*!< T[6:0] bits (7-Bit counter (MSB to LSB)) */
+#define WWDG_CR_T0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define WWDG_CR_T1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define WWDG_CR_T2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define WWDG_CR_T3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define WWDG_CR_T4 ((uint32_t)0x00000010) /*!< Bit 4 */
+#define WWDG_CR_T5 ((uint32_t)0x00000020) /*!< Bit 5 */
+#define WWDG_CR_T6 ((uint32_t)0x00000040) /*!< Bit 6 */
+
+#define WWDG_CR_WDGA ((uint32_t)0x00000080) /*!< Activation bit */
+
+/******************* Bit definition for WWDG_CFR register *******************/
+#define WWDG_CFR_W ((uint32_t)0x0000007F) /*!< W[6:0] bits (7-bit window value) */
+#define WWDG_CFR_W0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define WWDG_CFR_W1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define WWDG_CFR_W2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define WWDG_CFR_W3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define WWDG_CFR_W4 ((uint32_t)0x00000010) /*!< Bit 4 */
+#define WWDG_CFR_W5 ((uint32_t)0x00000020) /*!< Bit 5 */
+#define WWDG_CFR_W6 ((uint32_t)0x00000040) /*!< Bit 6 */
+
+#define WWDG_CFR_WDGTB ((uint32_t)0x00000180) /*!< WDGTB[1:0] bits (Timer Base) */
+#define WWDG_CFR_WDGTB0 ((uint32_t)0x00000080) /*!< Bit 0 */
+#define WWDG_CFR_WDGTB1 ((uint32_t)0x00000100) /*!< Bit 1 */
+
+#define WWDG_CFR_EWI ((uint32_t)0x00000200) /*!< Early Wakeup Interrupt */
+
+/******************* Bit definition for WWDG_SR register ********************/
+#define WWDG_SR_EWIF ((uint32_t)0x00000001) /*!< Early Wakeup Interrupt Flag */
+
+/******************************************************************************/
+/* */
+/* Flexible Static Memory Controller */
+/* */
+/******************************************************************************/
+
+/****************** Bit definition for FSMC_BCRx (x=1..4) register **********/
+#define FSMC_BCRx_MBKEN ((uint32_t)0x00000001) /*!< Memory bank enable bit */
+#define FSMC_BCRx_MUXEN ((uint32_t)0x00000002) /*!< Address/data multiplexing enable bit */
+
+#define FSMC_BCRx_MTYP ((uint32_t)0x0000000C) /*!< MTYP[1:0] bits (Memory type) */
+#define FSMC_BCRx_MTYP_0 ((uint32_t)0x00000004) /*!< Bit 0 */
+#define FSMC_BCRx_MTYP_1 ((uint32_t)0x00000008) /*!< Bit 1 */
+
+#define FSMC_BCRx_MWID ((uint32_t)0x00000030) /*!< MWID[1:0] bits (Memory data bus width) */
+#define FSMC_BCRx_MWID_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define FSMC_BCRx_MWID_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define FSMC_BCRx_FACCEN ((uint32_t)0x00000040) /*!< Flash access enable */
+#define FSMC_BCRx_BURSTEN ((uint32_t)0x00000100) /*!< Burst enable bit */
+#define FSMC_BCRx_WAITPOL ((uint32_t)0x00000200) /*!< Wait signal polarity bit */
+#define FSMC_BCRx_WRAPMOD ((uint32_t)0x00000400) /*!< Wrapped burst mode support */
+#define FSMC_BCRx_WAITCFG ((uint32_t)0x00000800) /*!< Wait timing configuration */
+#define FSMC_BCRx_WREN ((uint32_t)0x00001000) /*!< Write enable bit */
+#define FSMC_BCRx_WAITEN ((uint32_t)0x00002000) /*!< Wait enable bit */
+#define FSMC_BCRx_EXTMOD ((uint32_t)0x00004000) /*!< Extended mode enable */
+#define FSMC_BCRx_ASYNCWAIT ((uint32_t)0x00008000) /*!< Asynchronous wait */
+#define FSMC_BCRx_CBURSTRW ((uint32_t)0x00080000) /*!< Write burst enable */
+
+/****************** Bit definition for FSMC_BTRx (x=1..4) register ******/
+#define FSMC_BTRx_ADDSET ((uint32_t)0x0000000F) /*!< ADDSET[3:0] bits (Address setup phase duration) */
+#define FSMC_BTRx_ADDSET_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define FSMC_BTRx_ADDSET_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define FSMC_BTRx_ADDSET_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define FSMC_BTRx_ADDSET_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+
+#define FSMC_BTRx_ADDHLD ((uint32_t)0x000000F0) /*!< ADDHLD[3:0] bits (Address-hold phase duration) */
+#define FSMC_BTRx_ADDHLD_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define FSMC_BTRx_ADDHLD_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+#define FSMC_BTRx_ADDHLD_2 ((uint32_t)0x00000040) /*!< Bit 2 */
+#define FSMC_BTRx_ADDHLD_3 ((uint32_t)0x00000080) /*!< Bit 3 */
+
+#define FSMC_BTRx_DATAST ((uint32_t)0x0000FF00) /*!< DATAST [3:0] bits (Data-phase duration) */
+#define FSMC_BTRx_DATAST_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define FSMC_BTRx_DATAST_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+#define FSMC_BTRx_DATAST_2 ((uint32_t)0x00000400) /*!< Bit 2 */
+#define FSMC_BTRx_DATAST_3 ((uint32_t)0x00000800) /*!< Bit 3 */
+#define FSMC_BTRx_DATAST_4 ((uint32_t)0x00001000) /*!< Bit 4 */
+#define FSMC_BTRx_DATAST_5 ((uint32_t)0x00002000) /*!< Bit 5 */
+#define FSMC_BTRx_DATAST_6 ((uint32_t)0x00004000) /*!< Bit 6 */
+#define FSMC_BTRx_DATAST_7 ((uint32_t)0x00008000) /*!< Bit 7 */
+
+#define FSMC_BTRx_BUSTURN ((uint32_t)0x000F0000) /*!< BUSTURN[3:0] bits (Bus turnaround phase duration) */
+#define FSMC_BTRx_BUSTURN_0 ((uint32_t)0x00010000) /*!< Bit 0 */
+#define FSMC_BTRx_BUSTURN_1 ((uint32_t)0x00020000) /*!< Bit 1 */
+#define FSMC_BTRx_BUSTURN_2 ((uint32_t)0x00040000) /*!< Bit 2 */
+#define FSMC_BTRx_BUSTURN_3 ((uint32_t)0x00080000) /*!< Bit 3 */
+
+#define FSMC_BTRx_CLKDIV ((uint32_t)0x00F00000) /*!< CLKDIV[3:0] bits (Clock divide ratio) */
+#define FSMC_BTRx_CLKDIV_0 ((uint32_t)0x00100000) /*!< Bit 0 */
+#define FSMC_BTRx_CLKDIV_1 ((uint32_t)0x00200000) /*!< Bit 1 */
+#define FSMC_BTRx_CLKDIV_2 ((uint32_t)0x00400000) /*!< Bit 2 */
+#define FSMC_BTRx_CLKDIV_3 ((uint32_t)0x00800000) /*!< Bit 3 */
+
+#define FSMC_BTRx_DATLAT ((uint32_t)0x0F000000) /*!< DATLA[3:0] bits (Data latency) */
+#define FSMC_BTRx_DATLAT_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+#define FSMC_BTRx_DATLAT_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+#define FSMC_BTRx_DATLAT_2 ((uint32_t)0x04000000) /*!< Bit 2 */
+#define FSMC_BTRx_DATLAT_3 ((uint32_t)0x08000000) /*!< Bit 3 */
+
+#define FSMC_BTRx_ACCMOD ((uint32_t)0x30000000) /*!< ACCMOD[1:0] bits (Access mode) */
+#define FSMC_BTRx_ACCMOD_0 ((uint32_t)0x10000000) /*!< Bit 0 */
+#define FSMC_BTRx_ACCMOD_1 ((uint32_t)0x20000000) /*!< Bit 1 */
+
+/****************** Bit definition for FSMC_BWTRx (x=1..4) register ******/
+#define FSMC_BWTRx_ADDSET ((uint32_t)0x0000000F) /*!< ADDSET[3:0] bits (Address setup phase duration) */
+#define FSMC_BWTRx_ADDSET_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define FSMC_BWTRx_ADDSET_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define FSMC_BWTRx_ADDSET_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define FSMC_BWTRx_ADDSET_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+
+#define FSMC_BWTRx_ADDHLD ((uint32_t)0x000000F0) /*!< ADDHLD[3:0] bits (Address-hold phase duration) */
+#define FSMC_BWTRx_ADDHLD_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define FSMC_BWTRx_ADDHLD_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+#define FSMC_BWTRx_ADDHLD_2 ((uint32_t)0x00000040) /*!< Bit 2 */
+#define FSMC_BWTRx_ADDHLD_3 ((uint32_t)0x00000080) /*!< Bit 3 */
+
+#define FSMC_BWTRx_DATAST ((uint32_t)0x0000FF00) /*!< DATAST [3:0] bits (Data-phase duration) */
+#define FSMC_BWTRx_DATAST_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define FSMC_BWTRx_DATAST_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+#define FSMC_BWTRx_DATAST_2 ((uint32_t)0x00000400) /*!< Bit 2 */
+#define FSMC_BWTRx_DATAST_3 ((uint32_t)0x00000800) /*!< Bit 3 */
+#define FSMC_BWTRx_DATAST_4 ((uint32_t)0x00001000) /*!< Bit 4 */
+#define FSMC_BWTRx_DATAST_5 ((uint32_t)0x00002000) /*!< Bit 5 */
+#define FSMC_BWTRx_DATAST_6 ((uint32_t)0x00004000) /*!< Bit 6 */
+#define FSMC_BWTRx_DATAST_7 ((uint32_t)0x00008000) /*!< Bit 7 */
+
+#define FSMC_BWTRx_BUSTURN ((uint32_t)0x000F0000) /*!< BUSTURN[3:0] bits (Bus turnaround phase duration) */
+#define FSMC_BWTRx_BUSTURN_0 ((uint32_t)0x00010000) /*!< Bit 0 */
+#define FSMC_BWTRx_BUSTURN_1 ((uint32_t)0x00020000) /*!< Bit 1 */
+#define FSMC_BWTRx_BUSTURN_2 ((uint32_t)0x00040000) /*!< Bit 2 */
+#define FSMC_BWTRx_BUSTURN_3 ((uint32_t)0x00080000) /*!< Bit 3 */
+
+#define FSMC_BWTRx_ACCMOD ((uint32_t)0x30000000) /*!< ACCMOD[1:0] bits (Access mode) */
+#define FSMC_BWTRx_ACCMOD_0 ((uint32_t)0x10000000) /*!< Bit 0 */
+#define FSMC_BWTRx_ACCMOD_1 ((uint32_t)0x20000000) /*!< Bit 1 */
+
+/****************** Bit definition for FSMC_PCRx (x = 2 to 4) register *******************/
+#define FSMC_PCRx_PWAITEN ((uint32_t)0x00000002) /*!< Wait feature enable bit */
+#define FSMC_PCRx_PBKEN ((uint32_t)0x00000004) /*!< PC Card/NAND Flash memory bank enable bit */
+#define FSMC_PCRx_PTYP ((uint32_t)0x00000008) /*!< Memory type */
+
+#define FSMC_PCRx_PWID ((uint32_t)0x00000030) /*!< PWID[1:0] bits (NAND Flash databus width) */
+#define FSMC_PCRx_PWID_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define FSMC_PCRx_PWID_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define FSMC_PCRx_ECCEN ((uint32_t)0x00000040) /*!< ECC computation logic enable bit */
+
+#define FSMC_PCRx_TCLR ((uint32_t)0x00001E00) /*!< TCLR[3:0] bits (CLE to RE delay) */
+#define FSMC_PCRx_TCLR_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define FSMC_PCRx_TCLR_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+#define FSMC_PCRx_TCLR_2 ((uint32_t)0x00000800) /*!< Bit 2 */
+#define FSMC_PCRx_TCLR_3 ((uint32_t)0x00001000) /*!< Bit 3 */
+
+#define FSMC_PCRx_TAR ((uint32_t)0x0001E000) /*!< TAR[3:0] bits (ALE to RE delay) */
+#define FSMC_PCRx_TAR_0 ((uint32_t)0x00002000) /*!< Bit 0 */
+#define FSMC_PCRx_TAR_1 ((uint32_t)0x00004000) /*!< Bit 1 */
+#define FSMC_PCRx_TAR_2 ((uint32_t)0x00008000) /*!< Bit 2 */
+#define FSMC_PCRx_TAR_3 ((uint32_t)0x00010000) /*!< Bit 3 */
+
+#define FSMC_PCRx_ECCPS ((uint32_t)0x000E0000) /*!< ECCPS[1:0] bits (ECC page size) */
+#define FSMC_PCRx_ECCPS_0 ((uint32_t)0x00020000) /*!< Bit 0 */
+#define FSMC_PCRx_ECCPS_1 ((uint32_t)0x00040000) /*!< Bit 1 */
+#define FSMC_PCRx_ECCPS_2 ((uint32_t)0x00080000) /*!< Bit 2 */
+
+/******************* Bit definition for FSMC_SRx (x = 2 to 4) register *******************/
+#define FSMC_SRx_IRS ((uint32_t)0x00000001) /*!< Interrupt Rising Edge status */
+#define FSMC_SRx_ILS ((uint32_t)0x00000002) /*!< Interrupt Level status */
+#define FSMC_SRx_IFS ((uint32_t)0x00000004) /*!< Interrupt Falling Edge status */
+#define FSMC_SRx_IREN ((uint32_t)0x00000008) /*!< Interrupt Rising Edge detection Enable bit */
+#define FSMC_SRx_ILEN ((uint32_t)0x00000010) /*!< Interrupt Level detection Enable bit */
+#define FSMC_SRx_IFEN ((uint32_t)0x00000020) /*!< Interrupt Falling Edge detection Enable bit */
+#define FSMC_SRx_FEMPT ((uint32_t)0x00000040) /*!< FIFO empty */
+
+/****************** Bit definition for FSMC_PMEMx (x = 2 to 4) register ******************/
+#define FSMC_PMEMx_MEMSETx ((uint32_t)0x000000FF) /*!< MEMSETx[7:0] bits (Common memory x setup time) */
+#define FSMC_PMEMx_MEMSETx_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define FSMC_PMEMx_MEMSETx_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define FSMC_PMEMx_MEMSETx_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define FSMC_PMEMx_MEMSETx_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define FSMC_PMEMx_MEMSETx_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+#define FSMC_PMEMx_MEMSETx_5 ((uint32_t)0x00000020) /*!< Bit 5 */
+#define FSMC_PMEMx_MEMSETx_6 ((uint32_t)0x00000040) /*!< Bit 6 */
+#define FSMC_PMEMx_MEMSETx_7 ((uint32_t)0x00000080) /*!< Bit 7 */
+
+#define FSMC_PMEMx_MEMWAITx ((uint32_t)0x0000FF00) /*!< MEMWAITx[7:0] bits (Common memory x wait time) */
+#define FSMC_PMEMx_MEMWAIT2_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define FSMC_PMEMx_MEMWAITx_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+#define FSMC_PMEMx_MEMWAITx_2 ((uint32_t)0x00000400) /*!< Bit 2 */
+#define FSMC_PMEMx_MEMWAITx_3 ((uint32_t)0x00000800) /*!< Bit 3 */
+#define FSMC_PMEMx_MEMWAITx_4 ((uint32_t)0x00001000) /*!< Bit 4 */
+#define FSMC_PMEMx_MEMWAITx_5 ((uint32_t)0x00002000) /*!< Bit 5 */
+#define FSMC_PMEMx_MEMWAITx_6 ((uint32_t)0x00004000) /*!< Bit 6 */
+#define FSMC_PMEMx_MEMWAITx_7 ((uint32_t)0x00008000) /*!< Bit 7 */
+
+#define FSMC_PMEMx_MEMHOLDx ((uint32_t)0x00FF0000) /*!< MEMHOLDx[7:0] bits (Common memory x hold time) */
+#define FSMC_PMEMx_MEMHOLDx_0 ((uint32_t)0x00010000) /*!< Bit 0 */
+#define FSMC_PMEMx_MEMHOLDx_1 ((uint32_t)0x00020000) /*!< Bit 1 */
+#define FSMC_PMEMx_MEMHOLDx_2 ((uint32_t)0x00040000) /*!< Bit 2 */
+#define FSMC_PMEMx_MEMHOLDx_3 ((uint32_t)0x00080000) /*!< Bit 3 */
+#define FSMC_PMEMx_MEMHOLDx_4 ((uint32_t)0x00100000) /*!< Bit 4 */
+#define FSMC_PMEMx_MEMHOLDx_5 ((uint32_t)0x00200000) /*!< Bit 5 */
+#define FSMC_PMEMx_MEMHOLDx_6 ((uint32_t)0x00400000) /*!< Bit 6 */
+#define FSMC_PMEMx_MEMHOLDx_7 ((uint32_t)0x00800000) /*!< Bit 7 */
+
+#define FSMC_PMEMx_MEMHIZx ((uint32_t)0xFF000000) /*!< MEMHIZx[7:0] bits (Common memory x databus HiZ time) */
+#define FSMC_PMEMx_MEMHIZx_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+#define FSMC_PMEMx_MEMHIZx_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+#define FSMC_PMEMx_MEMHIZx_2 ((uint32_t)0x04000000) /*!< Bit 2 */
+#define FSMC_PMEMx_MEMHIZx_3 ((uint32_t)0x08000000) /*!< Bit 3 */
+#define FSMC_PMEMx_MEMHIZx_4 ((uint32_t)0x10000000) /*!< Bit 4 */
+#define FSMC_PMEMx_MEMHIZx_5 ((uint32_t)0x20000000) /*!< Bit 5 */
+#define FSMC_PMEMx_MEMHIZx_6 ((uint32_t)0x40000000) /*!< Bit 6 */
+#define FSMC_PMEMx_MEMHIZx_7 ((uint32_t)0x80000000) /*!< Bit 7 */
+
+/****************** Bit definition for FSMC_PATTx (x = 2 to 4) register ******************/
+#define FSMC_PATTx_ATTSETx ((uint32_t)0x000000FF) /*!< ATTSETx[7:0] bits (Attribute memory x setup time) */
+#define FSMC_PATTx_ATTSETx_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define FSMC_PATTx_ATTSETx_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define FSMC_PATTx_ATTSETx_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define FSMC_PATTx_ATTSETx_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define FSMC_PATTx_ATTSETx_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+#define FSMC_PATTx_ATTSETx_5 ((uint32_t)0x00000020) /*!< Bit 5 */
+#define FSMC_PATTx_ATTSETx_6 ((uint32_t)0x00000040) /*!< Bit 6 */
+#define FSMC_PATTx_ATTSETx_7 ((uint32_t)0x00000080) /*!< Bit 7 */
+
+#define FSMC_PATTx_ATTWAITx ((uint32_t)0x0000FF00) /*!< ATTWAITx[7:0] bits (Attribute memory x wait time) */
+#define FSMC_PATTx_ATTWAITx_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define FSMC_PATTx_ATTWAITx_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+#define FSMC_PATTx_ATTWAITx_2 ((uint32_t)0x00000400) /*!< Bit 2 */
+#define FSMC_PATTx_ATTWAITx_3 ((uint32_t)0x00000800) /*!< Bit 3 */
+#define FSMC_PATTx_ATTWAITx_4 ((uint32_t)0x00001000) /*!< Bit 4 */
+#define FSMC_PATTx_ATTWAITx_5 ((uint32_t)0x00002000) /*!< Bit 5 */
+#define FSMC_PATTx_ATTWAITx_6 ((uint32_t)0x00004000) /*!< Bit 6 */
+#define FSMC_PATTx_ATTWAITx_7 ((uint32_t)0x00008000) /*!< Bit 7 */
+
+#define FSMC_PATTx_ATTHOLDx ((uint32_t)0x00FF0000) /*!< ATTHOLDx[7:0] bits (Attribute memory x hold time) */
+#define FSMC_PATTx_ATTHOLDx_0 ((uint32_t)0x00010000) /*!< Bit 0 */
+#define FSMC_PATTx_ATTHOLDx_1 ((uint32_t)0x00020000) /*!< Bit 1 */
+#define FSMC_PATTx_ATTHOLDx_2 ((uint32_t)0x00040000) /*!< Bit 2 */
+#define FSMC_PATTx_ATTHOLDx_3 ((uint32_t)0x00080000) /*!< Bit 3 */
+#define FSMC_PATTx_ATTHOLDx_4 ((uint32_t)0x00100000) /*!< Bit 4 */
+#define FSMC_PATTx_ATTHOLDx_5 ((uint32_t)0x00200000) /*!< Bit 5 */
+#define FSMC_PATTx_ATTHOLDx_6 ((uint32_t)0x00400000) /*!< Bit 6 */
+#define FSMC_PATTx_ATTHOLDx_7 ((uint32_t)0x00800000) /*!< Bit 7 */
+
+#define FSMC_PATTx_ATTHIZx ((uint32_t)0xFF000000) /*!< ATTHIZx[7:0] bits (Attribute memory x databus HiZ time) */
+#define FSMC_PATTx_ATTHIZx_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+#define FSMC_PATTx_ATTHIZx_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+#define FSMC_PATTx_ATTHIZx_2 ((uint32_t)0x04000000) /*!< Bit 2 */
+#define FSMC_PATTx_ATTHIZx_3 ((uint32_t)0x08000000) /*!< Bit 3 */
+#define FSMC_PATTx_ATTHIZx_4 ((uint32_t)0x10000000) /*!< Bit 4 */
+#define FSMC_PATTx_ATTHIZx_5 ((uint32_t)0x20000000) /*!< Bit 5 */
+#define FSMC_PATTx_ATTHIZx_6 ((uint32_t)0x40000000) /*!< Bit 6 */
+#define FSMC_PATTx_ATTHIZx_7 ((uint32_t)0x80000000) /*!< Bit 7 */
+
+/****************** Bit definition for FSMC_PIO4 register *******************/
+#define FSMC_PIO4_IOSET4 ((uint32_t)0x000000FF) /*!< IOSET4[7:0] bits (I/O 4 setup time) */
+#define FSMC_PIO4_IOSET4_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define FSMC_PIO4_IOSET4_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define FSMC_PIO4_IOSET4_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define FSMC_PIO4_IOSET4_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define FSMC_PIO4_IOSET4_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+#define FSMC_PIO4_IOSET4_5 ((uint32_t)0x00000020) /*!< Bit 5 */
+#define FSMC_PIO4_IOSET4_6 ((uint32_t)0x00000040) /*!< Bit 6 */
+#define FSMC_PIO4_IOSET4_7 ((uint32_t)0x00000080) /*!< Bit 7 */
+
+#define FSMC_PIO4_IOWAIT4 ((uint32_t)0x0000FF00) /*!< IOWAIT4[7:0] bits (I/O 4 wait time) */
+#define FSMC_PIO4_IOWAIT4_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define FSMC_PIO4_IOWAIT4_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+#define FSMC_PIO4_IOWAIT4_2 ((uint32_t)0x00000400) /*!< Bit 2 */
+#define FSMC_PIO4_IOWAIT4_3 ((uint32_t)0x00000800) /*!< Bit 3 */
+#define FSMC_PIO4_IOWAIT4_4 ((uint32_t)0x00001000) /*!< Bit 4 */
+#define FSMC_PIO4_IOWAIT4_5 ((uint32_t)0x00002000) /*!< Bit 5 */
+#define FSMC_PIO4_IOWAIT4_6 ((uint32_t)0x00004000) /*!< Bit 6 */
+#define FSMC_PIO4_IOWAIT4_7 ((uint32_t)0x00008000) /*!< Bit 7 */
+
+#define FSMC_PIO4_IOHOLD4 ((uint32_t)0x00FF0000) /*!< IOHOLD4[7:0] bits (I/O 4 hold time) */
+#define FSMC_PIO4_IOHOLD4_0 ((uint32_t)0x00010000) /*!< Bit 0 */
+#define FSMC_PIO4_IOHOLD4_1 ((uint32_t)0x00020000) /*!< Bit 1 */
+#define FSMC_PIO4_IOHOLD4_2 ((uint32_t)0x00040000) /*!< Bit 2 */
+#define FSMC_PIO4_IOHOLD4_3 ((uint32_t)0x00080000) /*!< Bit 3 */
+#define FSMC_PIO4_IOHOLD4_4 ((uint32_t)0x00100000) /*!< Bit 4 */
+#define FSMC_PIO4_IOHOLD4_5 ((uint32_t)0x00200000) /*!< Bit 5 */
+#define FSMC_PIO4_IOHOLD4_6 ((uint32_t)0x00400000) /*!< Bit 6 */
+#define FSMC_PIO4_IOHOLD4_7 ((uint32_t)0x00800000) /*!< Bit 7 */
+
+#define FSMC_PIO4_IOHIZ4 ((uint32_t)0xFF000000) /*!< IOHIZ4[7:0] bits (I/O 4 databus HiZ time) */
+#define FSMC_PIO4_IOHIZ4_0 ((uint32_t)0x01000000) /*!< Bit 0 */
+#define FSMC_PIO4_IOHIZ4_1 ((uint32_t)0x02000000) /*!< Bit 1 */
+#define FSMC_PIO4_IOHIZ4_2 ((uint32_t)0x04000000) /*!< Bit 2 */
+#define FSMC_PIO4_IOHIZ4_3 ((uint32_t)0x08000000) /*!< Bit 3 */
+#define FSMC_PIO4_IOHIZ4_4 ((uint32_t)0x10000000) /*!< Bit 4 */
+#define FSMC_PIO4_IOHIZ4_5 ((uint32_t)0x20000000) /*!< Bit 5 */
+#define FSMC_PIO4_IOHIZ4_6 ((uint32_t)0x40000000) /*!< Bit 6 */
+#define FSMC_PIO4_IOHIZ4_7 ((uint32_t)0x80000000) /*!< Bit 7 */
+
+/****************** Bit definition for FSMC_ECCR2 register ******************/
+#define FSMC_ECCR2_ECC2 ((uint32_t)0xFFFFFFFF) /*!< ECC result */
+
+/****************** Bit definition for FSMC_ECCR3 register ******************/
+#define FSMC_ECCR3_ECC3 ((uint32_t)0xFFFFFFFF) /*!< ECC result */
+
+/******************************************************************************/
+/* */
+/* SD host Interface */
+/* */
+/******************************************************************************/
+
+/****************** Bit definition for SDIO_POWER register ******************/
+#define SDIO_POWER_PWRCTRL ((uint32_t)0x03) /*!< PWRCTRL[1:0] bits (Power supply control bits) */
+#define SDIO_POWER_PWRCTRL_0 ((uint32_t)0x01) /*!< Bit 0 */
+#define SDIO_POWER_PWRCTRL_1 ((uint32_t)0x02) /*!< Bit 1 */
+
+/****************** Bit definition for SDIO_CLKCR register ******************/
+#define SDIO_CLKCR_CLKDIV ((uint32_t)0x00FF) /*!< Clock divide factor */
+#define SDIO_CLKCR_CLKEN ((uint32_t)0x0100) /*!< Clock enable bit */
+#define SDIO_CLKCR_PWRSAV ((uint32_t)0x0200) /*!< Power saving configuration bit */
+#define SDIO_CLKCR_BYPASS ((uint32_t)0x0400) /*!< Clock divider bypass enable bit */
+
+#define SDIO_CLKCR_WIDBUS ((uint32_t)0x1800) /*!< WIDBUS[1:0] bits (Wide bus mode enable bit) */
+#define SDIO_CLKCR_WIDBUS_0 ((uint32_t)0x0800) /*!< Bit 0 */
+#define SDIO_CLKCR_WIDBUS_1 ((uint32_t)0x1000) /*!< Bit 1 */
+
+#define SDIO_CLKCR_NEGEDGE ((uint32_t)0x2000) /*!< SDIO_CK dephasing selection bit */
+#define SDIO_CLKCR_HWFC_EN ((uint32_t)0x4000) /*!< HW Flow Control enable */
+
+/******************* Bit definition for SDIO_ARG register *******************/
+#define SDIO_ARG_CMDARG ((uint32_t)0xFFFFFFFF) /*!< Command argument */
+
+/******************* Bit definition for SDIO_CMD register *******************/
+#define SDIO_CMD_CMDINDEX ((uint32_t)0x003F) /*!< Command Index */
+
+#define SDIO_CMD_WAITRESP ((uint32_t)0x00C0) /*!< WAITRESP[1:0] bits (Wait for response bits) */
+#define SDIO_CMD_WAITRESP_0 ((uint32_t)0x0040) /*!< Bit 0 */
+#define SDIO_CMD_WAITRESP_1 ((uint32_t)0x0080) /*!< Bit 1 */
+
+#define SDIO_CMD_WAITINT ((uint32_t)0x0100) /*!< CPSM Waits for Interrupt Request */
+#define SDIO_CMD_WAITPEND ((uint32_t)0x0200) /*!< CPSM Waits for ends of data transfer (CmdPend internal signal) */
+#define SDIO_CMD_CPSMEN ((uint32_t)0x0400) /*!< Command path state machine (CPSM) Enable bit */
+#define SDIO_CMD_SDIOSUSPEND ((uint32_t)0x0800) /*!< SD I/O suspend command */
+#define SDIO_CMD_ENCMDCOMPL ((uint32_t)0x1000) /*!< Enable CMD completion */
+#define SDIO_CMD_NIEN ((uint32_t)0x2000) /*!< Not Interrupt Enable */
+#define SDIO_CMD_CEATACMD ((uint32_t)0x4000) /*!< CE-ATA command */
+
+/***************** Bit definition for SDIO_RESPCMD register *****************/
+#define SDIO_RESPCMD_RESPCMD ((uint32_t)0x3F) /*!< Response command index */
+
+/****************** Bit definition for SDIO_RESP0 register ******************/
+#define SDIO_RESP0_CARDSTATUS0 ((uint32_t)0xFFFFFFFF) /*!< Card Status */
+
+/****************** Bit definition for SDIO_RESP1 register ******************/
+#define SDIO_RESP1_CARDSTATUS1 ((uint32_t)0xFFFFFFFF) /*!< Card Status */
+
+/****************** Bit definition for SDIO_RESP2 register ******************/
+#define SDIO_RESP2_CARDSTATUS2 ((uint32_t)0xFFFFFFFF) /*!< Card Status */
+
+/****************** Bit definition for SDIO_RESP3 register ******************/
+#define SDIO_RESP3_CARDSTATUS3 ((uint32_t)0xFFFFFFFF) /*!< Card Status */
+
+/****************** Bit definition for SDIO_RESP4 register ******************/
+#define SDIO_RESP4_CARDSTATUS4 ((uint32_t)0xFFFFFFFF) /*!< Card Status */
+
+/****************** Bit definition for SDIO_DTIMER register *****************/
+#define SDIO_DTIMER_DATATIME ((uint32_t)0xFFFFFFFF) /*!< Data timeout period. */
+
+/****************** Bit definition for SDIO_DLEN register *******************/
+#define SDIO_DLEN_DATALENGTH ((uint32_t)0x01FFFFFF) /*!< Data length value */
+
+/****************** Bit definition for SDIO_DCTRL register ******************/
+#define SDIO_DCTRL_DTEN ((uint32_t)0x0001) /*!< Data transfer enabled bit */
+#define SDIO_DCTRL_DTDIR ((uint32_t)0x0002) /*!< Data transfer direction selection */
+#define SDIO_DCTRL_DTMODE ((uint32_t)0x0004) /*!< Data transfer mode selection */
+#define SDIO_DCTRL_DMAEN ((uint32_t)0x0008) /*!< DMA enabled bit */
+
+#define SDIO_DCTRL_DBLOCKSIZE ((uint32_t)0x00F0) /*!< DBLOCKSIZE[3:0] bits (Data block size) */
+#define SDIO_DCTRL_DBLOCKSIZE_0 ((uint32_t)0x0010) /*!< Bit 0 */
+#define SDIO_DCTRL_DBLOCKSIZE_1 ((uint32_t)0x0020) /*!< Bit 1 */
+#define SDIO_DCTRL_DBLOCKSIZE_2 ((uint32_t)0x0040) /*!< Bit 2 */
+#define SDIO_DCTRL_DBLOCKSIZE_3 ((uint32_t)0x0080) /*!< Bit 3 */
+
+#define SDIO_DCTRL_RWSTART ((uint32_t)0x0100) /*!< Read wait start */
+#define SDIO_DCTRL_RWSTOP ((uint32_t)0x0200) /*!< Read wait stop */
+#define SDIO_DCTRL_RWMOD ((uint32_t)0x0400) /*!< Read wait mode */
+#define SDIO_DCTRL_SDIOEN ((uint32_t)0x0800) /*!< SD I/O enable functions */
+
+/****************** Bit definition for SDIO_DCOUNT register *****************/
+#define SDIO_DCOUNT_DATACOUNT ((uint32_t)0x01FFFFFF) /*!< Data count value */
+
+/****************** Bit definition for SDIO_STA register ********************/
+#define SDIO_STA_CCRCFAIL ((uint32_t)0x00000001) /*!< Command response received (CRC check failed) */
+#define SDIO_STA_DCRCFAIL ((uint32_t)0x00000002) /*!< Data block sent/received (CRC check failed) */
+#define SDIO_STA_CTIMEOUT ((uint32_t)0x00000004) /*!< Command response timeout */
+#define SDIO_STA_DTIMEOUT ((uint32_t)0x00000008) /*!< Data timeout */
+#define SDIO_STA_TXUNDERR ((uint32_t)0x00000010) /*!< Transmit FIFO underrun error */
+#define SDIO_STA_RXOVERR ((uint32_t)0x00000020) /*!< Received FIFO overrun error */
+#define SDIO_STA_CMDREND ((uint32_t)0x00000040) /*!< Command response received (CRC check passed) */
+#define SDIO_STA_CMDSENT ((uint32_t)0x00000080) /*!< Command sent (no response required) */
+#define SDIO_STA_DATAEND ((uint32_t)0x00000100) /*!< Data end (data counter, SDIDCOUNT, is zero) */
+#define SDIO_STA_STBITERR ((uint32_t)0x00000200) /*!< Start bit not detected on all data signals in wide bus mode */
+#define SDIO_STA_DBCKEND ((uint32_t)0x00000400) /*!< Data block sent/received (CRC check passed) */
+#define SDIO_STA_CMDACT ((uint32_t)0x00000800) /*!< Command transfer in progress */
+#define SDIO_STA_TXACT ((uint32_t)0x00001000) /*!< Data transmit in progress */
+#define SDIO_STA_RXACT ((uint32_t)0x00002000) /*!< Data receive in progress */
+#define SDIO_STA_TXFIFOHE ((uint32_t)0x00004000) /*!< Transmit FIFO Half Empty: at least 8 words can be written into the FIFO */
+#define SDIO_STA_RXFIFOHF ((uint32_t)0x00008000) /*!< Receive FIFO Half Full: there are at least 8 words in the FIFO */
+#define SDIO_STA_TXFIFOF ((uint32_t)0x00010000) /*!< Transmit FIFO full */
+#define SDIO_STA_RXFIFOF ((uint32_t)0x00020000) /*!< Receive FIFO full */
+#define SDIO_STA_TXFIFOE ((uint32_t)0x00040000) /*!< Transmit FIFO empty */
+#define SDIO_STA_RXFIFOE ((uint32_t)0x00080000) /*!< Receive FIFO empty */
+#define SDIO_STA_TXDAVL ((uint32_t)0x00100000) /*!< Data available in transmit FIFO */
+#define SDIO_STA_RXDAVL ((uint32_t)0x00200000) /*!< Data available in receive FIFO */
+#define SDIO_STA_SDIOIT ((uint32_t)0x00400000) /*!< SDIO interrupt received */
+#define SDIO_STA_CEATAEND ((uint32_t)0x00800000) /*!< CE-ATA command completion signal received for CMD61 */
+
+/******************* Bit definition for SDIO_ICR register *******************/
+#define SDIO_ICR_CCRCFAILC ((uint32_t)0x00000001) /*!< CCRCFAIL flag clear bit */
+#define SDIO_ICR_DCRCFAILC ((uint32_t)0x00000002) /*!< DCRCFAIL flag clear bit */
+#define SDIO_ICR_CTIMEOUTC ((uint32_t)0x00000004) /*!< CTIMEOUT flag clear bit */
+#define SDIO_ICR_DTIMEOUTC ((uint32_t)0x00000008) /*!< DTIMEOUT flag clear bit */
+#define SDIO_ICR_TXUNDERRC ((uint32_t)0x00000010) /*!< TXUNDERR flag clear bit */
+#define SDIO_ICR_RXOVERRC ((uint32_t)0x00000020) /*!< RXOVERR flag clear bit */
+#define SDIO_ICR_CMDRENDC ((uint32_t)0x00000040) /*!< CMDREND flag clear bit */
+#define SDIO_ICR_CMDSENTC ((uint32_t)0x00000080) /*!< CMDSENT flag clear bit */
+#define SDIO_ICR_DATAENDC ((uint32_t)0x00000100) /*!< DATAEND flag clear bit */
+#define SDIO_ICR_STBITERRC ((uint32_t)0x00000200) /*!< STBITERR flag clear bit */
+#define SDIO_ICR_DBCKENDC ((uint32_t)0x00000400) /*!< DBCKEND flag clear bit */
+#define SDIO_ICR_SDIOITC ((uint32_t)0x00400000) /*!< SDIOIT flag clear bit */
+#define SDIO_ICR_CEATAENDC ((uint32_t)0x00800000) /*!< CEATAEND flag clear bit */
+
+/****************** Bit definition for SDIO_MASK register *******************/
+#define SDIO_MASK_CCRCFAILIE ((uint32_t)0x00000001) /*!< Command CRC Fail Interrupt Enable */
+#define SDIO_MASK_DCRCFAILIE ((uint32_t)0x00000002) /*!< Data CRC Fail Interrupt Enable */
+#define SDIO_MASK_CTIMEOUTIE ((uint32_t)0x00000004) /*!< Command TimeOut Interrupt Enable */
+#define SDIO_MASK_DTIMEOUTIE ((uint32_t)0x00000008) /*!< Data TimeOut Interrupt Enable */
+#define SDIO_MASK_TXUNDERRIE ((uint32_t)0x00000010) /*!< Tx FIFO UnderRun Error Interrupt Enable */
+#define SDIO_MASK_RXOVERRIE ((uint32_t)0x00000020) /*!< Rx FIFO OverRun Error Interrupt Enable */
+#define SDIO_MASK_CMDRENDIE ((uint32_t)0x00000040) /*!< Command Response Received Interrupt Enable */
+#define SDIO_MASK_CMDSENTIE ((uint32_t)0x00000080) /*!< Command Sent Interrupt Enable */
+#define SDIO_MASK_DATAENDIE ((uint32_t)0x00000100) /*!< Data End Interrupt Enable */
+#define SDIO_MASK_STBITERRIE ((uint32_t)0x00000200) /*!< Start Bit Error Interrupt Enable */
+#define SDIO_MASK_DBCKENDIE ((uint32_t)0x00000400) /*!< Data Block End Interrupt Enable */
+#define SDIO_MASK_CMDACTIE ((uint32_t)0x00000800) /*!< Command Acting Interrupt Enable */
+#define SDIO_MASK_TXACTIE ((uint32_t)0x00001000) /*!< Data Transmit Acting Interrupt Enable */
+#define SDIO_MASK_RXACTIE ((uint32_t)0x00002000) /*!< Data receive acting interrupt enabled */
+#define SDIO_MASK_TXFIFOHEIE ((uint32_t)0x00004000) /*!< Tx FIFO Half Empty interrupt Enable */
+#define SDIO_MASK_RXFIFOHFIE ((uint32_t)0x00008000) /*!< Rx FIFO Half Full interrupt Enable */
+#define SDIO_MASK_TXFIFOFIE ((uint32_t)0x00010000) /*!< Tx FIFO Full interrupt Enable */
+#define SDIO_MASK_RXFIFOFIE ((uint32_t)0x00020000) /*!< Rx FIFO Full interrupt Enable */
+#define SDIO_MASK_TXFIFOEIE ((uint32_t)0x00040000) /*!< Tx FIFO Empty interrupt Enable */
+#define SDIO_MASK_RXFIFOEIE ((uint32_t)0x00080000) /*!< Rx FIFO Empty interrupt Enable */
+#define SDIO_MASK_TXDAVLIE ((uint32_t)0x00100000) /*!< Data available in Tx FIFO interrupt Enable */
+#define SDIO_MASK_RXDAVLIE ((uint32_t)0x00200000) /*!< Data available in Rx FIFO interrupt Enable */
+#define SDIO_MASK_SDIOITIE ((uint32_t)0x00400000) /*!< SDIO Mode Interrupt Received interrupt Enable */
+#define SDIO_MASK_CEATAENDIE ((uint32_t)0x00800000) /*!< CE-ATA command completion signal received Interrupt Enable */
+
+/***************** Bit definition for SDIO_FIFOCNT register *****************/
+#define SDIO_FIFOCNT_FIFOCOUNT ((uint32_t)0x00FFFFFF) /*!< Remaining number of words to be written to or read from the FIFO */
+
+/****************** Bit definition for SDIO_FIFO register *******************/
+#define SDIO_FIFO_FIFODATA ((uint32_t)0xFFFFFFFF) /*!< Receive and transmit FIFO data */
+
+/******************************************************************************/
+/* */
+/* USB Device FS */
+/* */
+/******************************************************************************/
+
+/*!< Endpoint-specific registers */
+#define USB_EP0R USB_BASE /*!< Endpoint 0 register address */
+#define USB_EP1R (USB_BASE + 0x00000004) /*!< Endpoint 1 register address */
+#define USB_EP2R (USB_BASE + 0x00000008) /*!< Endpoint 2 register address */
+#define USB_EP3R (USB_BASE + 0x0000000C) /*!< Endpoint 3 register address */
+#define USB_EP4R (USB_BASE + 0x00000010) /*!< Endpoint 4 register address */
+#define USB_EP5R (USB_BASE + 0x00000014) /*!< Endpoint 5 register address */
+#define USB_EP6R (USB_BASE + 0x00000018) /*!< Endpoint 6 register address */
+#define USB_EP7R (USB_BASE + 0x0000001C) /*!< Endpoint 7 register address */
+
+/* bit positions */
+#define USB_EP_CTR_RX ((uint32_t)0x00008000) /*!< EndPoint Correct TRansfer RX */
+#define USB_EP_DTOG_RX ((uint32_t)0x00004000) /*!< EndPoint Data TOGGLE RX */
+#define USB_EPRX_STAT ((uint32_t)0x00003000) /*!< EndPoint RX STATus bit field */
+#define USB_EP_SETUP ((uint32_t)0x00000800) /*!< EndPoint SETUP */
+#define USB_EP_T_FIELD ((uint32_t)0x00000600) /*!< EndPoint TYPE */
+#define USB_EP_KIND ((uint32_t)0x00000100) /*!< EndPoint KIND */
+#define USB_EP_CTR_TX ((uint32_t)0x00000080) /*!< EndPoint Correct TRansfer TX */
+#define USB_EP_DTOG_TX ((uint32_t)0x00000040) /*!< EndPoint Data TOGGLE TX */
+#define USB_EPTX_STAT ((uint32_t)0x00000030) /*!< EndPoint TX STATus bit field */
+#define USB_EPADDR_FIELD ((uint32_t)0x0000000F) /*!< EndPoint ADDRess FIELD */
+
+/* EndPoint REGister MASK (no toggle fields) */
+#define USB_EPREG_MASK (USB_EP_CTR_RX|USB_EP_SETUP|USB_EP_T_FIELD|USB_EP_KIND|USB_EP_CTR_TX|USB_EPADDR_FIELD)
+ /*!< EP_TYPE[1:0] EndPoint TYPE */
+#define USB_EP_TYPE_MASK ((uint32_t)0x00000600) /*!< EndPoint TYPE Mask */
+#define USB_EP_BULK ((uint32_t)0x00000000) /*!< EndPoint BULK */
+#define USB_EP_CONTROL ((uint32_t)0x00000200) /*!< EndPoint CONTROL */
+#define USB_EP_ISOCHRONOUS ((uint32_t)0x00000400) /*!< EndPoint ISOCHRONOUS */
+#define USB_EP_INTERRUPT ((uint32_t)0x00000600) /*!< EndPoint INTERRUPT */
+#define USB_EP_T_MASK (~USB_EP_T_FIELD & USB_EPREG_MASK)
+
+#define USB_EPKIND_MASK (~USB_EP_KIND & USB_EPREG_MASK) /*!< EP_KIND EndPoint KIND */
+ /*!< STAT_TX[1:0] STATus for TX transfer */
+#define USB_EP_TX_DIS ((uint32_t)0x00000000) /*!< EndPoint TX DISabled */
+#define USB_EP_TX_STALL ((uint32_t)0x00000010) /*!< EndPoint TX STALLed */
+#define USB_EP_TX_NAK ((uint32_t)0x00000020) /*!< EndPoint TX NAKed */
+#define USB_EP_TX_VALID ((uint32_t)0x00000030) /*!< EndPoint TX VALID */
+#define USB_EPTX_DTOG1 ((uint32_t)0x00000010) /*!< EndPoint TX Data TOGgle bit1 */
+#define USB_EPTX_DTOG2 ((uint32_t)0x00000020) /*!< EndPoint TX Data TOGgle bit2 */
+#define USB_EPTX_DTOGMASK (USB_EPTX_STAT|USB_EPREG_MASK)
+ /*!< STAT_RX[1:0] STATus for RX transfer */
+#define USB_EP_RX_DIS ((uint32_t)0x00000000) /*!< EndPoint RX DISabled */
+#define USB_EP_RX_STALL ((uint32_t)0x00001000) /*!< EndPoint RX STALLed */
+#define USB_EP_RX_NAK ((uint32_t)0x00002000) /*!< EndPoint RX NAKed */
+#define USB_EP_RX_VALID ((uint32_t)0x00003000) /*!< EndPoint RX VALID */
+#define USB_EPRX_DTOG1 ((uint32_t)0x00001000) /*!< EndPoint RX Data TOGgle bit1 */
+#define USB_EPRX_DTOG2 ((uint32_t)0x00002000) /*!< EndPoint RX Data TOGgle bit1 */
+#define USB_EPRX_DTOGMASK (USB_EPRX_STAT|USB_EPREG_MASK)
+
+/******************* Bit definition for USB_EP0R register *******************/
+#define USB_EP0R_EA ((uint32_t)0x0000000F) /*!< Endpoint Address */
+
+#define USB_EP0R_STAT_TX ((uint32_t)0x00000030) /*!< STAT_TX[1:0] bits (Status bits, for transmission transfers) */
+#define USB_EP0R_STAT_TX_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define USB_EP0R_STAT_TX_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define USB_EP0R_DTOG_TX ((uint32_t)0x00000040) /*!< Data Toggle, for transmission transfers */
+#define USB_EP0R_CTR_TX ((uint32_t)0x00000080) /*!< Correct Transfer for transmission */
+#define USB_EP0R_EP_KIND ((uint32_t)0x00000100) /*!< Endpoint Kind */
+
+#define USB_EP0R_EP_TYPE ((uint32_t)0x00000600) /*!< EP_TYPE[1:0] bits (Endpoint type) */
+#define USB_EP0R_EP_TYPE_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define USB_EP0R_EP_TYPE_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+
+#define USB_EP0R_SETUP ((uint32_t)0x00000800) /*!< Setup transaction completed */
+
+#define USB_EP0R_STAT_RX ((uint32_t)0x00003000) /*!< STAT_RX[1:0] bits (Status bits, for reception transfers) */
+#define USB_EP0R_STAT_RX_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define USB_EP0R_STAT_RX_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define USB_EP0R_DTOG_RX ((uint32_t)0x00004000) /*!< Data Toggle, for reception transfers */
+#define USB_EP0R_CTR_RX ((uint32_t)0x00008000) /*!< Correct Transfer for reception */
+
+/******************* Bit definition for USB_EP1R register *******************/
+#define USB_EP1R_EA ((uint32_t)0x0000000F) /*!< Endpoint Address */
+
+#define USB_EP1R_STAT_TX ((uint32_t)0x00000030) /*!< STAT_TX[1:0] bits (Status bits, for transmission transfers) */
+#define USB_EP1R_STAT_TX_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define USB_EP1R_STAT_TX_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define USB_EP1R_DTOG_TX ((uint32_t)0x00000040) /*!< Data Toggle, for transmission transfers */
+#define USB_EP1R_CTR_TX ((uint32_t)0x00000080) /*!< Correct Transfer for transmission */
+#define USB_EP1R_EP_KIND ((uint32_t)0x00000100) /*!< Endpoint Kind */
+
+#define USB_EP1R_EP_TYPE ((uint32_t)0x00000600) /*!< EP_TYPE[1:0] bits (Endpoint type) */
+#define USB_EP1R_EP_TYPE_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define USB_EP1R_EP_TYPE_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+
+#define USB_EP1R_SETUP ((uint32_t)0x00000800) /*!< Setup transaction completed */
+
+#define USB_EP1R_STAT_RX ((uint32_t)0x00003000) /*!< STAT_RX[1:0] bits (Status bits, for reception transfers) */
+#define USB_EP1R_STAT_RX_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define USB_EP1R_STAT_RX_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define USB_EP1R_DTOG_RX ((uint32_t)0x00004000) /*!< Data Toggle, for reception transfers */
+#define USB_EP1R_CTR_RX ((uint32_t)0x00008000) /*!< Correct Transfer for reception */
+
+/******************* Bit definition for USB_EP2R register *******************/
+#define USB_EP2R_EA ((uint32_t)0x0000000F) /*!< Endpoint Address */
+
+#define USB_EP2R_STAT_TX ((uint32_t)0x00000030) /*!< STAT_TX[1:0] bits (Status bits, for transmission transfers) */
+#define USB_EP2R_STAT_TX_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define USB_EP2R_STAT_TX_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define USB_EP2R_DTOG_TX ((uint32_t)0x00000040) /*!< Data Toggle, for transmission transfers */
+#define USB_EP2R_CTR_TX ((uint32_t)0x00000080) /*!< Correct Transfer for transmission */
+#define USB_EP2R_EP_KIND ((uint32_t)0x00000100) /*!< Endpoint Kind */
+
+#define USB_EP2R_EP_TYPE ((uint32_t)0x00000600) /*!< EP_TYPE[1:0] bits (Endpoint type) */
+#define USB_EP2R_EP_TYPE_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define USB_EP2R_EP_TYPE_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+
+#define USB_EP2R_SETUP ((uint32_t)0x00000800) /*!< Setup transaction completed */
+
+#define USB_EP2R_STAT_RX ((uint32_t)0x00003000) /*!< STAT_RX[1:0] bits (Status bits, for reception transfers) */
+#define USB_EP2R_STAT_RX_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define USB_EP2R_STAT_RX_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define USB_EP2R_DTOG_RX ((uint32_t)0x00004000) /*!< Data Toggle, for reception transfers */
+#define USB_EP2R_CTR_RX ((uint32_t)0x00008000) /*!< Correct Transfer for reception */
+
+/******************* Bit definition for USB_EP3R register *******************/
+#define USB_EP3R_EA ((uint32_t)0x0000000F) /*!< Endpoint Address */
+
+#define USB_EP3R_STAT_TX ((uint32_t)0x00000030) /*!< STAT_TX[1:0] bits (Status bits, for transmission transfers) */
+#define USB_EP3R_STAT_TX_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define USB_EP3R_STAT_TX_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define USB_EP3R_DTOG_TX ((uint32_t)0x00000040) /*!< Data Toggle, for transmission transfers */
+#define USB_EP3R_CTR_TX ((uint32_t)0x00000080) /*!< Correct Transfer for transmission */
+#define USB_EP3R_EP_KIND ((uint32_t)0x00000100) /*!< Endpoint Kind */
+
+#define USB_EP3R_EP_TYPE ((uint32_t)0x00000600) /*!< EP_TYPE[1:0] bits (Endpoint type) */
+#define USB_EP3R_EP_TYPE_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define USB_EP3R_EP_TYPE_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+
+#define USB_EP3R_SETUP ((uint32_t)0x00000800) /*!< Setup transaction completed */
+
+#define USB_EP3R_STAT_RX ((uint32_t)0x00003000) /*!< STAT_RX[1:0] bits (Status bits, for reception transfers) */
+#define USB_EP3R_STAT_RX_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define USB_EP3R_STAT_RX_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define USB_EP3R_DTOG_RX ((uint32_t)0x00004000) /*!< Data Toggle, for reception transfers */
+#define USB_EP3R_CTR_RX ((uint32_t)0x00008000) /*!< Correct Transfer for reception */
+
+/******************* Bit definition for USB_EP4R register *******************/
+#define USB_EP4R_EA ((uint32_t)0x0000000F) /*!< Endpoint Address */
+
+#define USB_EP4R_STAT_TX ((uint32_t)0x00000030) /*!< STAT_TX[1:0] bits (Status bits, for transmission transfers) */
+#define USB_EP4R_STAT_TX_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define USB_EP4R_STAT_TX_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define USB_EP4R_DTOG_TX ((uint32_t)0x00000040) /*!< Data Toggle, for transmission transfers */
+#define USB_EP4R_CTR_TX ((uint32_t)0x00000080) /*!< Correct Transfer for transmission */
+#define USB_EP4R_EP_KIND ((uint32_t)0x00000100) /*!< Endpoint Kind */
+
+#define USB_EP4R_EP_TYPE ((uint32_t)0x00000600) /*!< EP_TYPE[1:0] bits (Endpoint type) */
+#define USB_EP4R_EP_TYPE_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define USB_EP4R_EP_TYPE_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+
+#define USB_EP4R_SETUP ((uint32_t)0x00000800) /*!< Setup transaction completed */
+
+#define USB_EP4R_STAT_RX ((uint32_t)0x00003000) /*!< STAT_RX[1:0] bits (Status bits, for reception transfers) */
+#define USB_EP4R_STAT_RX_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define USB_EP4R_STAT_RX_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define USB_EP4R_DTOG_RX ((uint32_t)0x00004000) /*!< Data Toggle, for reception transfers */
+#define USB_EP4R_CTR_RX ((uint32_t)0x00008000) /*!< Correct Transfer for reception */
+
+/******************* Bit definition for USB_EP5R register *******************/
+#define USB_EP5R_EA ((uint32_t)0x0000000F) /*!< Endpoint Address */
+
+#define USB_EP5R_STAT_TX ((uint32_t)0x00000030) /*!< STAT_TX[1:0] bits (Status bits, for transmission transfers) */
+#define USB_EP5R_STAT_TX_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define USB_EP5R_STAT_TX_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define USB_EP5R_DTOG_TX ((uint32_t)0x00000040) /*!< Data Toggle, for transmission transfers */
+#define USB_EP5R_CTR_TX ((uint32_t)0x00000080) /*!< Correct Transfer for transmission */
+#define USB_EP5R_EP_KIND ((uint32_t)0x00000100) /*!< Endpoint Kind */
+
+#define USB_EP5R_EP_TYPE ((uint32_t)0x00000600) /*!< EP_TYPE[1:0] bits (Endpoint type) */
+#define USB_EP5R_EP_TYPE_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define USB_EP5R_EP_TYPE_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+
+#define USB_EP5R_SETUP ((uint32_t)0x00000800) /*!< Setup transaction completed */
+
+#define USB_EP5R_STAT_RX ((uint32_t)0x00003000) /*!< STAT_RX[1:0] bits (Status bits, for reception transfers) */
+#define USB_EP5R_STAT_RX_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define USB_EP5R_STAT_RX_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define USB_EP5R_DTOG_RX ((uint32_t)0x00004000) /*!< Data Toggle, for reception transfers */
+#define USB_EP5R_CTR_RX ((uint32_t)0x00008000) /*!< Correct Transfer for reception */
+
+/******************* Bit definition for USB_EP6R register *******************/
+#define USB_EP6R_EA ((uint32_t)0x0000000F) /*!< Endpoint Address */
+
+#define USB_EP6R_STAT_TX ((uint32_t)0x00000030) /*!< STAT_TX[1:0] bits (Status bits, for transmission transfers) */
+#define USB_EP6R_STAT_TX_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define USB_EP6R_STAT_TX_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define USB_EP6R_DTOG_TX ((uint32_t)0x00000040) /*!< Data Toggle, for transmission transfers */
+#define USB_EP6R_CTR_TX ((uint32_t)0x00000080) /*!< Correct Transfer for transmission */
+#define USB_EP6R_EP_KIND ((uint32_t)0x00000100) /*!< Endpoint Kind */
+
+#define USB_EP6R_EP_TYPE ((uint32_t)0x00000600) /*!< EP_TYPE[1:0] bits (Endpoint type) */
+#define USB_EP6R_EP_TYPE_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define USB_EP6R_EP_TYPE_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+
+#define USB_EP6R_SETUP ((uint32_t)0x00000800) /*!< Setup transaction completed */
+
+#define USB_EP6R_STAT_RX ((uint32_t)0x00003000) /*!< STAT_RX[1:0] bits (Status bits, for reception transfers) */
+#define USB_EP6R_STAT_RX_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define USB_EP6R_STAT_RX_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define USB_EP6R_DTOG_RX ((uint32_t)0x00004000) /*!< Data Toggle, for reception transfers */
+#define USB_EP6R_CTR_RX ((uint32_t)0x00008000) /*!< Correct Transfer for reception */
+
+/******************* Bit definition for USB_EP7R register *******************/
+#define USB_EP7R_EA ((uint32_t)0x0000000F) /*!< Endpoint Address */
+
+#define USB_EP7R_STAT_TX ((uint32_t)0x00000030) /*!< STAT_TX[1:0] bits (Status bits, for transmission transfers) */
+#define USB_EP7R_STAT_TX_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define USB_EP7R_STAT_TX_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define USB_EP7R_DTOG_TX ((uint32_t)0x00000040) /*!< Data Toggle, for transmission transfers */
+#define USB_EP7R_CTR_TX ((uint32_t)0x00000080) /*!< Correct Transfer for transmission */
+#define USB_EP7R_EP_KIND ((uint32_t)0x00000100) /*!< Endpoint Kind */
+
+#define USB_EP7R_EP_TYPE ((uint32_t)0x00000600) /*!< EP_TYPE[1:0] bits (Endpoint type) */
+#define USB_EP7R_EP_TYPE_0 ((uint32_t)0x00000200) /*!< Bit 0 */
+#define USB_EP7R_EP_TYPE_1 ((uint32_t)0x00000400) /*!< Bit 1 */
+
+#define USB_EP7R_SETUP ((uint32_t)0x00000800) /*!< Setup transaction completed */
+
+#define USB_EP7R_STAT_RX ((uint32_t)0x00003000) /*!< STAT_RX[1:0] bits (Status bits, for reception transfers) */
+#define USB_EP7R_STAT_RX_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define USB_EP7R_STAT_RX_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define USB_EP7R_DTOG_RX ((uint32_t)0x00004000) /*!< Data Toggle, for reception transfers */
+#define USB_EP7R_CTR_RX ((uint32_t)0x00008000) /*!< Correct Transfer for reception */
+
+/*!< Common registers */
+/******************* Bit definition for USB_CNTR register *******************/
+#define USB_CNTR_FRES ((uint32_t)0x00000001) /*!< Force USB Reset */
+#define USB_CNTR_PDWN ((uint32_t)0x00000002) /*!< Power down */
+#define USB_CNTR_LP_MODE ((uint32_t)0x00000004) /*!< Low-power mode */
+#define USB_CNTR_FSUSP ((uint32_t)0x00000008) /*!< Force suspend */
+#define USB_CNTR_RESUME ((uint32_t)0x00000010) /*!< Resume request */
+#define USB_CNTR_ESOFM ((uint32_t)0x00000100) /*!< Expected Start Of Frame Interrupt Mask */
+#define USB_CNTR_SOFM ((uint32_t)0x00000200) /*!< Start Of Frame Interrupt Mask */
+#define USB_CNTR_RESETM ((uint32_t)0x00000400) /*!< RESET Interrupt Mask */
+#define USB_CNTR_SUSPM ((uint32_t)0x00000800) /*!< Suspend mode Interrupt Mask */
+#define USB_CNTR_WKUPM ((uint32_t)0x00001000) /*!< Wakeup Interrupt Mask */
+#define USB_CNTR_ERRM ((uint32_t)0x00002000) /*!< Error Interrupt Mask */
+#define USB_CNTR_PMAOVRM ((uint32_t)0x00004000) /*!< Packet Memory Area Over / Underrun Interrupt Mask */
+#define USB_CNTR_CTRM ((uint32_t)0x00008000) /*!< Correct Transfer Interrupt Mask */
+
+/******************* Bit definition for USB_ISTR register *******************/
+#define USB_ISTR_EP_ID ((uint32_t)0x0000000F) /*!< Endpoint Identifier */
+#define USB_ISTR_DIR ((uint32_t)0x00000010) /*!< Direction of transaction */
+#define USB_ISTR_ESOF ((uint32_t)0x00000100) /*!< Expected Start Of Frame */
+#define USB_ISTR_SOF ((uint32_t)0x00000200) /*!< Start Of Frame */
+#define USB_ISTR_RESET ((uint32_t)0x00000400) /*!< USB RESET request */
+#define USB_ISTR_SUSP ((uint32_t)0x00000800) /*!< Suspend mode request */
+#define USB_ISTR_WKUP ((uint32_t)0x00001000) /*!< Wake up */
+#define USB_ISTR_ERR ((uint32_t)0x00002000) /*!< Error */
+#define USB_ISTR_PMAOVR ((uint32_t)0x00004000) /*!< Packet Memory Area Over / Underrun */
+#define USB_ISTR_CTR ((uint32_t)0x00008000) /*!< Correct Transfer */
+
+/******************* Bit definition for USB_FNR register ********************/
+#define USB_FNR_FN ((uint32_t)0x000007FF) /*!< Frame Number */
+#define USB_FNR_LSOF ((uint32_t)0x00001800) /*!< Lost SOF */
+#define USB_FNR_LCK ((uint32_t)0x00002000) /*!< Locked */
+#define USB_FNR_RXDM ((uint32_t)0x00004000) /*!< Receive Data - Line Status */
+#define USB_FNR_RXDP ((uint32_t)0x00008000) /*!< Receive Data + Line Status */
+
+/****************** Bit definition for USB_DADDR register *******************/
+#define USB_DADDR_ADD ((uint32_t)0x0000007F) /*!< ADD[6:0] bits (Device Address) */
+#define USB_DADDR_ADD0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define USB_DADDR_ADD1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define USB_DADDR_ADD2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define USB_DADDR_ADD3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define USB_DADDR_ADD4 ((uint32_t)0x00000010) /*!< Bit 4 */
+#define USB_DADDR_ADD5 ((uint32_t)0x00000020) /*!< Bit 5 */
+#define USB_DADDR_ADD6 ((uint32_t)0x00000040) /*!< Bit 6 */
+
+#define USB_DADDR_EF ((uint32_t)0x00000080) /*!< Enable Function */
+
+/****************** Bit definition for USB_BTABLE register ******************/
+#define USB_BTABLE_BTABLE ((uint32_t)0x0000FFF8) /*!< Buffer Table */
+
+/*!< Buffer descriptor table */
+/***************** Bit definition for USB_ADDR0_TX register *****************/
+#define USB_ADDR0_TX_ADDR0_TX ((uint32_t)0x0000FFFE) /*!< Transmission Buffer Address 0 */
+
+/***************** Bit definition for USB_ADDR1_TX register *****************/
+#define USB_ADDR1_TX_ADDR1_TX ((uint32_t)0x0000FFFE) /*!< Transmission Buffer Address 1 */
+
+/***************** Bit definition for USB_ADDR2_TX register *****************/
+#define USB_ADDR2_TX_ADDR2_TX ((uint32_t)0x0000FFFE) /*!< Transmission Buffer Address 2 */
+
+/***************** Bit definition for USB_ADDR3_TX register *****************/
+#define USB_ADDR3_TX_ADDR3_TX ((uint32_t)0x0000FFFE) /*!< Transmission Buffer Address 3 */
+
+/***************** Bit definition for USB_ADDR4_TX register *****************/
+#define USB_ADDR4_TX_ADDR4_TX ((uint32_t)0x0000FFFE) /*!< Transmission Buffer Address 4 */
+
+/***************** Bit definition for USB_ADDR5_TX register *****************/
+#define USB_ADDR5_TX_ADDR5_TX ((uint32_t)0x0000FFFE) /*!< Transmission Buffer Address 5 */
+
+/***************** Bit definition for USB_ADDR6_TX register *****************/
+#define USB_ADDR6_TX_ADDR6_TX ((uint32_t)0x0000FFFE) /*!< Transmission Buffer Address 6 */
+
+/***************** Bit definition for USB_ADDR7_TX register *****************/
+#define USB_ADDR7_TX_ADDR7_TX ((uint32_t)0x0000FFFE) /*!< Transmission Buffer Address 7 */
+
+/*----------------------------------------------------------------------------*/
+
+/***************** Bit definition for USB_COUNT0_TX register ****************/
+#define USB_COUNT0_TX_COUNT0_TX ((uint32_t)0x000003FF) /*!< Transmission Byte Count 0 */
+
+/***************** Bit definition for USB_COUNT1_TX register ****************/
+#define USB_COUNT1_TX_COUNT1_TX ((uint32_t)0x000003FF) /*!< Transmission Byte Count 1 */
+
+/***************** Bit definition for USB_COUNT2_TX register ****************/
+#define USB_COUNT2_TX_COUNT2_TX ((uint32_t)0x000003FF) /*!< Transmission Byte Count 2 */
+
+/***************** Bit definition for USB_COUNT3_TX register ****************/
+#define USB_COUNT3_TX_COUNT3_TX ((uint32_t)0x000003FF) /*!< Transmission Byte Count 3 */
+
+/***************** Bit definition for USB_COUNT4_TX register ****************/
+#define USB_COUNT4_TX_COUNT4_TX ((uint32_t)0x000003FF) /*!< Transmission Byte Count 4 */
+
+/***************** Bit definition for USB_COUNT5_TX register ****************/
+#define USB_COUNT5_TX_COUNT5_TX ((uint32_t)0x000003FF) /*!< Transmission Byte Count 5 */
+
+/***************** Bit definition for USB_COUNT6_TX register ****************/
+#define USB_COUNT6_TX_COUNT6_TX ((uint32_t)0x000003FF) /*!< Transmission Byte Count 6 */
+
+/***************** Bit definition for USB_COUNT7_TX register ****************/
+#define USB_COUNT7_TX_COUNT7_TX ((uint32_t)0x000003FF) /*!< Transmission Byte Count 7 */
+
+/*----------------------------------------------------------------------------*/
+
+/**************** Bit definition for USB_COUNT0_TX_0 register ***************/
+#define USB_COUNT0_TX_0_COUNT0_TX_0 ((uint32_t)0x000003FF) /*!< Transmission Byte Count 0 (low) */
+
+/**************** Bit definition for USB_COUNT0_TX_1 register ***************/
+#define USB_COUNT0_TX_1_COUNT0_TX_1 ((uint32_t)0x03FF0000) /*!< Transmission Byte Count 0 (high) */
+
+/**************** Bit definition for USB_COUNT1_TX_0 register ***************/
+#define USB_COUNT1_TX_0_COUNT1_TX_0 ((uint32_t)0x000003FF) /*!< Transmission Byte Count 1 (low) */
+
+/**************** Bit definition for USB_COUNT1_TX_1 register ***************/
+#define USB_COUNT1_TX_1_COUNT1_TX_1 ((uint32_t)0x03FF0000) /*!< Transmission Byte Count 1 (high) */
+
+/**************** Bit definition for USB_COUNT2_TX_0 register ***************/
+#define USB_COUNT2_TX_0_COUNT2_TX_0 ((uint32_t)0x000003FF) /*!< Transmission Byte Count 2 (low) */
+
+/**************** Bit definition for USB_COUNT2_TX_1 register ***************/
+#define USB_COUNT2_TX_1_COUNT2_TX_1 ((uint32_t)0x03FF0000) /*!< Transmission Byte Count 2 (high) */
+
+/**************** Bit definition for USB_COUNT3_TX_0 register ***************/
+#define USB_COUNT3_TX_0_COUNT3_TX_0 ((uint32_t)0x000003FF) /*!< Transmission Byte Count 3 (low) */
+
+/**************** Bit definition for USB_COUNT3_TX_1 register ***************/
+#define USB_COUNT3_TX_1_COUNT3_TX_1 ((uint32_t)0x03FF0000) /*!< Transmission Byte Count 3 (high) */
+
+/**************** Bit definition for USB_COUNT4_TX_0 register ***************/
+#define USB_COUNT4_TX_0_COUNT4_TX_0 ((uint32_t)0x000003FF) /*!< Transmission Byte Count 4 (low) */
+
+/**************** Bit definition for USB_COUNT4_TX_1 register ***************/
+#define USB_COUNT4_TX_1_COUNT4_TX_1 ((uint32_t)0x03FF0000) /*!< Transmission Byte Count 4 (high) */
+
+/**************** Bit definition for USB_COUNT5_TX_0 register ***************/
+#define USB_COUNT5_TX_0_COUNT5_TX_0 ((uint32_t)0x000003FF) /*!< Transmission Byte Count 5 (low) */
+
+/**************** Bit definition for USB_COUNT5_TX_1 register ***************/
+#define USB_COUNT5_TX_1_COUNT5_TX_1 ((uint32_t)0x03FF0000) /*!< Transmission Byte Count 5 (high) */
+
+/**************** Bit definition for USB_COUNT6_TX_0 register ***************/
+#define USB_COUNT6_TX_0_COUNT6_TX_0 ((uint32_t)0x000003FF) /*!< Transmission Byte Count 6 (low) */
+
+/**************** Bit definition for USB_COUNT6_TX_1 register ***************/
+#define USB_COUNT6_TX_1_COUNT6_TX_1 ((uint32_t)0x03FF0000) /*!< Transmission Byte Count 6 (high) */
+
+/**************** Bit definition for USB_COUNT7_TX_0 register ***************/
+#define USB_COUNT7_TX_0_COUNT7_TX_0 ((uint32_t)0x000003FF) /*!< Transmission Byte Count 7 (low) */
+
+/**************** Bit definition for USB_COUNT7_TX_1 register ***************/
+#define USB_COUNT7_TX_1_COUNT7_TX_1 ((uint32_t)0x03FF0000) /*!< Transmission Byte Count 7 (high) */
+
+/*----------------------------------------------------------------------------*/
+
+/***************** Bit definition for USB_ADDR0_RX register *****************/
+#define USB_ADDR0_RX_ADDR0_RX ((uint32_t)0x0000FFFE) /*!< Reception Buffer Address 0 */
+
+/***************** Bit definition for USB_ADDR1_RX register *****************/
+#define USB_ADDR1_RX_ADDR1_RX ((uint32_t)0x0000FFFE) /*!< Reception Buffer Address 1 */
+
+/***************** Bit definition for USB_ADDR2_RX register *****************/
+#define USB_ADDR2_RX_ADDR2_RX ((uint32_t)0x0000FFFE) /*!< Reception Buffer Address 2 */
+
+/***************** Bit definition for USB_ADDR3_RX register *****************/
+#define USB_ADDR3_RX_ADDR3_RX ((uint32_t)0x0000FFFE) /*!< Reception Buffer Address 3 */
+
+/***************** Bit definition for USB_ADDR4_RX register *****************/
+#define USB_ADDR4_RX_ADDR4_RX ((uint32_t)0x0000FFFE) /*!< Reception Buffer Address 4 */
+
+/***************** Bit definition for USB_ADDR5_RX register *****************/
+#define USB_ADDR5_RX_ADDR5_RX ((uint32_t)0x0000FFFE) /*!< Reception Buffer Address 5 */
+
+/***************** Bit definition for USB_ADDR6_RX register *****************/
+#define USB_ADDR6_RX_ADDR6_RX ((uint32_t)0x0000FFFE) /*!< Reception Buffer Address 6 */
+
+/***************** Bit definition for USB_ADDR7_RX register *****************/
+#define USB_ADDR7_RX_ADDR7_RX ((uint32_t)0x0000FFFE) /*!< Reception Buffer Address 7 */
+
+/*----------------------------------------------------------------------------*/
+
+/***************** Bit definition for USB_COUNT0_RX register ****************/
+#define USB_COUNT0_RX_COUNT0_RX ((uint32_t)0x000003FF) /*!< Reception Byte Count */
+
+#define USB_COUNT0_RX_NUM_BLOCK ((uint32_t)0x00007C00) /*!< NUM_BLOCK[4:0] bits (Number of blocks) */
+#define USB_COUNT0_RX_NUM_BLOCK_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT0_RX_NUM_BLOCK_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT0_RX_NUM_BLOCK_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT0_RX_NUM_BLOCK_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT0_RX_NUM_BLOCK_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT0_RX_BLSIZE ((uint32_t)0x00008000) /*!< BLock SIZE */
+
+/***************** Bit definition for USB_COUNT1_RX register ****************/
+#define USB_COUNT1_RX_COUNT1_RX ((uint32_t)0x000003FF) /*!< Reception Byte Count */
+
+#define USB_COUNT1_RX_NUM_BLOCK ((uint32_t)0x00007C00) /*!< NUM_BLOCK[4:0] bits (Number of blocks) */
+#define USB_COUNT1_RX_NUM_BLOCK_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT1_RX_NUM_BLOCK_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT1_RX_NUM_BLOCK_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT1_RX_NUM_BLOCK_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT1_RX_NUM_BLOCK_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT1_RX_BLSIZE ((uint32_t)0x00008000) /*!< BLock SIZE */
+
+/***************** Bit definition for USB_COUNT2_RX register ****************/
+#define USB_COUNT2_RX_COUNT2_RX ((uint32_t)0x000003FF) /*!< Reception Byte Count */
+
+#define USB_COUNT2_RX_NUM_BLOCK ((uint32_t)0x00007C00) /*!< NUM_BLOCK[4:0] bits (Number of blocks) */
+#define USB_COUNT2_RX_NUM_BLOCK_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT2_RX_NUM_BLOCK_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT2_RX_NUM_BLOCK_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT2_RX_NUM_BLOCK_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT2_RX_NUM_BLOCK_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT2_RX_BLSIZE ((uint32_t)0x00008000) /*!< BLock SIZE */
+
+/***************** Bit definition for USB_COUNT3_RX register ****************/
+#define USB_COUNT3_RX_COUNT3_RX ((uint32_t)0x000003FF) /*!< Reception Byte Count */
+
+#define USB_COUNT3_RX_NUM_BLOCK ((uint32_t)0x00007C00) /*!< NUM_BLOCK[4:0] bits (Number of blocks) */
+#define USB_COUNT3_RX_NUM_BLOCK_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT3_RX_NUM_BLOCK_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT3_RX_NUM_BLOCK_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT3_RX_NUM_BLOCK_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT3_RX_NUM_BLOCK_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT3_RX_BLSIZE ((uint32_t)0x00008000) /*!< BLock SIZE */
+
+/***************** Bit definition for USB_COUNT4_RX register ****************/
+#define USB_COUNT4_RX_COUNT4_RX ((uint32_t)0x000003FF) /*!< Reception Byte Count */
+
+#define USB_COUNT4_RX_NUM_BLOCK ((uint32_t)0x00007C00) /*!< NUM_BLOCK[4:0] bits (Number of blocks) */
+#define USB_COUNT4_RX_NUM_BLOCK_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT4_RX_NUM_BLOCK_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT4_RX_NUM_BLOCK_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT4_RX_NUM_BLOCK_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT4_RX_NUM_BLOCK_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT4_RX_BLSIZE ((uint32_t)0x00008000) /*!< BLock SIZE */
+
+/***************** Bit definition for USB_COUNT5_RX register ****************/
+#define USB_COUNT5_RX_COUNT5_RX ((uint32_t)0x000003FF) /*!< Reception Byte Count */
+
+#define USB_COUNT5_RX_NUM_BLOCK ((uint32_t)0x00007C00) /*!< NUM_BLOCK[4:0] bits (Number of blocks) */
+#define USB_COUNT5_RX_NUM_BLOCK_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT5_RX_NUM_BLOCK_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT5_RX_NUM_BLOCK_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT5_RX_NUM_BLOCK_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT5_RX_NUM_BLOCK_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT5_RX_BLSIZE ((uint32_t)0x00008000) /*!< BLock SIZE */
+
+/***************** Bit definition for USB_COUNT6_RX register ****************/
+#define USB_COUNT6_RX_COUNT6_RX ((uint32_t)0x000003FF) /*!< Reception Byte Count */
+
+#define USB_COUNT6_RX_NUM_BLOCK ((uint32_t)0x00007C00) /*!< NUM_BLOCK[4:0] bits (Number of blocks) */
+#define USB_COUNT6_RX_NUM_BLOCK_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT6_RX_NUM_BLOCK_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT6_RX_NUM_BLOCK_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT6_RX_NUM_BLOCK_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT6_RX_NUM_BLOCK_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT6_RX_BLSIZE ((uint32_t)0x00008000) /*!< BLock SIZE */
+
+/***************** Bit definition for USB_COUNT7_RX register ****************/
+#define USB_COUNT7_RX_COUNT7_RX ((uint32_t)0x000003FF) /*!< Reception Byte Count */
+
+#define USB_COUNT7_RX_NUM_BLOCK ((uint32_t)0x00007C00) /*!< NUM_BLOCK[4:0] bits (Number of blocks) */
+#define USB_COUNT7_RX_NUM_BLOCK_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT7_RX_NUM_BLOCK_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT7_RX_NUM_BLOCK_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT7_RX_NUM_BLOCK_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT7_RX_NUM_BLOCK_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT7_RX_BLSIZE ((uint32_t)0x00008000) /*!< BLock SIZE */
+
+/*----------------------------------------------------------------------------*/
+
+/**************** Bit definition for USB_COUNT0_RX_0 register ***************/
+#define USB_COUNT0_RX_0_COUNT0_RX_0 ((uint32_t)0x000003FF) /*!< Reception Byte Count (low) */
+
+#define USB_COUNT0_RX_0_NUM_BLOCK_0 ((uint32_t)0x00007C00) /*!< NUM_BLOCK_0[4:0] bits (Number of blocks) (low) */
+#define USB_COUNT0_RX_0_NUM_BLOCK_0_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT0_RX_0_NUM_BLOCK_0_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT0_RX_0_NUM_BLOCK_0_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT0_RX_0_NUM_BLOCK_0_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT0_RX_0_NUM_BLOCK_0_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT0_RX_0_BLSIZE_0 ((uint32_t)0x00008000) /*!< BLock SIZE (low) */
+
+/**************** Bit definition for USB_COUNT0_RX_1 register ***************/
+#define USB_COUNT0_RX_1_COUNT0_RX_1 ((uint32_t)0x03FF0000) /*!< Reception Byte Count (high) */
+
+#define USB_COUNT0_RX_1_NUM_BLOCK_1 ((uint32_t)0x7C000000) /*!< NUM_BLOCK_1[4:0] bits (Number of blocks) (high) */
+#define USB_COUNT0_RX_1_NUM_BLOCK_1_0 ((uint32_t)0x04000000) /*!< Bit 1 */
+#define USB_COUNT0_RX_1_NUM_BLOCK_1_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+#define USB_COUNT0_RX_1_NUM_BLOCK_1_2 ((uint32_t)0x10000000) /*!< Bit 2 */
+#define USB_COUNT0_RX_1_NUM_BLOCK_1_3 ((uint32_t)0x20000000) /*!< Bit 3 */
+#define USB_COUNT0_RX_1_NUM_BLOCK_1_4 ((uint32_t)0x40000000) /*!< Bit 4 */
+
+#define USB_COUNT0_RX_1_BLSIZE_1 ((uint32_t)0x80000000) /*!< BLock SIZE (high) */
+
+/**************** Bit definition for USB_COUNT1_RX_0 register ***************/
+#define USB_COUNT1_RX_0_COUNT1_RX_0 ((uint32_t)0x000003FF) /*!< Reception Byte Count (low) */
+
+#define USB_COUNT1_RX_0_NUM_BLOCK_0 ((uint32_t)0x00007C00) /*!< NUM_BLOCK_0[4:0] bits (Number of blocks) (low) */
+#define USB_COUNT1_RX_0_NUM_BLOCK_0_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT1_RX_0_NUM_BLOCK_0_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT1_RX_0_NUM_BLOCK_0_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT1_RX_0_NUM_BLOCK_0_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT1_RX_0_NUM_BLOCK_0_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT1_RX_0_BLSIZE_0 ((uint32_t)0x00008000) /*!< BLock SIZE (low) */
+
+/**************** Bit definition for USB_COUNT1_RX_1 register ***************/
+#define USB_COUNT1_RX_1_COUNT1_RX_1 ((uint32_t)0x03FF0000) /*!< Reception Byte Count (high) */
+
+#define USB_COUNT1_RX_1_NUM_BLOCK_1 ((uint32_t)0x7C000000) /*!< NUM_BLOCK_1[4:0] bits (Number of blocks) (high) */
+#define USB_COUNT1_RX_1_NUM_BLOCK_1_0 ((uint32_t)0x04000000) /*!< Bit 0 */
+#define USB_COUNT1_RX_1_NUM_BLOCK_1_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+#define USB_COUNT1_RX_1_NUM_BLOCK_1_2 ((uint32_t)0x10000000) /*!< Bit 2 */
+#define USB_COUNT1_RX_1_NUM_BLOCK_1_3 ((uint32_t)0x20000000) /*!< Bit 3 */
+#define USB_COUNT1_RX_1_NUM_BLOCK_1_4 ((uint32_t)0x40000000) /*!< Bit 4 */
+
+#define USB_COUNT1_RX_1_BLSIZE_1 ((uint32_t)0x80000000) /*!< BLock SIZE (high) */
+
+/**************** Bit definition for USB_COUNT2_RX_0 register ***************/
+#define USB_COUNT2_RX_0_COUNT2_RX_0 ((uint32_t)0x000003FF) /*!< Reception Byte Count (low) */
+
+#define USB_COUNT2_RX_0_NUM_BLOCK_0 ((uint32_t)0x00007C00) /*!< NUM_BLOCK_0[4:0] bits (Number of blocks) (low) */
+#define USB_COUNT2_RX_0_NUM_BLOCK_0_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT2_RX_0_NUM_BLOCK_0_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT2_RX_0_NUM_BLOCK_0_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT2_RX_0_NUM_BLOCK_0_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT2_RX_0_NUM_BLOCK_0_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT2_RX_0_BLSIZE_0 ((uint32_t)0x00008000) /*!< BLock SIZE (low) */
+
+/**************** Bit definition for USB_COUNT2_RX_1 register ***************/
+#define USB_COUNT2_RX_1_COUNT2_RX_1 ((uint32_t)0x03FF0000) /*!< Reception Byte Count (high) */
+
+#define USB_COUNT2_RX_1_NUM_BLOCK_1 ((uint32_t)0x7C000000) /*!< NUM_BLOCK_1[4:0] bits (Number of blocks) (high) */
+#define USB_COUNT2_RX_1_NUM_BLOCK_1_0 ((uint32_t)0x04000000) /*!< Bit 0 */
+#define USB_COUNT2_RX_1_NUM_BLOCK_1_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+#define USB_COUNT2_RX_1_NUM_BLOCK_1_2 ((uint32_t)0x10000000) /*!< Bit 2 */
+#define USB_COUNT2_RX_1_NUM_BLOCK_1_3 ((uint32_t)0x20000000) /*!< Bit 3 */
+#define USB_COUNT2_RX_1_NUM_BLOCK_1_4 ((uint32_t)0x40000000) /*!< Bit 4 */
+
+#define USB_COUNT2_RX_1_BLSIZE_1 ((uint32_t)0x80000000) /*!< BLock SIZE (high) */
+
+/**************** Bit definition for USB_COUNT3_RX_0 register ***************/
+#define USB_COUNT3_RX_0_COUNT3_RX_0 ((uint32_t)0x000003FF) /*!< Reception Byte Count (low) */
+
+#define USB_COUNT3_RX_0_NUM_BLOCK_0 ((uint32_t)0x00007C00) /*!< NUM_BLOCK_0[4:0] bits (Number of blocks) (low) */
+#define USB_COUNT3_RX_0_NUM_BLOCK_0_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT3_RX_0_NUM_BLOCK_0_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT3_RX_0_NUM_BLOCK_0_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT3_RX_0_NUM_BLOCK_0_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT3_RX_0_NUM_BLOCK_0_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT3_RX_0_BLSIZE_0 ((uint32_t)0x00008000) /*!< BLock SIZE (low) */
+
+/**************** Bit definition for USB_COUNT3_RX_1 register ***************/
+#define USB_COUNT3_RX_1_COUNT3_RX_1 ((uint32_t)0x03FF0000) /*!< Reception Byte Count (high) */
+
+#define USB_COUNT3_RX_1_NUM_BLOCK_1 ((uint32_t)0x7C000000) /*!< NUM_BLOCK_1[4:0] bits (Number of blocks) (high) */
+#define USB_COUNT3_RX_1_NUM_BLOCK_1_0 ((uint32_t)0x04000000) /*!< Bit 0 */
+#define USB_COUNT3_RX_1_NUM_BLOCK_1_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+#define USB_COUNT3_RX_1_NUM_BLOCK_1_2 ((uint32_t)0x10000000) /*!< Bit 2 */
+#define USB_COUNT3_RX_1_NUM_BLOCK_1_3 ((uint32_t)0x20000000) /*!< Bit 3 */
+#define USB_COUNT3_RX_1_NUM_BLOCK_1_4 ((uint32_t)0x40000000) /*!< Bit 4 */
+
+#define USB_COUNT3_RX_1_BLSIZE_1 ((uint32_t)0x80000000) /*!< BLock SIZE (high) */
+
+/**************** Bit definition for USB_COUNT4_RX_0 register ***************/
+#define USB_COUNT4_RX_0_COUNT4_RX_0 ((uint32_t)0x000003FF) /*!< Reception Byte Count (low) */
+
+#define USB_COUNT4_RX_0_NUM_BLOCK_0 ((uint32_t)0x00007C00) /*!< NUM_BLOCK_0[4:0] bits (Number of blocks) (low) */
+#define USB_COUNT4_RX_0_NUM_BLOCK_0_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT4_RX_0_NUM_BLOCK_0_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT4_RX_0_NUM_BLOCK_0_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT4_RX_0_NUM_BLOCK_0_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT4_RX_0_NUM_BLOCK_0_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT4_RX_0_BLSIZE_0 ((uint32_t)0x00008000) /*!< BLock SIZE (low) */
+
+/**************** Bit definition for USB_COUNT4_RX_1 register ***************/
+#define USB_COUNT4_RX_1_COUNT4_RX_1 ((uint32_t)0x03FF0000) /*!< Reception Byte Count (high) */
+
+#define USB_COUNT4_RX_1_NUM_BLOCK_1 ((uint32_t)0x7C000000) /*!< NUM_BLOCK_1[4:0] bits (Number of blocks) (high) */
+#define USB_COUNT4_RX_1_NUM_BLOCK_1_0 ((uint32_t)0x04000000) /*!< Bit 0 */
+#define USB_COUNT4_RX_1_NUM_BLOCK_1_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+#define USB_COUNT4_RX_1_NUM_BLOCK_1_2 ((uint32_t)0x10000000) /*!< Bit 2 */
+#define USB_COUNT4_RX_1_NUM_BLOCK_1_3 ((uint32_t)0x20000000) /*!< Bit 3 */
+#define USB_COUNT4_RX_1_NUM_BLOCK_1_4 ((uint32_t)0x40000000) /*!< Bit 4 */
+
+#define USB_COUNT4_RX_1_BLSIZE_1 ((uint32_t)0x80000000) /*!< BLock SIZE (high) */
+
+/**************** Bit definition for USB_COUNT5_RX_0 register ***************/
+#define USB_COUNT5_RX_0_COUNT5_RX_0 ((uint32_t)0x000003FF) /*!< Reception Byte Count (low) */
+
+#define USB_COUNT5_RX_0_NUM_BLOCK_0 ((uint32_t)0x00007C00) /*!< NUM_BLOCK_0[4:0] bits (Number of blocks) (low) */
+#define USB_COUNT5_RX_0_NUM_BLOCK_0_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT5_RX_0_NUM_BLOCK_0_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT5_RX_0_NUM_BLOCK_0_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT5_RX_0_NUM_BLOCK_0_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT5_RX_0_NUM_BLOCK_0_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT5_RX_0_BLSIZE_0 ((uint32_t)0x00008000) /*!< BLock SIZE (low) */
+
+/**************** Bit definition for USB_COUNT5_RX_1 register ***************/
+#define USB_COUNT5_RX_1_COUNT5_RX_1 ((uint32_t)0x03FF0000) /*!< Reception Byte Count (high) */
+
+#define USB_COUNT5_RX_1_NUM_BLOCK_1 ((uint32_t)0x7C000000) /*!< NUM_BLOCK_1[4:0] bits (Number of blocks) (high) */
+#define USB_COUNT5_RX_1_NUM_BLOCK_1_0 ((uint32_t)0x04000000) /*!< Bit 0 */
+#define USB_COUNT5_RX_1_NUM_BLOCK_1_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+#define USB_COUNT5_RX_1_NUM_BLOCK_1_2 ((uint32_t)0x10000000) /*!< Bit 2 */
+#define USB_COUNT5_RX_1_NUM_BLOCK_1_3 ((uint32_t)0x20000000) /*!< Bit 3 */
+#define USB_COUNT5_RX_1_NUM_BLOCK_1_4 ((uint32_t)0x40000000) /*!< Bit 4 */
+
+#define USB_COUNT5_RX_1_BLSIZE_1 ((uint32_t)0x80000000) /*!< BLock SIZE (high) */
+
+/*************** Bit definition for USB_COUNT6_RX_0 register ***************/
+#define USB_COUNT6_RX_0_COUNT6_RX_0 ((uint32_t)0x000003FF) /*!< Reception Byte Count (low) */
+
+#define USB_COUNT6_RX_0_NUM_BLOCK_0 ((uint32_t)0x00007C00) /*!< NUM_BLOCK_0[4:0] bits (Number of blocks) (low) */
+#define USB_COUNT6_RX_0_NUM_BLOCK_0_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT6_RX_0_NUM_BLOCK_0_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT6_RX_0_NUM_BLOCK_0_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT6_RX_0_NUM_BLOCK_0_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT6_RX_0_NUM_BLOCK_0_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT6_RX_0_BLSIZE_0 ((uint32_t)0x00008000) /*!< BLock SIZE (low) */
+
+/**************** Bit definition for USB_COUNT6_RX_1 register ***************/
+#define USB_COUNT6_RX_1_COUNT6_RX_1 ((uint32_t)0x03FF0000) /*!< Reception Byte Count (high) */
+
+#define USB_COUNT6_RX_1_NUM_BLOCK_1 ((uint32_t)0x7C000000) /*!< NUM_BLOCK_1[4:0] bits (Number of blocks) (high) */
+#define USB_COUNT6_RX_1_NUM_BLOCK_1_0 ((uint32_t)0x04000000) /*!< Bit 0 */
+#define USB_COUNT6_RX_1_NUM_BLOCK_1_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+#define USB_COUNT6_RX_1_NUM_BLOCK_1_2 ((uint32_t)0x10000000) /*!< Bit 2 */
+#define USB_COUNT6_RX_1_NUM_BLOCK_1_3 ((uint32_t)0x20000000) /*!< Bit 3 */
+#define USB_COUNT6_RX_1_NUM_BLOCK_1_4 ((uint32_t)0x40000000) /*!< Bit 4 */
+
+#define USB_COUNT6_RX_1_BLSIZE_1 ((uint32_t)0x80000000) /*!< BLock SIZE (high) */
+
+/*************** Bit definition for USB_COUNT7_RX_0 register ****************/
+#define USB_COUNT7_RX_0_COUNT7_RX_0 ((uint32_t)0x000003FF) /*!< Reception Byte Count (low) */
+
+#define USB_COUNT7_RX_0_NUM_BLOCK_0 ((uint32_t)0x00007C00) /*!< NUM_BLOCK_0[4:0] bits (Number of blocks) (low) */
+#define USB_COUNT7_RX_0_NUM_BLOCK_0_0 ((uint32_t)0x00000400) /*!< Bit 0 */
+#define USB_COUNT7_RX_0_NUM_BLOCK_0_1 ((uint32_t)0x00000800) /*!< Bit 1 */
+#define USB_COUNT7_RX_0_NUM_BLOCK_0_2 ((uint32_t)0x00001000) /*!< Bit 2 */
+#define USB_COUNT7_RX_0_NUM_BLOCK_0_3 ((uint32_t)0x00002000) /*!< Bit 3 */
+#define USB_COUNT7_RX_0_NUM_BLOCK_0_4 ((uint32_t)0x00004000) /*!< Bit 4 */
+
+#define USB_COUNT7_RX_0_BLSIZE_0 ((uint32_t)0x00008000) /*!< BLock SIZE (low) */
+
+/*************** Bit definition for USB_COUNT7_RX_1 register ****************/
+#define USB_COUNT7_RX_1_COUNT7_RX_1 ((uint32_t)0x03FF0000) /*!< Reception Byte Count (high) */
+
+#define USB_COUNT7_RX_1_NUM_BLOCK_1 ((uint32_t)0x7C000000) /*!< NUM_BLOCK_1[4:0] bits (Number of blocks) (high) */
+#define USB_COUNT7_RX_1_NUM_BLOCK_1_0 ((uint32_t)0x04000000) /*!< Bit 0 */
+#define USB_COUNT7_RX_1_NUM_BLOCK_1_1 ((uint32_t)0x08000000) /*!< Bit 1 */
+#define USB_COUNT7_RX_1_NUM_BLOCK_1_2 ((uint32_t)0x10000000) /*!< Bit 2 */
+#define USB_COUNT7_RX_1_NUM_BLOCK_1_3 ((uint32_t)0x20000000) /*!< Bit 3 */
+#define USB_COUNT7_RX_1_NUM_BLOCK_1_4 ((uint32_t)0x40000000) /*!< Bit 4 */
+
+#define USB_COUNT7_RX_1_BLSIZE_1 ((uint32_t)0x80000000) /*!< BLock SIZE (high) */
+
+/******************************************************************************/
+/* */
+/* Controller Area Network */
+/* */
+/******************************************************************************/
+
+/*!< CAN control and status registers */
+/******************* Bit definition for CAN_MCR register ********************/
+#define CAN_MCR_INRQ ((uint32_t)0x00000001) /*!< Initialization Request */
+#define CAN_MCR_SLEEP ((uint32_t)0x00000002) /*!< Sleep Mode Request */
+#define CAN_MCR_TXFP ((uint32_t)0x00000004) /*!< Transmit FIFO Priority */
+#define CAN_MCR_RFLM ((uint32_t)0x00000008) /*!< Receive FIFO Locked Mode */
+#define CAN_MCR_NART ((uint32_t)0x00000010) /*!< No Automatic Retransmission */
+#define CAN_MCR_AWUM ((uint32_t)0x00000020) /*!< Automatic Wakeup Mode */
+#define CAN_MCR_ABOM ((uint32_t)0x00000040) /*!< Automatic Bus-Off Management */
+#define CAN_MCR_TTCM ((uint32_t)0x00000080) /*!< Time Triggered Communication Mode */
+#define CAN_MCR_RESET ((uint32_t)0x00008000) /*!< CAN software master reset */
+#define CAN_MCR_DBF ((uint32_t)0x00010000) /*!< CAN Debug freeze */
+
+/******************* Bit definition for CAN_MSR register ********************/
+#define CAN_MSR_INAK ((uint32_t)0x00000001) /*!< Initialization Acknowledge */
+#define CAN_MSR_SLAK ((uint32_t)0x00000002) /*!< Sleep Acknowledge */
+#define CAN_MSR_ERRI ((uint32_t)0x00000004) /*!< Error Interrupt */
+#define CAN_MSR_WKUI ((uint32_t)0x00000008) /*!< Wakeup Interrupt */
+#define CAN_MSR_SLAKI ((uint32_t)0x00000010) /*!< Sleep Acknowledge Interrupt */
+#define CAN_MSR_TXM ((uint32_t)0x00000100) /*!< Transmit Mode */
+#define CAN_MSR_RXM ((uint32_t)0x00000200) /*!< Receive Mode */
+#define CAN_MSR_SAMP ((uint32_t)0x00000400) /*!< Last Sample Point */
+#define CAN_MSR_RX ((uint32_t)0x00000800) /*!< CAN Rx Signal */
+
+/******************* Bit definition for CAN_TSR register ********************/
+#define CAN_TSR_RQCP0 ((uint32_t)0x00000001) /*!< Request Completed Mailbox0 */
+#define CAN_TSR_TXOK0 ((uint32_t)0x00000002) /*!< Transmission OK of Mailbox0 */
+#define CAN_TSR_ALST0 ((uint32_t)0x00000004) /*!< Arbitration Lost for Mailbox0 */
+#define CAN_TSR_TERR0 ((uint32_t)0x00000008) /*!< Transmission Error of Mailbox0 */
+#define CAN_TSR_ABRQ0 ((uint32_t)0x00000080) /*!< Abort Request for Mailbox0 */
+#define CAN_TSR_RQCP1 ((uint32_t)0x00000100) /*!< Request Completed Mailbox1 */
+#define CAN_TSR_TXOK1 ((uint32_t)0x00000200) /*!< Transmission OK of Mailbox1 */
+#define CAN_TSR_ALST1 ((uint32_t)0x00000400) /*!< Arbitration Lost for Mailbox1 */
+#define CAN_TSR_TERR1 ((uint32_t)0x00000800) /*!< Transmission Error of Mailbox1 */
+#define CAN_TSR_ABRQ1 ((uint32_t)0x00008000) /*!< Abort Request for Mailbox 1 */
+#define CAN_TSR_RQCP2 ((uint32_t)0x00010000) /*!< Request Completed Mailbox2 */
+#define CAN_TSR_TXOK2 ((uint32_t)0x00020000) /*!< Transmission OK of Mailbox 2 */
+#define CAN_TSR_ALST2 ((uint32_t)0x00040000) /*!< Arbitration Lost for mailbox 2 */
+#define CAN_TSR_TERR2 ((uint32_t)0x00080000) /*!< Transmission Error of Mailbox 2 */
+#define CAN_TSR_ABRQ2 ((uint32_t)0x00800000) /*!< Abort Request for Mailbox 2 */
+#define CAN_TSR_CODE ((uint32_t)0x03000000) /*!< Mailbox Code */
+
+#define CAN_TSR_TME ((uint32_t)0x1C000000) /*!< TME[2:0] bits */
+#define CAN_TSR_TME0 ((uint32_t)0x04000000) /*!< Transmit Mailbox 0 Empty */
+#define CAN_TSR_TME1 ((uint32_t)0x08000000) /*!< Transmit Mailbox 1 Empty */
+#define CAN_TSR_TME2 ((uint32_t)0x10000000) /*!< Transmit Mailbox 2 Empty */
+
+#define CAN_TSR_LOW ((uint32_t)0xE0000000) /*!< LOW[2:0] bits */
+#define CAN_TSR_LOW0 ((uint32_t)0x20000000) /*!< Lowest Priority Flag for Mailbox 0 */
+#define CAN_TSR_LOW1 ((uint32_t)0x40000000) /*!< Lowest Priority Flag for Mailbox 1 */
+#define CAN_TSR_LOW2 ((uint32_t)0x80000000) /*!< Lowest Priority Flag for Mailbox 2 */
+
+/******************* Bit definition for CAN_RF0R register *******************/
+#define CAN_RF0R_FMP0 ((uint32_t)0x00000003) /*!< FIFO 0 Message Pending */
+#define CAN_RF0R_FULL0 ((uint32_t)0x00000008) /*!< FIFO 0 Full */
+#define CAN_RF0R_FOVR0 ((uint32_t)0x00000010) /*!< FIFO 0 Overrun */
+#define CAN_RF0R_RFOM0 ((uint32_t)0x00000020) /*!< Release FIFO 0 Output Mailbox */
+
+/******************* Bit definition for CAN_RF1R register *******************/
+#define CAN_RF1R_FMP1 ((uint32_t)0x00000003) /*!< FIFO 1 Message Pending */
+#define CAN_RF1R_FULL1 ((uint32_t)0x00000008) /*!< FIFO 1 Full */
+#define CAN_RF1R_FOVR1 ((uint32_t)0x00000010) /*!< FIFO 1 Overrun */
+#define CAN_RF1R_RFOM1 ((uint32_t)0x00000020) /*!< Release FIFO 1 Output Mailbox */
+
+/******************** Bit definition for CAN_IER register *******************/
+#define CAN_IER_TMEIE ((uint32_t)0x00000001) /*!< Transmit Mailbox Empty Interrupt Enable */
+#define CAN_IER_FMPIE0 ((uint32_t)0x00000002) /*!< FIFO Message Pending Interrupt Enable */
+#define CAN_IER_FFIE0 ((uint32_t)0x00000004) /*!< FIFO Full Interrupt Enable */
+#define CAN_IER_FOVIE0 ((uint32_t)0x00000008) /*!< FIFO Overrun Interrupt Enable */
+#define CAN_IER_FMPIE1 ((uint32_t)0x00000010) /*!< FIFO Message Pending Interrupt Enable */
+#define CAN_IER_FFIE1 ((uint32_t)0x00000020) /*!< FIFO Full Interrupt Enable */
+#define CAN_IER_FOVIE1 ((uint32_t)0x00000040) /*!< FIFO Overrun Interrupt Enable */
+#define CAN_IER_EWGIE ((uint32_t)0x00000100) /*!< Error Warning Interrupt Enable */
+#define CAN_IER_EPVIE ((uint32_t)0x00000200) /*!< Error Passive Interrupt Enable */
+#define CAN_IER_BOFIE ((uint32_t)0x00000400) /*!< Bus-Off Interrupt Enable */
+#define CAN_IER_LECIE ((uint32_t)0x00000800) /*!< Last Error Code Interrupt Enable */
+#define CAN_IER_ERRIE ((uint32_t)0x00008000) /*!< Error Interrupt Enable */
+#define CAN_IER_WKUIE ((uint32_t)0x00010000) /*!< Wakeup Interrupt Enable */
+#define CAN_IER_SLKIE ((uint32_t)0x00020000) /*!< Sleep Interrupt Enable */
+
+/******************** Bit definition for CAN_ESR register *******************/
+#define CAN_ESR_EWGF ((uint32_t)0x00000001) /*!< Error Warning Flag */
+#define CAN_ESR_EPVF ((uint32_t)0x00000002) /*!< Error Passive Flag */
+#define CAN_ESR_BOFF ((uint32_t)0x00000004) /*!< Bus-Off Flag */
+
+#define CAN_ESR_LEC ((uint32_t)0x00000070) /*!< LEC[2:0] bits (Last Error Code) */
+#define CAN_ESR_LEC_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define CAN_ESR_LEC_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+#define CAN_ESR_LEC_2 ((uint32_t)0x00000040) /*!< Bit 2 */
+
+#define CAN_ESR_TEC ((uint32_t)0x00FF0000) /*!< Least significant byte of the 9-bit Transmit Error Counter */
+#define CAN_ESR_REC ((uint32_t)0xFF000000) /*!< Receive Error Counter */
+
+/******************* Bit definition for CAN_BTR register ********************/
+#define CAN_BTR_BRP ((uint32_t)0x000003FF) /*!<Baud Rate Prescaler */
+#define CAN_BTR_TS1 ((uint32_t)0x000F0000) /*!<Time Segment 1 */
+#define CAN_BTR_TS1_0 ((uint32_t)0x00010000) /*!<Time Segment 1 (Bit 0) */
+#define CAN_BTR_TS1_1 ((uint32_t)0x00020000) /*!<Time Segment 1 (Bit 1) */
+#define CAN_BTR_TS1_2 ((uint32_t)0x00040000) /*!<Time Segment 1 (Bit 2) */
+#define CAN_BTR_TS1_3 ((uint32_t)0x00080000) /*!<Time Segment 1 (Bit 3) */
+#define CAN_BTR_TS2 ((uint32_t)0x00700000) /*!<Time Segment 2 */
+#define CAN_BTR_TS2_0 ((uint32_t)0x00100000) /*!<Time Segment 2 (Bit 0) */
+#define CAN_BTR_TS2_1 ((uint32_t)0x00200000) /*!<Time Segment 2 (Bit 1) */
+#define CAN_BTR_TS2_2 ((uint32_t)0x00400000) /*!<Time Segment 2 (Bit 2) */
+#define CAN_BTR_SJW ((uint32_t)0x03000000) /*!<Resynchronization Jump Width */
+#define CAN_BTR_SJW_0 ((uint32_t)0x01000000) /*!<Resynchronization Jump Width (Bit 0) */
+#define CAN_BTR_SJW_1 ((uint32_t)0x02000000) /*!<Resynchronization Jump Width (Bit 1) */
+#define CAN_BTR_LBKM ((uint32_t)0x40000000) /*!<Loop Back Mode (Debug) */
+#define CAN_BTR_SILM ((uint32_t)0x80000000) /*!<Silent Mode */
+
+/*!< Mailbox registers */
+/****************** Bit definition for CAN_TI0R register ********************/
+#define CAN_TI0R_TXRQ ((uint32_t)0x00000001) /*!< Transmit Mailbox Request */
+#define CAN_TI0R_RTR ((uint32_t)0x00000002) /*!< Remote Transmission Request */
+#define CAN_TI0R_IDE ((uint32_t)0x00000004) /*!< Identifier Extension */
+#define CAN_TI0R_EXID ((uint32_t)0x001FFFF8) /*!< Extended Identifier */
+#define CAN_TI0R_STID ((uint32_t)0xFFE00000) /*!< Standard Identifier or Extended Identifier */
+
+/****************** Bit definition for CAN_TDT0R register *******************/
+#define CAN_TDT0R_DLC ((uint32_t)0x0000000F) /*!< Data Length Code */
+#define CAN_TDT0R_TGT ((uint32_t)0x00000100) /*!< Transmit Global Time */
+#define CAN_TDT0R_TIME ((uint32_t)0xFFFF0000) /*!< Message Time Stamp */
+
+/****************** Bit definition for CAN_TDL0R register *******************/
+#define CAN_TDL0R_DATA0 ((uint32_t)0x000000FF) /*!< Data byte 0 */
+#define CAN_TDL0R_DATA1 ((uint32_t)0x0000FF00) /*!< Data byte 1 */
+#define CAN_TDL0R_DATA2 ((uint32_t)0x00FF0000) /*!< Data byte 2 */
+#define CAN_TDL0R_DATA3 ((uint32_t)0xFF000000) /*!< Data byte 3 */
+
+/****************** Bit definition for CAN_TDH0R register *******************/
+#define CAN_TDH0R_DATA4 ((uint32_t)0x000000FF) /*!< Data byte 4 */
+#define CAN_TDH0R_DATA5 ((uint32_t)0x0000FF00) /*!< Data byte 5 */
+#define CAN_TDH0R_DATA6 ((uint32_t)0x00FF0000) /*!< Data byte 6 */
+#define CAN_TDH0R_DATA7 ((uint32_t)0xFF000000) /*!< Data byte 7 */
+
+/******************* Bit definition for CAN_TI1R register *******************/
+#define CAN_TI1R_TXRQ ((uint32_t)0x00000001) /*!< Transmit Mailbox Request */
+#define CAN_TI1R_RTR ((uint32_t)0x00000002) /*!< Remote Transmission Request */
+#define CAN_TI1R_IDE ((uint32_t)0x00000004) /*!< Identifier Extension */
+#define CAN_TI1R_EXID ((uint32_t)0x001FFFF8) /*!< Extended Identifier */
+#define CAN_TI1R_STID ((uint32_t)0xFFE00000) /*!< Standard Identifier or Extended Identifier */
+
+/******************* Bit definition for CAN_TDT1R register ******************/
+#define CAN_TDT1R_DLC ((uint32_t)0x0000000F) /*!< Data Length Code */
+#define CAN_TDT1R_TGT ((uint32_t)0x00000100) /*!< Transmit Global Time */
+#define CAN_TDT1R_TIME ((uint32_t)0xFFFF0000) /*!< Message Time Stamp */
+
+/******************* Bit definition for CAN_TDL1R register ******************/
+#define CAN_TDL1R_DATA0 ((uint32_t)0x000000FF) /*!< Data byte 0 */
+#define CAN_TDL1R_DATA1 ((uint32_t)0x0000FF00) /*!< Data byte 1 */
+#define CAN_TDL1R_DATA2 ((uint32_t)0x00FF0000) /*!< Data byte 2 */
+#define CAN_TDL1R_DATA3 ((uint32_t)0xFF000000) /*!< Data byte 3 */
+
+/******************* Bit definition for CAN_TDH1R register ******************/
+#define CAN_TDH1R_DATA4 ((uint32_t)0x000000FF) /*!< Data byte 4 */
+#define CAN_TDH1R_DATA5 ((uint32_t)0x0000FF00) /*!< Data byte 5 */
+#define CAN_TDH1R_DATA6 ((uint32_t)0x00FF0000) /*!< Data byte 6 */
+#define CAN_TDH1R_DATA7 ((uint32_t)0xFF000000) /*!< Data byte 7 */
+
+/******************* Bit definition for CAN_TI2R register *******************/
+#define CAN_TI2R_TXRQ ((uint32_t)0x00000001) /*!< Transmit Mailbox Request */
+#define CAN_TI2R_RTR ((uint32_t)0x00000002) /*!< Remote Transmission Request */
+#define CAN_TI2R_IDE ((uint32_t)0x00000004) /*!< Identifier Extension */
+#define CAN_TI2R_EXID ((uint32_t)0x001FFFF8) /*!< Extended identifier */
+#define CAN_TI2R_STID ((uint32_t)0xFFE00000) /*!< Standard Identifier or Extended Identifier */
+
+/******************* Bit definition for CAN_TDT2R register ******************/
+#define CAN_TDT2R_DLC ((uint32_t)0x0000000F) /*!< Data Length Code */
+#define CAN_TDT2R_TGT ((uint32_t)0x00000100) /*!< Transmit Global Time */
+#define CAN_TDT2R_TIME ((uint32_t)0xFFFF0000) /*!< Message Time Stamp */
+
+/******************* Bit definition for CAN_TDL2R register ******************/
+#define CAN_TDL2R_DATA0 ((uint32_t)0x000000FF) /*!< Data byte 0 */
+#define CAN_TDL2R_DATA1 ((uint32_t)0x0000FF00) /*!< Data byte 1 */
+#define CAN_TDL2R_DATA2 ((uint32_t)0x00FF0000) /*!< Data byte 2 */
+#define CAN_TDL2R_DATA3 ((uint32_t)0xFF000000) /*!< Data byte 3 */
+
+/******************* Bit definition for CAN_TDH2R register ******************/
+#define CAN_TDH2R_DATA4 ((uint32_t)0x000000FF) /*!< Data byte 4 */
+#define CAN_TDH2R_DATA5 ((uint32_t)0x0000FF00) /*!< Data byte 5 */
+#define CAN_TDH2R_DATA6 ((uint32_t)0x00FF0000) /*!< Data byte 6 */
+#define CAN_TDH2R_DATA7 ((uint32_t)0xFF000000) /*!< Data byte 7 */
+
+/******************* Bit definition for CAN_RI0R register *******************/
+#define CAN_RI0R_RTR ((uint32_t)0x00000002) /*!< Remote Transmission Request */
+#define CAN_RI0R_IDE ((uint32_t)0x00000004) /*!< Identifier Extension */
+#define CAN_RI0R_EXID ((uint32_t)0x001FFFF8) /*!< Extended Identifier */
+#define CAN_RI0R_STID ((uint32_t)0xFFE00000) /*!< Standard Identifier or Extended Identifier */
+
+/******************* Bit definition for CAN_RDT0R register ******************/
+#define CAN_RDT0R_DLC ((uint32_t)0x0000000F) /*!< Data Length Code */
+#define CAN_RDT0R_FMI ((uint32_t)0x0000FF00) /*!< Filter Match Index */
+#define CAN_RDT0R_TIME ((uint32_t)0xFFFF0000) /*!< Message Time Stamp */
+
+/******************* Bit definition for CAN_RDL0R register ******************/
+#define CAN_RDL0R_DATA0 ((uint32_t)0x000000FF) /*!< Data byte 0 */
+#define CAN_RDL0R_DATA1 ((uint32_t)0x0000FF00) /*!< Data byte 1 */
+#define CAN_RDL0R_DATA2 ((uint32_t)0x00FF0000) /*!< Data byte 2 */
+#define CAN_RDL0R_DATA3 ((uint32_t)0xFF000000) /*!< Data byte 3 */
+
+/******************* Bit definition for CAN_RDH0R register ******************/
+#define CAN_RDH0R_DATA4 ((uint32_t)0x000000FF) /*!< Data byte 4 */
+#define CAN_RDH0R_DATA5 ((uint32_t)0x0000FF00) /*!< Data byte 5 */
+#define CAN_RDH0R_DATA6 ((uint32_t)0x00FF0000) /*!< Data byte 6 */
+#define CAN_RDH0R_DATA7 ((uint32_t)0xFF000000) /*!< Data byte 7 */
+
+/******************* Bit definition for CAN_RI1R register *******************/
+#define CAN_RI1R_RTR ((uint32_t)0x00000002) /*!< Remote Transmission Request */
+#define CAN_RI1R_IDE ((uint32_t)0x00000004) /*!< Identifier Extension */
+#define CAN_RI1R_EXID ((uint32_t)0x001FFFF8) /*!< Extended identifier */
+#define CAN_RI1R_STID ((uint32_t)0xFFE00000) /*!< Standard Identifier or Extended Identifier */
+
+/******************* Bit definition for CAN_RDT1R register ******************/
+#define CAN_RDT1R_DLC ((uint32_t)0x0000000F) /*!< Data Length Code */
+#define CAN_RDT1R_FMI ((uint32_t)0x0000FF00) /*!< Filter Match Index */
+#define CAN_RDT1R_TIME ((uint32_t)0xFFFF0000) /*!< Message Time Stamp */
+
+/******************* Bit definition for CAN_RDL1R register ******************/
+#define CAN_RDL1R_DATA0 ((uint32_t)0x000000FF) /*!< Data byte 0 */
+#define CAN_RDL1R_DATA1 ((uint32_t)0x0000FF00) /*!< Data byte 1 */
+#define CAN_RDL1R_DATA2 ((uint32_t)0x00FF0000) /*!< Data byte 2 */
+#define CAN_RDL1R_DATA3 ((uint32_t)0xFF000000) /*!< Data byte 3 */
+
+/******************* Bit definition for CAN_RDH1R register ******************/
+#define CAN_RDH1R_DATA4 ((uint32_t)0x000000FF) /*!< Data byte 4 */
+#define CAN_RDH1R_DATA5 ((uint32_t)0x0000FF00) /*!< Data byte 5 */
+#define CAN_RDH1R_DATA6 ((uint32_t)0x00FF0000) /*!< Data byte 6 */
+#define CAN_RDH1R_DATA7 ((uint32_t)0xFF000000) /*!< Data byte 7 */
+
+/*!< CAN filter registers */
+/******************* Bit definition for CAN_FMR register ********************/
+#define CAN_FMR_FINIT ((uint32_t)0x00000001) /*!< Filter Init Mode */
+#define CAN_FMR_CAN2SB ((uint32_t)0x00003F00) /*!< CAN2 start bank */
+
+/******************* Bit definition for CAN_FM1R register *******************/
+#define CAN_FM1R_FBM ((uint32_t)0x00003FFF) /*!< Filter Mode */
+#define CAN_FM1R_FBM0 ((uint32_t)0x00000001) /*!< Filter Init Mode for filter 0 */
+#define CAN_FM1R_FBM1 ((uint32_t)0x00000002) /*!< Filter Init Mode for filter 1 */
+#define CAN_FM1R_FBM2 ((uint32_t)0x00000004) /*!< Filter Init Mode for filter 2 */
+#define CAN_FM1R_FBM3 ((uint32_t)0x00000008) /*!< Filter Init Mode for filter 3 */
+#define CAN_FM1R_FBM4 ((uint32_t)0x00000010) /*!< Filter Init Mode for filter 4 */
+#define CAN_FM1R_FBM5 ((uint32_t)0x00000020) /*!< Filter Init Mode for filter 5 */
+#define CAN_FM1R_FBM6 ((uint32_t)0x00000040) /*!< Filter Init Mode for filter 6 */
+#define CAN_FM1R_FBM7 ((uint32_t)0x00000080) /*!< Filter Init Mode for filter 7 */
+#define CAN_FM1R_FBM8 ((uint32_t)0x00000100) /*!< Filter Init Mode for filter 8 */
+#define CAN_FM1R_FBM9 ((uint32_t)0x00000200) /*!< Filter Init Mode for filter 9 */
+#define CAN_FM1R_FBM10 ((uint32_t)0x00000400) /*!< Filter Init Mode for filter 10 */
+#define CAN_FM1R_FBM11 ((uint32_t)0x00000800) /*!< Filter Init Mode for filter 11 */
+#define CAN_FM1R_FBM12 ((uint32_t)0x00001000) /*!< Filter Init Mode for filter 12 */
+#define CAN_FM1R_FBM13 ((uint32_t)0x00002000) /*!< Filter Init Mode for filter 13 */
+
+/******************* Bit definition for CAN_FS1R register *******************/
+#define CAN_FS1R_FSC ((uint32_t)0x00003FFF) /*!< Filter Scale Configuration */
+#define CAN_FS1R_FSC0 ((uint32_t)0x00000001) /*!< Filter Scale Configuration for filter 0 */
+#define CAN_FS1R_FSC1 ((uint32_t)0x00000002) /*!< Filter Scale Configuration for filter 1 */
+#define CAN_FS1R_FSC2 ((uint32_t)0x00000004) /*!< Filter Scale Configuration for filter 2 */
+#define CAN_FS1R_FSC3 ((uint32_t)0x00000008) /*!< Filter Scale Configuration for filter 3 */
+#define CAN_FS1R_FSC4 ((uint32_t)0x00000010) /*!< Filter Scale Configuration for filter 4 */
+#define CAN_FS1R_FSC5 ((uint32_t)0x00000020) /*!< Filter Scale Configuration for filter 5 */
+#define CAN_FS1R_FSC6 ((uint32_t)0x00000040) /*!< Filter Scale Configuration for filter 6 */
+#define CAN_FS1R_FSC7 ((uint32_t)0x00000080) /*!< Filter Scale Configuration for filter 7 */
+#define CAN_FS1R_FSC8 ((uint32_t)0x00000100) /*!< Filter Scale Configuration for filter 8 */
+#define CAN_FS1R_FSC9 ((uint32_t)0x00000200) /*!< Filter Scale Configuration for filter 9 */
+#define CAN_FS1R_FSC10 ((uint32_t)0x00000400) /*!< Filter Scale Configuration for filter 10 */
+#define CAN_FS1R_FSC11 ((uint32_t)0x00000800) /*!< Filter Scale Configuration for filter 11 */
+#define CAN_FS1R_FSC12 ((uint32_t)0x00001000) /*!< Filter Scale Configuration for filter 12 */
+#define CAN_FS1R_FSC13 ((uint32_t)0x00002000) /*!< Filter Scale Configuration for filter 13 */
+
+/****************** Bit definition for CAN_FFA1R register *******************/
+#define CAN_FFA1R_FFA ((uint32_t)0x00003FFF) /*!< Filter FIFO Assignment */
+#define CAN_FFA1R_FFA0 ((uint32_t)0x00000001) /*!< Filter FIFO Assignment for filter 0 */
+#define CAN_FFA1R_FFA1 ((uint32_t)0x00000002) /*!< Filter FIFO Assignment for filter 1 */
+#define CAN_FFA1R_FFA2 ((uint32_t)0x00000004) /*!< Filter FIFO Assignment for filter 2 */
+#define CAN_FFA1R_FFA3 ((uint32_t)0x00000008) /*!< Filter FIFO Assignment for filter 3 */
+#define CAN_FFA1R_FFA4 ((uint32_t)0x00000010) /*!< Filter FIFO Assignment for filter 4 */
+#define CAN_FFA1R_FFA5 ((uint32_t)0x00000020) /*!< Filter FIFO Assignment for filter 5 */
+#define CAN_FFA1R_FFA6 ((uint32_t)0x00000040) /*!< Filter FIFO Assignment for filter 6 */
+#define CAN_FFA1R_FFA7 ((uint32_t)0x00000080) /*!< Filter FIFO Assignment for filter 7 */
+#define CAN_FFA1R_FFA8 ((uint32_t)0x00000100) /*!< Filter FIFO Assignment for filter 8 */
+#define CAN_FFA1R_FFA9 ((uint32_t)0x00000200) /*!< Filter FIFO Assignment for filter 9 */
+#define CAN_FFA1R_FFA10 ((uint32_t)0x00000400) /*!< Filter FIFO Assignment for filter 10 */
+#define CAN_FFA1R_FFA11 ((uint32_t)0x00000800) /*!< Filter FIFO Assignment for filter 11 */
+#define CAN_FFA1R_FFA12 ((uint32_t)0x00001000) /*!< Filter FIFO Assignment for filter 12 */
+#define CAN_FFA1R_FFA13 ((uint32_t)0x00002000) /*!< Filter FIFO Assignment for filter 13 */
+
+/******************* Bit definition for CAN_FA1R register *******************/
+#define CAN_FA1R_FACT ((uint32_t)0x00003FFF) /*!< Filter Active */
+#define CAN_FA1R_FACT0 ((uint32_t)0x00000001) /*!< Filter 0 Active */
+#define CAN_FA1R_FACT1 ((uint32_t)0x00000002) /*!< Filter 1 Active */
+#define CAN_FA1R_FACT2 ((uint32_t)0x00000004) /*!< Filter 2 Active */
+#define CAN_FA1R_FACT3 ((uint32_t)0x00000008) /*!< Filter 3 Active */
+#define CAN_FA1R_FACT4 ((uint32_t)0x00000010) /*!< Filter 4 Active */
+#define CAN_FA1R_FACT5 ((uint32_t)0x00000020) /*!< Filter 5 Active */
+#define CAN_FA1R_FACT6 ((uint32_t)0x00000040) /*!< Filter 6 Active */
+#define CAN_FA1R_FACT7 ((uint32_t)0x00000080) /*!< Filter 7 Active */
+#define CAN_FA1R_FACT8 ((uint32_t)0x00000100) /*!< Filter 8 Active */
+#define CAN_FA1R_FACT9 ((uint32_t)0x00000200) /*!< Filter 9 Active */
+#define CAN_FA1R_FACT10 ((uint32_t)0x00000400) /*!< Filter 10 Active */
+#define CAN_FA1R_FACT11 ((uint32_t)0x00000800) /*!< Filter 11 Active */
+#define CAN_FA1R_FACT12 ((uint32_t)0x00001000) /*!< Filter 12 Active */
+#define CAN_FA1R_FACT13 ((uint32_t)0x00002000) /*!< Filter 13 Active */
+
+/******************* Bit definition for CAN_F0R1 register *******************/
+#define CAN_F0R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F0R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F0R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F0R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F0R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F0R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F0R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F0R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F0R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F0R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F0R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F0R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F0R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F0R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F0R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F0R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F0R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F0R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F0R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F0R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F0R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F0R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F0R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F0R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F0R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F0R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F0R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F0R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F0R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F0R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F0R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F0R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F1R1 register *******************/
+#define CAN_F1R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F1R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F1R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F1R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F1R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F1R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F1R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F1R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F1R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F1R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F1R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F1R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F1R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F1R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F1R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F1R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F1R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F1R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F1R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F1R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F1R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F1R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F1R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F1R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F1R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F1R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F1R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F1R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F1R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F1R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F1R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F1R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F2R1 register *******************/
+#define CAN_F2R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F2R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F2R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F2R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F2R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F2R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F2R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F2R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F2R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F2R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F2R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F2R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F2R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F2R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F2R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F2R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F2R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F2R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F2R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F2R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F2R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F2R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F2R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F2R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F2R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F2R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F2R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F2R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F2R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F2R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F2R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F2R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F3R1 register *******************/
+#define CAN_F3R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F3R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F3R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F3R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F3R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F3R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F3R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F3R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F3R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F3R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F3R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F3R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F3R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F3R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F3R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F3R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F3R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F3R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F3R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F3R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F3R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F3R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F3R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F3R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F3R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F3R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F3R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F3R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F3R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F3R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F3R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F3R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F4R1 register *******************/
+#define CAN_F4R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F4R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F4R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F4R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F4R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F4R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F4R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F4R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F4R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F4R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F4R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F4R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F4R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F4R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F4R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F4R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F4R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F4R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F4R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F4R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F4R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F4R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F4R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F4R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F4R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F4R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F4R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F4R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F4R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F4R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F4R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F4R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F5R1 register *******************/
+#define CAN_F5R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F5R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F5R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F5R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F5R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F5R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F5R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F5R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F5R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F5R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F5R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F5R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F5R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F5R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F5R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F5R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F5R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F5R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F5R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F5R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F5R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F5R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F5R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F5R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F5R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F5R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F5R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F5R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F5R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F5R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F5R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F5R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F6R1 register *******************/
+#define CAN_F6R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F6R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F6R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F6R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F6R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F6R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F6R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F6R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F6R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F6R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F6R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F6R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F6R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F6R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F6R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F6R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F6R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F6R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F6R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F6R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F6R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F6R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F6R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F6R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F6R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F6R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F6R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F6R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F6R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F6R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F6R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F6R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F7R1 register *******************/
+#define CAN_F7R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F7R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F7R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F7R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F7R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F7R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F7R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F7R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F7R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F7R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F7R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F7R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F7R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F7R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F7R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F7R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F7R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F7R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F7R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F7R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F7R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F7R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F7R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F7R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F7R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F7R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F7R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F7R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F7R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F7R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F7R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F7R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F8R1 register *******************/
+#define CAN_F8R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F8R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F8R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F8R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F8R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F8R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F8R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F8R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F8R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F8R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F8R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F8R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F8R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F8R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F8R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F8R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F8R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F8R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F8R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F8R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F8R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F8R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F8R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F8R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F8R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F8R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F8R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F8R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F8R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F8R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F8R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F8R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F9R1 register *******************/
+#define CAN_F9R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F9R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F9R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F9R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F9R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F9R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F9R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F9R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F9R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F9R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F9R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F9R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F9R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F9R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F9R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F9R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F9R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F9R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F9R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F9R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F9R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F9R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F9R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F9R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F9R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F9R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F9R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F9R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F9R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F9R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F9R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F9R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F10R1 register ******************/
+#define CAN_F10R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F10R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F10R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F10R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F10R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F10R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F10R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F10R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F10R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F10R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F10R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F10R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F10R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F10R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F10R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F10R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F10R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F10R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F10R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F10R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F10R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F10R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F10R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F10R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F10R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F10R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F10R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F10R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F10R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F10R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F10R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F10R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F11R1 register ******************/
+#define CAN_F11R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F11R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F11R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F11R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F11R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F11R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F11R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F11R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F11R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F11R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F11R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F11R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F11R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F11R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F11R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F11R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F11R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F11R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F11R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F11R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F11R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F11R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F11R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F11R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F11R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F11R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F11R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F11R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F11R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F11R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F11R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F11R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F12R1 register ******************/
+#define CAN_F12R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F12R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F12R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F12R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F12R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F12R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F12R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F12R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F12R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F12R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F12R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F12R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F12R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F12R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F12R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F12R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F12R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F12R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F12R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F12R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F12R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F12R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F12R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F12R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F12R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F12R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F12R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F12R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F12R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F12R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F12R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F12R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F13R1 register ******************/
+#define CAN_F13R1_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F13R1_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F13R1_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F13R1_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F13R1_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F13R1_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F13R1_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F13R1_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F13R1_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F13R1_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F13R1_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F13R1_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F13R1_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F13R1_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F13R1_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F13R1_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F13R1_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F13R1_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F13R1_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F13R1_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F13R1_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F13R1_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F13R1_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F13R1_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F13R1_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F13R1_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F13R1_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F13R1_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F13R1_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F13R1_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F13R1_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F13R1_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F0R2 register *******************/
+#define CAN_F0R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F0R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F0R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F0R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F0R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F0R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F0R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F0R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F0R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F0R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F0R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F0R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F0R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F0R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F0R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F0R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F0R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F0R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F0R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F0R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F0R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F0R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F0R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F0R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F0R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F0R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F0R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F0R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F0R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F0R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F0R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F0R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F1R2 register *******************/
+#define CAN_F1R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F1R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F1R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F1R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F1R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F1R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F1R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F1R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F1R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F1R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F1R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F1R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F1R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F1R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F1R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F1R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F1R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F1R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F1R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F1R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F1R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F1R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F1R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F1R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F1R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F1R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F1R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F1R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F1R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F1R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F1R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F1R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F2R2 register *******************/
+#define CAN_F2R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F2R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F2R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F2R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F2R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F2R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F2R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F2R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F2R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F2R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F2R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F2R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F2R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F2R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F2R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F2R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F2R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F2R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F2R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F2R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F2R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F2R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F2R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F2R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F2R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F2R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F2R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F2R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F2R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F2R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F2R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F2R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F3R2 register *******************/
+#define CAN_F3R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F3R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F3R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F3R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F3R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F3R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F3R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F3R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F3R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F3R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F3R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F3R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F3R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F3R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F3R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F3R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F3R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F3R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F3R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F3R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F3R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F3R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F3R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F3R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F3R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F3R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F3R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F3R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F3R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F3R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F3R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F3R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F4R2 register *******************/
+#define CAN_F4R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F4R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F4R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F4R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F4R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F4R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F4R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F4R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F4R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F4R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F4R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F4R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F4R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F4R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F4R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F4R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F4R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F4R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F4R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F4R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F4R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F4R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F4R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F4R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F4R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F4R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F4R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F4R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F4R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F4R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F4R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F4R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F5R2 register *******************/
+#define CAN_F5R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F5R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F5R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F5R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F5R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F5R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F5R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F5R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F5R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F5R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F5R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F5R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F5R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F5R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F5R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F5R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F5R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F5R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F5R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F5R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F5R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F5R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F5R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F5R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F5R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F5R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F5R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F5R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F5R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F5R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F5R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F5R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F6R2 register *******************/
+#define CAN_F6R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F6R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F6R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F6R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F6R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F6R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F6R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F6R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F6R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F6R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F6R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F6R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F6R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F6R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F6R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F6R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F6R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F6R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F6R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F6R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F6R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F6R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F6R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F6R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F6R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F6R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F6R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F6R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F6R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F6R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F6R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F6R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F7R2 register *******************/
+#define CAN_F7R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F7R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F7R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F7R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F7R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F7R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F7R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F7R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F7R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F7R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F7R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F7R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F7R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F7R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F7R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F7R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F7R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F7R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F7R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F7R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F7R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F7R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F7R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F7R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F7R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F7R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F7R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F7R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F7R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F7R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F7R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F7R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F8R2 register *******************/
+#define CAN_F8R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F8R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F8R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F8R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F8R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F8R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F8R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F8R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F8R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F8R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F8R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F8R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F8R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F8R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F8R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F8R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F8R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F8R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F8R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F8R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F8R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F8R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F8R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F8R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F8R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F8R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F8R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F8R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F8R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F8R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F8R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F8R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F9R2 register *******************/
+#define CAN_F9R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F9R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F9R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F9R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F9R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F9R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F9R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F9R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F9R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F9R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F9R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F9R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F9R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F9R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F9R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F9R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F9R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F9R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F9R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F9R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F9R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F9R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F9R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F9R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F9R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F9R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F9R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F9R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F9R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F9R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F9R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F9R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F10R2 register ******************/
+#define CAN_F10R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F10R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F10R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F10R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F10R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F10R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F10R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F10R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F10R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F10R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F10R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F10R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F10R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F10R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F10R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F10R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F10R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F10R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F10R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F10R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F10R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F10R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F10R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F10R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F10R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F10R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F10R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F10R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F10R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F10R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F10R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F10R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F11R2 register ******************/
+#define CAN_F11R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F11R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F11R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F11R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F11R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F11R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F11R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F11R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F11R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F11R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F11R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F11R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F11R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F11R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F11R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F11R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F11R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F11R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F11R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F11R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F11R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F11R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F11R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F11R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F11R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F11R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F11R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F11R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F11R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F11R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F11R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F11R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F12R2 register ******************/
+#define CAN_F12R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F12R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F12R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F12R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F12R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F12R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F12R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F12R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F12R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F12R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F12R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F12R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F12R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F12R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F12R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F12R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F12R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F12R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F12R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F12R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F12R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F12R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F12R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F12R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F12R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F12R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F12R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F12R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F12R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F12R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F12R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F12R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************* Bit definition for CAN_F13R2 register ******************/
+#define CAN_F13R2_FB0 ((uint32_t)0x00000001) /*!< Filter bit 0 */
+#define CAN_F13R2_FB1 ((uint32_t)0x00000002) /*!< Filter bit 1 */
+#define CAN_F13R2_FB2 ((uint32_t)0x00000004) /*!< Filter bit 2 */
+#define CAN_F13R2_FB3 ((uint32_t)0x00000008) /*!< Filter bit 3 */
+#define CAN_F13R2_FB4 ((uint32_t)0x00000010) /*!< Filter bit 4 */
+#define CAN_F13R2_FB5 ((uint32_t)0x00000020) /*!< Filter bit 5 */
+#define CAN_F13R2_FB6 ((uint32_t)0x00000040) /*!< Filter bit 6 */
+#define CAN_F13R2_FB7 ((uint32_t)0x00000080) /*!< Filter bit 7 */
+#define CAN_F13R2_FB8 ((uint32_t)0x00000100) /*!< Filter bit 8 */
+#define CAN_F13R2_FB9 ((uint32_t)0x00000200) /*!< Filter bit 9 */
+#define CAN_F13R2_FB10 ((uint32_t)0x00000400) /*!< Filter bit 10 */
+#define CAN_F13R2_FB11 ((uint32_t)0x00000800) /*!< Filter bit 11 */
+#define CAN_F13R2_FB12 ((uint32_t)0x00001000) /*!< Filter bit 12 */
+#define CAN_F13R2_FB13 ((uint32_t)0x00002000) /*!< Filter bit 13 */
+#define CAN_F13R2_FB14 ((uint32_t)0x00004000) /*!< Filter bit 14 */
+#define CAN_F13R2_FB15 ((uint32_t)0x00008000) /*!< Filter bit 15 */
+#define CAN_F13R2_FB16 ((uint32_t)0x00010000) /*!< Filter bit 16 */
+#define CAN_F13R2_FB17 ((uint32_t)0x00020000) /*!< Filter bit 17 */
+#define CAN_F13R2_FB18 ((uint32_t)0x00040000) /*!< Filter bit 18 */
+#define CAN_F13R2_FB19 ((uint32_t)0x00080000) /*!< Filter bit 19 */
+#define CAN_F13R2_FB20 ((uint32_t)0x00100000) /*!< Filter bit 20 */
+#define CAN_F13R2_FB21 ((uint32_t)0x00200000) /*!< Filter bit 21 */
+#define CAN_F13R2_FB22 ((uint32_t)0x00400000) /*!< Filter bit 22 */
+#define CAN_F13R2_FB23 ((uint32_t)0x00800000) /*!< Filter bit 23 */
+#define CAN_F13R2_FB24 ((uint32_t)0x01000000) /*!< Filter bit 24 */
+#define CAN_F13R2_FB25 ((uint32_t)0x02000000) /*!< Filter bit 25 */
+#define CAN_F13R2_FB26 ((uint32_t)0x04000000) /*!< Filter bit 26 */
+#define CAN_F13R2_FB27 ((uint32_t)0x08000000) /*!< Filter bit 27 */
+#define CAN_F13R2_FB28 ((uint32_t)0x10000000) /*!< Filter bit 28 */
+#define CAN_F13R2_FB29 ((uint32_t)0x20000000) /*!< Filter bit 29 */
+#define CAN_F13R2_FB30 ((uint32_t)0x40000000) /*!< Filter bit 30 */
+#define CAN_F13R2_FB31 ((uint32_t)0x80000000) /*!< Filter bit 31 */
+
+/******************************************************************************/
+/* */
+/* Serial Peripheral Interface */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for SPI_CR1 register ********************/
+#define SPI_CR1_CPHA ((uint32_t)0x00000001) /*!< Clock Phase */
+#define SPI_CR1_CPOL ((uint32_t)0x00000002) /*!< Clock Polarity */
+#define SPI_CR1_MSTR ((uint32_t)0x00000004) /*!< Master Selection */
+
+#define SPI_CR1_BR ((uint32_t)0x00000038) /*!< BR[2:0] bits (Baud Rate Control) */
+#define SPI_CR1_BR_0 ((uint32_t)0x00000008) /*!< Bit 0 */
+#define SPI_CR1_BR_1 ((uint32_t)0x00000010) /*!< Bit 1 */
+#define SPI_CR1_BR_2 ((uint32_t)0x00000020) /*!< Bit 2 */
+
+#define SPI_CR1_SPE ((uint32_t)0x00000040) /*!< SPI Enable */
+#define SPI_CR1_LSBFIRST ((uint32_t)0x00000080) /*!< Frame Format */
+#define SPI_CR1_SSI ((uint32_t)0x00000100) /*!< Internal slave select */
+#define SPI_CR1_SSM ((uint32_t)0x00000200) /*!< Software slave management */
+#define SPI_CR1_RXONLY ((uint32_t)0x00000400) /*!< Receive only */
+#define SPI_CR1_DFF ((uint32_t)0x00000800) /*!< Data Frame Format */
+#define SPI_CR1_CRCNEXT ((uint32_t)0x00001000) /*!< Transmit CRC next */
+#define SPI_CR1_CRCEN ((uint32_t)0x00002000) /*!< Hardware CRC calculation enable */
+#define SPI_CR1_BIDIOE ((uint32_t)0x00004000) /*!< Output enable in bidirectional mode */
+#define SPI_CR1_BIDIMODE ((uint32_t)0x00008000) /*!< Bidirectional data mode enable */
+
+/******************* Bit definition for SPI_CR2 register ********************/
+#define SPI_CR2_RXDMAEN ((uint32_t)0x00000001) /*!< Rx Buffer DMA Enable */
+#define SPI_CR2_TXDMAEN ((uint32_t)0x00000002) /*!< Tx Buffer DMA Enable */
+#define SPI_CR2_SSOE ((uint32_t)0x00000004) /*!< SS Output Enable */
+#define SPI_CR2_ERRIE ((uint32_t)0x00000020) /*!< Error Interrupt Enable */
+#define SPI_CR2_RXNEIE ((uint32_t)0x00000040) /*!< RX buffer Not Empty Interrupt Enable */
+#define SPI_CR2_TXEIE ((uint32_t)0x00000080) /*!< Tx buffer Empty Interrupt Enable */
+
+/******************** Bit definition for SPI_SR register ********************/
+#define SPI_SR_RXNE ((uint32_t)0x00000001) /*!< Receive buffer Not Empty */
+#define SPI_SR_TXE ((uint32_t)0x00000002) /*!< Transmit buffer Empty */
+#define SPI_SR_CHSIDE ((uint32_t)0x00000004) /*!< Channel side */
+#define SPI_SR_UDR ((uint32_t)0x00000008) /*!< Underrun flag */
+#define SPI_SR_CRCERR ((uint32_t)0x00000010) /*!< CRC Error flag */
+#define SPI_SR_MODF ((uint32_t)0x00000020) /*!< Mode fault */
+#define SPI_SR_OVR ((uint32_t)0x00000040) /*!< Overrun flag */
+#define SPI_SR_BSY ((uint32_t)0x00000080) /*!< Busy flag */
+
+/******************** Bit definition for SPI_DR register ********************/
+#define SPI_DR_DR ((uint32_t)0x0000FFFF) /*!< Data Register */
+
+/******************* Bit definition for SPI_CRCPR register ******************/
+#define SPI_CRCPR_CRCPOLY ((uint32_t)0x0000FFFF) /*!< CRC polynomial register */
+
+/****************** Bit definition for SPI_RXCRCR register ******************/
+#define SPI_RXCRCR_RXCRC ((uint32_t)0x0000FFFF) /*!< Rx CRC Register */
+
+/****************** Bit definition for SPI_TXCRCR register ******************/
+#define SPI_TXCRCR_TXCRC ((uint32_t)0x0000FFFF) /*!< Tx CRC Register */
+
+/****************** Bit definition for SPI_I2SCFGR register *****************/
+#define SPI_I2SCFGR_CHLEN ((uint32_t)0x00000001) /*!< Channel length (number of bits per audio channel) */
+
+#define SPI_I2SCFGR_DATLEN ((uint32_t)0x00000006) /*!< DATLEN[1:0] bits (Data length to be transferred) */
+#define SPI_I2SCFGR_DATLEN_0 ((uint32_t)0x00000002) /*!< Bit 0 */
+#define SPI_I2SCFGR_DATLEN_1 ((uint32_t)0x00000004) /*!< Bit 1 */
+
+#define SPI_I2SCFGR_CKPOL ((uint32_t)0x00000008) /*!< steady state clock polarity */
+
+#define SPI_I2SCFGR_I2SSTD ((uint32_t)0x00000030) /*!< I2SSTD[1:0] bits (I2S standard selection) */
+#define SPI_I2SCFGR_I2SSTD_0 ((uint32_t)0x00000010) /*!< Bit 0 */
+#define SPI_I2SCFGR_I2SSTD_1 ((uint32_t)0x00000020) /*!< Bit 1 */
+
+#define SPI_I2SCFGR_PCMSYNC ((uint32_t)0x00000080) /*!< PCM frame synchronization */
+
+#define SPI_I2SCFGR_I2SCFG ((uint32_t)0x00000300) /*!< I2SCFG[1:0] bits (I2S configuration mode) */
+#define SPI_I2SCFGR_I2SCFG_0 ((uint32_t)0x00000100) /*!< Bit 0 */
+#define SPI_I2SCFGR_I2SCFG_1 ((uint32_t)0x00000200) /*!< Bit 1 */
+
+#define SPI_I2SCFGR_I2SE ((uint32_t)0x00000400) /*!< I2S Enable */
+#define SPI_I2SCFGR_I2SMOD ((uint32_t)0x00000800) /*!< I2S mode selection */
+
+/****************** Bit definition for SPI_I2SPR register *******************/
+#define SPI_I2SPR_I2SDIV ((uint32_t)0x000000FF) /*!< I2S Linear prescaler */
+#define SPI_I2SPR_ODD ((uint32_t)0x00000100) /*!< Odd factor for the prescaler */
+#define SPI_I2SPR_MCKOE ((uint32_t)0x00000200) /*!< Master Clock Output Enable */
+
+/******************************************************************************/
+/* */
+/* Inter-integrated Circuit Interface */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for I2C_CR1 register ********************/
+#define I2C_CR1_PE ((uint32_t)0x00000001) /*!< Peripheral Enable */
+#define I2C_CR1_SMBUS ((uint32_t)0x00000002) /*!< SMBus Mode */
+#define I2C_CR1_SMBTYPE ((uint32_t)0x00000008) /*!< SMBus Type */
+#define I2C_CR1_ENARP ((uint32_t)0x00000010) /*!< ARP Enable */
+#define I2C_CR1_ENPEC ((uint32_t)0x00000020) /*!< PEC Enable */
+#define I2C_CR1_ENGC ((uint32_t)0x00000040) /*!< General Call Enable */
+#define I2C_CR1_NOSTRETCH ((uint32_t)0x00000080) /*!< Clock Stretching Disable (Slave mode) */
+#define I2C_CR1_START ((uint32_t)0x00000100) /*!< Start Generation */
+#define I2C_CR1_STOP ((uint32_t)0x00000200) /*!< Stop Generation */
+#define I2C_CR1_ACK ((uint32_t)0x00000400) /*!< Acknowledge Enable */
+#define I2C_CR1_POS ((uint32_t)0x00000800) /*!< Acknowledge/PEC Position (for data reception) */
+#define I2C_CR1_PEC ((uint32_t)0x00001000) /*!< Packet Error Checking */
+#define I2C_CR1_ALERT ((uint32_t)0x00002000) /*!< SMBus Alert */
+#define I2C_CR1_SWRST ((uint32_t)0x00008000) /*!< Software Reset */
+
+/******************* Bit definition for I2C_CR2 register ********************/
+#define I2C_CR2_FREQ ((uint32_t)0x0000003F) /*!< FREQ[5:0] bits (Peripheral Clock Frequency) */
+#define I2C_CR2_FREQ_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define I2C_CR2_FREQ_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define I2C_CR2_FREQ_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define I2C_CR2_FREQ_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define I2C_CR2_FREQ_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+#define I2C_CR2_FREQ_5 ((uint32_t)0x00000020) /*!< Bit 5 */
+
+#define I2C_CR2_ITERREN ((uint32_t)0x00000100) /*!< Error Interrupt Enable */
+#define I2C_CR2_ITEVTEN ((uint32_t)0x00000200) /*!< Event Interrupt Enable */
+#define I2C_CR2_ITBUFEN ((uint32_t)0x00000400) /*!< Buffer Interrupt Enable */
+#define I2C_CR2_DMAEN ((uint32_t)0x00000800) /*!< DMA Requests Enable */
+#define I2C_CR2_LAST ((uint32_t)0x00001000) /*!< DMA Last Transfer */
+
+/******************* Bit definition for I2C_OAR1 register *******************/
+#define I2C_OAR1_ADD1_7 ((uint32_t)0x000000FE) /*!< Interface Address */
+#define I2C_OAR1_ADD8_9 ((uint32_t)0x00000300) /*!< Interface Address */
+
+#define I2C_OAR1_ADD0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define I2C_OAR1_ADD1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define I2C_OAR1_ADD2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define I2C_OAR1_ADD3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define I2C_OAR1_ADD4 ((uint32_t)0x00000010) /*!< Bit 4 */
+#define I2C_OAR1_ADD5 ((uint32_t)0x00000020) /*!< Bit 5 */
+#define I2C_OAR1_ADD6 ((uint32_t)0x00000040) /*!< Bit 6 */
+#define I2C_OAR1_ADD7 ((uint32_t)0x00000080) /*!< Bit 7 */
+#define I2C_OAR1_ADD8 ((uint32_t)0x00000100) /*!< Bit 8 */
+#define I2C_OAR1_ADD9 ((uint32_t)0x00000200) /*!< Bit 9 */
+
+#define I2C_OAR1_ADDMODE ((uint32_t)0x00008000) /*!< Addressing Mode (Slave mode) */
+
+/******************* Bit definition for I2C_OAR2 register *******************/
+#define I2C_OAR2_ENDUAL ((uint32_t)0x00000001) /*!< Dual addressing mode enable */
+#define I2C_OAR2_ADD2 ((uint32_t)0x000000FE) /*!< Interface address */
+
+/******************* Bit definition for I2C_SR1 register ********************/
+#define I2C_SR1_SB ((uint32_t)0x00000001) /*!< Start Bit (Master mode) */
+#define I2C_SR1_ADDR ((uint32_t)0x00000002) /*!< Address sent (master mode)/matched (slave mode) */
+#define I2C_SR1_BTF ((uint32_t)0x00000004) /*!< Byte Transfer Finished */
+#define I2C_SR1_ADD10 ((uint32_t)0x00000008) /*!< 10-bit header sent (Master mode) */
+#define I2C_SR1_STOPF ((uint32_t)0x00000010) /*!< Stop detection (Slave mode) */
+#define I2C_SR1_RXNE ((uint32_t)0x00000040) /*!< Data Register not Empty (receivers) */
+#define I2C_SR1_TXE ((uint32_t)0x00000080) /*!< Data Register Empty (transmitters) */
+#define I2C_SR1_BERR ((uint32_t)0x00000100) /*!< Bus Error */
+#define I2C_SR1_ARLO ((uint32_t)0x00000200) /*!< Arbitration Lost (master mode) */
+#define I2C_SR1_AF ((uint32_t)0x00000400) /*!< Acknowledge Failure */
+#define I2C_SR1_OVR ((uint32_t)0x00000800) /*!< Overrun/Underrun */
+#define I2C_SR1_PECERR ((uint32_t)0x00001000) /*!< PEC Error in reception */
+#define I2C_SR1_TIMEOUT ((uint32_t)0x00004000) /*!< Timeout or Tlow Error */
+#define I2C_SR1_SMBALERT ((uint32_t)0x00008000) /*!< SMBus Alert */
+
+/******************* Bit definition for I2C_SR2 register ********************/
+#define I2C_SR2_MSL ((uint32_t)0x00000001) /*!< Master/Slave */
+#define I2C_SR2_BUSY ((uint32_t)0x00000002) /*!< Bus Busy */
+#define I2C_SR2_TRA ((uint32_t)0x00000004) /*!< Transmitter/Receiver */
+#define I2C_SR2_GENCALL ((uint32_t)0x00000010) /*!< General Call Address (Slave mode) */
+#define I2C_SR2_SMBDEFAULT ((uint32_t)0x00000020) /*!< SMBus Device Default Address (Slave mode) */
+#define I2C_SR2_SMBHOST ((uint32_t)0x00000040) /*!< SMBus Host Header (Slave mode) */
+#define I2C_SR2_DUALF ((uint32_t)0x00000080) /*!< Dual Flag (Slave mode) */
+#define I2C_SR2_PEC ((uint32_t)0x0000FF00) /*!< Packet Error Checking Register */
+
+/******************* Bit definition for I2C_CCR register ********************/
+#define I2C_CCR_CCR ((uint32_t)0x00000FFF) /*!< Clock Control Register in Fast/Standard mode (Master mode) */
+#define I2C_CCR_DUTY ((uint32_t)0x00004000) /*!< Fast Mode Duty Cycle */
+#define I2C_CCR_FS ((uint32_t)0x00008000) /*!< I2C Master Mode Selection */
+
+/****************** Bit definition for I2C_TRISE register *******************/
+#define I2C_TRISE_TRISE ((uint32_t)0x0000003F) /*!< Maximum Rise Time in Fast/Standard mode (Master mode) */
+
+/******************************************************************************/
+/* */
+/* Universal Synchronous Asynchronous Receiver Transmitter */
+/* */
+/******************************************************************************/
+
+/******************* Bit definition for USART_SR register *******************/
+#define USART_SR_PE ((uint32_t)0x00000001) /*!< Parity Error */
+#define USART_SR_FE ((uint32_t)0x00000002) /*!< Framing Error */
+#define USART_SR_NE ((uint32_t)0x00000004) /*!< Noise Error Flag */
+#define USART_SR_ORE ((uint32_t)0x00000008) /*!< OverRun Error */
+#define USART_SR_IDLE ((uint32_t)0x00000010) /*!< IDLE line detected */
+#define USART_SR_RXNE ((uint32_t)0x00000020) /*!< Read Data Register Not Empty */
+#define USART_SR_TC ((uint32_t)0x00000040) /*!< Transmission Complete */
+#define USART_SR_TXE ((uint32_t)0x00000080) /*!< Transmit Data Register Empty */
+#define USART_SR_LBD ((uint32_t)0x00000100) /*!< LIN Break Detection Flag */
+#define USART_SR_CTS ((uint32_t)0x00000200) /*!< CTS Flag */
+
+/******************* Bit definition for USART_DR register *******************/
+#define USART_DR_DR ((uint32_t)0x000001FF) /*!< Data value */
+
+/****************** Bit definition for USART_BRR register *******************/
+#define USART_BRR_DIV_Fraction ((uint32_t)0x0000000F) /*!< Fraction of USARTDIV */
+#define USART_BRR_DIV_Mantissa ((uint32_t)0x0000FFF0) /*!< Mantissa of USARTDIV */
+
+/****************** Bit definition for USART_CR1 register *******************/
+#define USART_CR1_SBK ((uint32_t)0x00000001) /*!< Send Break */
+#define USART_CR1_RWU ((uint32_t)0x00000002) /*!< Receiver wakeup */
+#define USART_CR1_RE ((uint32_t)0x00000004) /*!< Receiver Enable */
+#define USART_CR1_TE ((uint32_t)0x00000008) /*!< Transmitter Enable */
+#define USART_CR1_IDLEIE ((uint32_t)0x00000010) /*!< IDLE Interrupt Enable */
+#define USART_CR1_RXNEIE ((uint32_t)0x00000020) /*!< RXNE Interrupt Enable */
+#define USART_CR1_TCIE ((uint32_t)0x00000040) /*!< Transmission Complete Interrupt Enable */
+#define USART_CR1_TXEIE ((uint32_t)0x00000080) /*!< PE Interrupt Enable */
+#define USART_CR1_PEIE ((uint32_t)0x00000100) /*!< PE Interrupt Enable */
+#define USART_CR1_PS ((uint32_t)0x00000200) /*!< Parity Selection */
+#define USART_CR1_PCE ((uint32_t)0x00000400) /*!< Parity Control Enable */
+#define USART_CR1_WAKE ((uint32_t)0x00000800) /*!< Wakeup method */
+#define USART_CR1_M ((uint32_t)0x00001000) /*!< Word length */
+#define USART_CR1_UE ((uint32_t)0x00002000) /*!< USART Enable */
+
+/****************** Bit definition for USART_CR2 register *******************/
+#define USART_CR2_ADD ((uint32_t)0x0000000F) /*!< Address of the USART node */
+#define USART_CR2_LBDL ((uint32_t)0x00000020) /*!< LIN Break Detection Length */
+#define USART_CR2_LBDIE ((uint32_t)0x00000040) /*!< LIN Break Detection Interrupt Enable */
+#define USART_CR2_LBCL ((uint32_t)0x00000100) /*!< Last Bit Clock pulse */
+#define USART_CR2_CPHA ((uint32_t)0x00000200) /*!< Clock Phase */
+#define USART_CR2_CPOL ((uint32_t)0x00000400) /*!< Clock Polarity */
+#define USART_CR2_CLKEN ((uint32_t)0x00000800) /*!< Clock Enable */
+
+#define USART_CR2_STOP ((uint32_t)0x00003000) /*!< STOP[1:0] bits (STOP bits) */
+#define USART_CR2_STOP_0 ((uint32_t)0x00001000) /*!< Bit 0 */
+#define USART_CR2_STOP_1 ((uint32_t)0x00002000) /*!< Bit 1 */
+
+#define USART_CR2_LINEN ((uint32_t)0x00004000) /*!< LIN mode enable */
+
+/****************** Bit definition for USART_CR3 register *******************/
+#define USART_CR3_EIE ((uint32_t)0x00000001) /*!< Error Interrupt Enable */
+#define USART_CR3_IREN ((uint32_t)0x00000002) /*!< IrDA mode Enable */
+#define USART_CR3_IRLP ((uint32_t)0x00000004) /*!< IrDA Low-Power */
+#define USART_CR3_HDSEL ((uint32_t)0x00000008) /*!< Half-Duplex Selection */
+#define USART_CR3_NACK ((uint32_t)0x00000010) /*!< Smartcard NACK enable */
+#define USART_CR3_SCEN ((uint32_t)0x00000020) /*!< Smartcard mode enable */
+#define USART_CR3_DMAR ((uint32_t)0x00000040) /*!< DMA Enable Receiver */
+#define USART_CR3_DMAT ((uint32_t)0x00000080) /*!< DMA Enable Transmitter */
+#define USART_CR3_RTSE ((uint32_t)0x00000100) /*!< RTS Enable */
+#define USART_CR3_CTSE ((uint32_t)0x00000200) /*!< CTS Enable */
+#define USART_CR3_CTSIE ((uint32_t)0x00000400) /*!< CTS Interrupt Enable */
+
+/****************** Bit definition for USART_GTPR register ******************/
+#define USART_GTPR_PSC ((uint32_t)0x000000FF) /*!< PSC[7:0] bits (Prescaler value) */
+#define USART_GTPR_PSC_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define USART_GTPR_PSC_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define USART_GTPR_PSC_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+#define USART_GTPR_PSC_3 ((uint32_t)0x00000008) /*!< Bit 3 */
+#define USART_GTPR_PSC_4 ((uint32_t)0x00000010) /*!< Bit 4 */
+#define USART_GTPR_PSC_5 ((uint32_t)0x00000020) /*!< Bit 5 */
+#define USART_GTPR_PSC_6 ((uint32_t)0x00000040) /*!< Bit 6 */
+#define USART_GTPR_PSC_7 ((uint32_t)0x00000080) /*!< Bit 7 */
+
+#define USART_GTPR_GT ((uint32_t)0x0000FF00) /*!< Guard time value */
+
+/******************************************************************************/
+/* */
+/* Debug MCU */
+/* */
+/******************************************************************************/
+
+/**************** Bit definition for DBGMCU_IDCODE register *****************/
+#define DBGMCU_IDCODE_DEV_ID ((uint32_t)0x00000FFF) /*!< Device Identifier */
+
+#define DBGMCU_IDCODE_REV_ID ((uint32_t)0xFFFF0000) /*!< REV_ID[15:0] bits (Revision Identifier) */
+#define DBGMCU_IDCODE_REV_ID_0 ((uint32_t)0x00010000) /*!< Bit 0 */
+#define DBGMCU_IDCODE_REV_ID_1 ((uint32_t)0x00020000) /*!< Bit 1 */
+#define DBGMCU_IDCODE_REV_ID_2 ((uint32_t)0x00040000) /*!< Bit 2 */
+#define DBGMCU_IDCODE_REV_ID_3 ((uint32_t)0x00080000) /*!< Bit 3 */
+#define DBGMCU_IDCODE_REV_ID_4 ((uint32_t)0x00100000) /*!< Bit 4 */
+#define DBGMCU_IDCODE_REV_ID_5 ((uint32_t)0x00200000) /*!< Bit 5 */
+#define DBGMCU_IDCODE_REV_ID_6 ((uint32_t)0x00400000) /*!< Bit 6 */
+#define DBGMCU_IDCODE_REV_ID_7 ((uint32_t)0x00800000) /*!< Bit 7 */
+#define DBGMCU_IDCODE_REV_ID_8 ((uint32_t)0x01000000) /*!< Bit 8 */
+#define DBGMCU_IDCODE_REV_ID_9 ((uint32_t)0x02000000) /*!< Bit 9 */
+#define DBGMCU_IDCODE_REV_ID_10 ((uint32_t)0x04000000) /*!< Bit 10 */
+#define DBGMCU_IDCODE_REV_ID_11 ((uint32_t)0x08000000) /*!< Bit 11 */
+#define DBGMCU_IDCODE_REV_ID_12 ((uint32_t)0x10000000) /*!< Bit 12 */
+#define DBGMCU_IDCODE_REV_ID_13 ((uint32_t)0x20000000) /*!< Bit 13 */
+#define DBGMCU_IDCODE_REV_ID_14 ((uint32_t)0x40000000) /*!< Bit 14 */
+#define DBGMCU_IDCODE_REV_ID_15 ((uint32_t)0x80000000) /*!< Bit 15 */
+
+/****************** Bit definition for DBGMCU_CR register *******************/
+#define DBGMCU_CR_DBG_SLEEP ((uint32_t)0x00000001) /*!< Debug Sleep Mode */
+#define DBGMCU_CR_DBG_STOP ((uint32_t)0x00000002) /*!< Debug Stop Mode */
+#define DBGMCU_CR_DBG_STANDBY ((uint32_t)0x00000004) /*!< Debug Standby mode */
+#define DBGMCU_CR_TRACE_IOEN ((uint32_t)0x00000020) /*!< Trace Pin Assignment Control */
+
+#define DBGMCU_CR_TRACE_MODE ((uint32_t)0x000000C0) /*!< TRACE_MODE[1:0] bits (Trace Pin Assignment Control) */
+#define DBGMCU_CR_TRACE_MODE_0 ((uint32_t)0x00000040) /*!< Bit 0 */
+#define DBGMCU_CR_TRACE_MODE_1 ((uint32_t)0x00000080) /*!< Bit 1 */
+
+#define DBGMCU_CR_DBG_IWDG_STOP ((uint32_t)0x00000100) /*!< Debug Independent Watchdog stopped when Core is halted */
+#define DBGMCU_CR_DBG_WWDG_STOP ((uint32_t)0x00000200) /*!< Debug Window Watchdog stopped when Core is halted */
+#define DBGMCU_CR_DBG_TIM1_STOP ((uint32_t)0x00000400) /*!< TIM1 counter stopped when core is halted */
+#define DBGMCU_CR_DBG_TIM2_STOP ((uint32_t)0x00000800) /*!< TIM2 counter stopped when core is halted */
+#define DBGMCU_CR_DBG_TIM3_STOP ((uint32_t)0x00001000) /*!< TIM3 counter stopped when core is halted */
+#define DBGMCU_CR_DBG_TIM4_STOP ((uint32_t)0x00002000) /*!< TIM4 counter stopped when core is halted */
+#define DBGMCU_CR_DBG_CAN1_STOP ((uint32_t)0x00004000) /*!< Debug CAN1 stopped when Core is halted */
+#define DBGMCU_CR_DBG_I2C1_SMBUS_TIMEOUT ((uint32_t)0x00008000) /*!< SMBUS timeout mode stopped when Core is halted */
+#define DBGMCU_CR_DBG_I2C2_SMBUS_TIMEOUT ((uint32_t)0x00010000) /*!< SMBUS timeout mode stopped when Core is halted */
+#define DBGMCU_CR_DBG_TIM8_STOP ((uint32_t)0x00020000) /*!< TIM8 counter stopped when core is halted */
+#define DBGMCU_CR_DBG_TIM5_STOP ((uint32_t)0x00040000) /*!< TIM5 counter stopped when core is halted */
+#define DBGMCU_CR_DBG_TIM6_STOP ((uint32_t)0x00080000) /*!< TIM6 counter stopped when core is halted */
+#define DBGMCU_CR_DBG_TIM7_STOP ((uint32_t)0x00100000) /*!< TIM7 counter stopped when core is halted */
+
+/******************************************************************************/
+/* */
+/* FLASH and Option Bytes Registers */
+/* */
+/******************************************************************************/
+/******************* Bit definition for FLASH_ACR register ******************/
+#define FLASH_ACR_LATENCY ((uint32_t)0x00000007) /*!< LATENCY[2:0] bits (Latency) */
+#define FLASH_ACR_LATENCY_0 ((uint32_t)0x00000001) /*!< Bit 0 */
+#define FLASH_ACR_LATENCY_1 ((uint32_t)0x00000002) /*!< Bit 1 */
+#define FLASH_ACR_LATENCY_2 ((uint32_t)0x00000004) /*!< Bit 2 */
+
+#define FLASH_ACR_HLFCYA ((uint32_t)0x00000008) /*!< Flash Half Cycle Access Enable */
+#define FLASH_ACR_PRFTBE ((uint32_t)0x00000010) /*!< Prefetch Buffer Enable */
+#define FLASH_ACR_PRFTBS ((uint32_t)0x00000020) /*!< Prefetch Buffer Status */
+
+/****************** Bit definition for FLASH_KEYR register ******************/
+#define FLASH_KEYR_FKEYR ((uint32_t)0xFFFFFFFF) /*!< FPEC Key */
+
+#define RDP_KEY ((uint32_t)0x000000A5) /*!< RDP Key */
+#define FLASH_KEY1 ((uint32_t)0x45670123) /*!< FPEC Key1 */
+#define FLASH_KEY2 ((uint32_t)0xCDEF89AB) /*!< FPEC Key2 */
+
+/***************** Bit definition for FLASH_OPTKEYR register ****************/
+#define FLASH_OPTKEYR_OPTKEYR ((uint32_t)0xFFFFFFFF) /*!< Option Byte Key */
+
+#define FLASH_OPTKEY1 FLASH_KEY1 /*!< Option Byte Key1 */
+#define FLASH_OPTKEY2 FLASH_KEY2 /*!< Option Byte Key2 */
+
+/****************** Bit definition for FLASH_SR register ********************/
+#define FLASH_SR_BSY ((uint32_t)0x00000001) /*!< Busy */
+#define FLASH_SR_PGERR ((uint32_t)0x00000004) /*!< Programming Error */
+#define FLASH_SR_WRPRTERR ((uint32_t)0x00000010) /*!< Write Protection Error */
+#define FLASH_SR_EOP ((uint32_t)0x00000020) /*!< End of operation */
+
+/******************* Bit definition for FLASH_CR register *******************/
+#define FLASH_CR_PG ((uint32_t)0x00000001) /*!< Programming */
+#define FLASH_CR_PER ((uint32_t)0x00000002) /*!< Page Erase */
+#define FLASH_CR_MER ((uint32_t)0x00000004) /*!< Mass Erase */
+#define FLASH_CR_OPTPG ((uint32_t)0x00000010) /*!< Option Byte Programming */
+#define FLASH_CR_OPTER ((uint32_t)0x00000020) /*!< Option Byte Erase */
+#define FLASH_CR_STRT ((uint32_t)0x00000040) /*!< Start */
+#define FLASH_CR_LOCK ((uint32_t)0x00000080) /*!< Lock */
+#define FLASH_CR_OPTWRE ((uint32_t)0x00000200) /*!< Option Bytes Write Enable */
+#define FLASH_CR_ERRIE ((uint32_t)0x00000400) /*!< Error Interrupt Enable */
+#define FLASH_CR_EOPIE ((uint32_t)0x00001000) /*!< End of operation interrupt enable */
+
+/******************* Bit definition for FLASH_AR register *******************/
+#define FLASH_AR_FAR ((uint32_t)0xFFFFFFFF) /*!< Flash Address */
+
+/****************** Bit definition for FLASH_OBR register *******************/
+#define FLASH_OBR_OPTERR ((uint32_t)0x00000001) /*!< Option Byte Error */
+#define FLASH_OBR_RDPRT ((uint32_t)0x00000002) /*!< Read protection */
+
+#define FLASH_OBR_IWDG_SW ((uint32_t)0x00000004) /*!< IWDG SW */
+#define FLASH_OBR_nRST_STOP ((uint32_t)0x00000008) /*!< nRST_STOP */
+#define FLASH_OBR_nRST_STDBY ((uint32_t)0x00000010) /*!< nRST_STDBY */
+#define FLASH_OBR_USER ((uint32_t)0x0000001C) /*!< User Option Bytes */
+
+/****************** Bit definition for FLASH_WRPR register ******************/
+#define FLASH_WRPR_WRP ((uint32_t)0xFFFFFFFF) /*!< Write Protect */
+
+/*----------------------------------------------------------------------------*/
+
+/****************** Bit definition for FLASH_RDP register *******************/
+#define FLASH_RDP_RDP ((uint32_t)0x000000FF) /*!< Read protection option byte */
+#define FLASH_RDP_nRDP ((uint32_t)0x0000FF00) /*!< Read protection complemented option byte */
+
+/****************** Bit definition for FLASH_USER register ******************/
+#define FLASH_USER_USER ((uint32_t)0x00FF0000) /*!< User option byte */
+#define FLASH_USER_nUSER ((uint32_t)0xFF000000) /*!< User complemented option byte */
+
+/****************** Bit definition for FLASH_Data0 register *****************/
+#define FLASH_DATA0_DATA0 ((uint32_t)0x000000FF) /*!< User data storage option byte */
+#define FLASH_DATA0_nDATA0 ((uint32_t)0x0000FF00) /*!< User data storage complemented option byte */
+
+/****************** Bit definition for FLASH_Data1 register *****************/
+#define FLASH_DATA1_DATA1 ((uint32_t)0x00FF0000) /*!< User data storage option byte */
+#define FLASH_DATA1_nDATA1 ((uint32_t)0xFF000000) /*!< User data storage complemented option byte */
+
+/****************** Bit definition for FLASH_WRP0 register ******************/
+#define FLASH_WRP0_WRP0 ((uint32_t)0x000000FF) /*!< Flash memory write protection option bytes */
+#define FLASH_WRP0_nWRP0 ((uint32_t)0x0000FF00) /*!< Flash memory write protection complemented option bytes */
+
+/****************** Bit definition for FLASH_WRP1 register ******************/
+#define FLASH_WRP1_WRP1 ((uint32_t)0x00FF0000) /*!< Flash memory write protection option bytes */
+#define FLASH_WRP1_nWRP1 ((uint32_t)0xFF000000) /*!< Flash memory write protection complemented option bytes */
+
+/****************** Bit definition for FLASH_WRP2 register ******************/
+#define FLASH_WRP2_WRP2 ((uint32_t)0x000000FF) /*!< Flash memory write protection option bytes */
+#define FLASH_WRP2_nWRP2 ((uint32_t)0x0000FF00) /*!< Flash memory write protection complemented option bytes */
+
+/****************** Bit definition for FLASH_WRP3 register ******************/
+#define FLASH_WRP3_WRP3 ((uint32_t)0x00FF0000) /*!< Flash memory write protection option bytes */
+#define FLASH_WRP3_nWRP3 ((uint32_t)0xFF000000) /*!< Flash memory write protection complemented option bytes */
+
+
+
+/**
+ * @}
+*/
+
+/**
+ * @}
+*/
+
+/** @addtogroup Exported_macro
+ * @{
+ */
+
+/****************************** ADC Instances *********************************/
+#define IS_ADC_ALL_INSTANCE(INSTANCE) (((INSTANCE) == ADC1) || \
+ ((INSTANCE) == ADC2) || \
+ ((INSTANCE) == ADC3))
+
+#define IS_ADC_MULTIMODE_MASTER_INSTANCE(INSTANCE) ((INSTANCE) == ADC1)
+
+#define IS_ADC_DMA_CAPABILITY_INSTANCE(INSTANCE) (((INSTANCE) == ADC1) || \
+ ((INSTANCE) == ADC3))
+
+/****************************** CAN Instances *********************************/
+#define IS_CAN_ALL_INSTANCE(INSTANCE) ((INSTANCE) == CAN1)
+
+/****************************** CRC Instances *********************************/
+#define IS_CRC_ALL_INSTANCE(INSTANCE) ((INSTANCE) == CRC)
+
+/****************************** DAC Instances *********************************/
+#define IS_DAC_ALL_INSTANCE(INSTANCE) ((INSTANCE) == DAC)
+
+/****************************** DMA Instances *********************************/
+#define IS_DMA_ALL_INSTANCE(INSTANCE) (((INSTANCE) == DMA1_Channel1) || \
+ ((INSTANCE) == DMA1_Channel2) || \
+ ((INSTANCE) == DMA1_Channel3) || \
+ ((INSTANCE) == DMA1_Channel4) || \
+ ((INSTANCE) == DMA1_Channel5) || \
+ ((INSTANCE) == DMA1_Channel6) || \
+ ((INSTANCE) == DMA1_Channel7) || \
+ ((INSTANCE) == DMA2_Channel1) || \
+ ((INSTANCE) == DMA2_Channel2) || \
+ ((INSTANCE) == DMA2_Channel3) || \
+ ((INSTANCE) == DMA2_Channel4) || \
+ ((INSTANCE) == DMA2_Channel5))
+
+/******************************* GPIO Instances *******************************/
+#define IS_GPIO_ALL_INSTANCE(INSTANCE) (((INSTANCE) == GPIOA) || \
+ ((INSTANCE) == GPIOB) || \
+ ((INSTANCE) == GPIOC) || \
+ ((INSTANCE) == GPIOD) || \
+ ((INSTANCE) == GPIOE) || \
+ ((INSTANCE) == GPIOF) || \
+ ((INSTANCE) == GPIOG))
+
+/**************************** GPIO Alternate Function Instances ***************/
+#define IS_GPIO_AF_INSTANCE(INSTANCE) IS_GPIO_ALL_INSTANCE(INSTANCE)
+
+/**************************** GPIO Lock Instances *****************************/
+#define IS_GPIO_LOCK_INSTANCE(INSTANCE) IS_GPIO_ALL_INSTANCE(INSTANCE)
+
+/******************************** I2C Instances *******************************/
+#define IS_I2C_ALL_INSTANCE(INSTANCE) (((INSTANCE) == I2C1) || \
+ ((INSTANCE) == I2C2))
+
+/******************************** I2S Instances *******************************/
+#define IS_I2S_ALL_INSTANCE(INSTANCE) (((INSTANCE) == SPI2) || \
+ ((INSTANCE) == SPI3))
+
+/****************************** IWDG Instances ********************************/
+#define IS_IWDG_ALL_INSTANCE(INSTANCE) ((INSTANCE) == IWDG)
+
+/****************************** SDIO Instances *********************************/
+#define IS_SDIO_ALL_INSTANCE(INSTANCE) ((INSTANCE) == SDIO)
+
+/******************************** SPI Instances *******************************/
+#define IS_SPI_ALL_INSTANCE(INSTANCE) (((INSTANCE) == SPI1) || \
+ ((INSTANCE) == SPI2) || \
+ ((INSTANCE) == SPI3))
+
+/****************************** START TIM Instances ***************************/
+/****************************** TIM Instances *********************************/
+#define IS_TIM_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5) || \
+ ((INSTANCE) == TIM6) || \
+ ((INSTANCE) == TIM7))
+
+#define IS_TIM_CC1_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_CC2_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_CC3_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_CC4_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_CLOCKSOURCE_ETRMODE1_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_CLOCKSOURCE_ETRMODE2_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_CLOCKSOURCE_TIX_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_CLOCKSOURCE_ITRX_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_OCXREF_CLEAR_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_ENCODER_INTERFACE_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_XOR_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_MASTER_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5) || \
+ ((INSTANCE) == TIM6) || \
+ ((INSTANCE) == TIM7))
+
+#define IS_TIM_SLAVE_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_DMABURST_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_BREAK_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8))
+
+#define IS_TIM_CCX_INSTANCE(INSTANCE, CHANNEL) \
+ ((((INSTANCE) == TIM1) && \
+ (((CHANNEL) == TIM_CHANNEL_1) || \
+ ((CHANNEL) == TIM_CHANNEL_2) || \
+ ((CHANNEL) == TIM_CHANNEL_3) || \
+ ((CHANNEL) == TIM_CHANNEL_4))) \
+ || \
+ (((INSTANCE) == TIM8) && \
+ (((CHANNEL) == TIM_CHANNEL_1) || \
+ ((CHANNEL) == TIM_CHANNEL_2) || \
+ ((CHANNEL) == TIM_CHANNEL_3) || \
+ ((CHANNEL) == TIM_CHANNEL_4))) \
+ || \
+ (((INSTANCE) == TIM2) && \
+ (((CHANNEL) == TIM_CHANNEL_1) || \
+ ((CHANNEL) == TIM_CHANNEL_2) || \
+ ((CHANNEL) == TIM_CHANNEL_3) || \
+ ((CHANNEL) == TIM_CHANNEL_4))) \
+ || \
+ (((INSTANCE) == TIM3) && \
+ (((CHANNEL) == TIM_CHANNEL_1) || \
+ ((CHANNEL) == TIM_CHANNEL_2) || \
+ ((CHANNEL) == TIM_CHANNEL_3) || \
+ ((CHANNEL) == TIM_CHANNEL_4))) \
+ || \
+ (((INSTANCE) == TIM4) && \
+ (((CHANNEL) == TIM_CHANNEL_1) || \
+ ((CHANNEL) == TIM_CHANNEL_2) || \
+ ((CHANNEL) == TIM_CHANNEL_3) || \
+ ((CHANNEL) == TIM_CHANNEL_4))) \
+ || \
+ (((INSTANCE) == TIM5) && \
+ (((CHANNEL) == TIM_CHANNEL_1) || \
+ ((CHANNEL) == TIM_CHANNEL_2) || \
+ ((CHANNEL) == TIM_CHANNEL_3) || \
+ ((CHANNEL) == TIM_CHANNEL_4))))
+
+#define IS_TIM_CCXN_INSTANCE(INSTANCE, CHANNEL) \
+ ((((INSTANCE) == TIM1) && \
+ (((CHANNEL) == TIM_CHANNEL_1) || \
+ ((CHANNEL) == TIM_CHANNEL_2) || \
+ ((CHANNEL) == TIM_CHANNEL_3))) \
+ || \
+ (((INSTANCE) == TIM8) && \
+ (((CHANNEL) == TIM_CHANNEL_1) || \
+ ((CHANNEL) == TIM_CHANNEL_2) || \
+ ((CHANNEL) == TIM_CHANNEL_3))))
+
+#define IS_TIM_COUNTER_MODE_SELECT_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_REPETITION_COUNTER_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8))
+
+#define IS_TIM_CLOCK_DIVISION_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_DMA_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5) || \
+ ((INSTANCE) == TIM6) || \
+ ((INSTANCE) == TIM7))
+
+#define IS_TIM_DMA_CC_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8) || \
+ ((INSTANCE) == TIM2) || \
+ ((INSTANCE) == TIM3) || \
+ ((INSTANCE) == TIM4) || \
+ ((INSTANCE) == TIM5))
+
+#define IS_TIM_COMMUTATION_EVENT_INSTANCE(INSTANCE)\
+ (((INSTANCE) == TIM1) || \
+ ((INSTANCE) == TIM8))
+
+/****************************** END TIM Instances *****************************/
+
+
+/******************** USART Instances : Synchronous mode **********************/
+#define IS_USART_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
+ ((INSTANCE) == USART2) || \
+ ((INSTANCE) == USART3))
+
+/******************** UART Instances : Asynchronous mode **********************/
+#define IS_UART_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
+ ((INSTANCE) == USART2) || \
+ ((INSTANCE) == USART3) || \
+ ((INSTANCE) == UART4) || \
+ ((INSTANCE) == UART5))
+
+/******************** UART Instances : Half-Duplex mode **********************/
+#define IS_UART_HALFDUPLEX_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
+ ((INSTANCE) == USART2) || \
+ ((INSTANCE) == USART3) || \
+ ((INSTANCE) == UART4) || \
+ ((INSTANCE) == UART5))
+
+/******************** UART Instances : LIN mode **********************/
+#define IS_UART_LIN_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
+ ((INSTANCE) == USART2) || \
+ ((INSTANCE) == USART3) || \
+ ((INSTANCE) == UART4) || \
+ ((INSTANCE) == UART5))
+
+/****************** UART Instances : Hardware Flow control ********************/
+#define IS_UART_HWFLOW_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
+ ((INSTANCE) == USART2) || \
+ ((INSTANCE) == USART3))
+
+/********************* UART Instances : Smard card mode ***********************/
+#define IS_SMARTCARD_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
+ ((INSTANCE) == USART2) || \
+ ((INSTANCE) == USART3))
+
+/*********************** UART Instances : IRDA mode ***************************/
+#define IS_IRDA_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
+ ((INSTANCE) == USART2) || \
+ ((INSTANCE) == USART3) || \
+ ((INSTANCE) == UART4) || \
+ ((INSTANCE) == UART5))
+
+/***************** UART Instances : Multi-Processor mode **********************/
+#define IS_UART_MULTIPROCESSOR_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
+ ((INSTANCE) == USART2) || \
+ ((INSTANCE) == USART3) || \
+ ((INSTANCE) == UART4) || \
+ ((INSTANCE) == UART5))
+
+/***************** UART Instances : DMA mode available **********************/
+#define IS_UART_DMA_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
+ ((INSTANCE) == USART2) || \
+ ((INSTANCE) == USART3) || \
+ ((INSTANCE) == UART4))
+
+/****************************** RTC Instances *********************************/
+#define IS_RTC_ALL_INSTANCE(INSTANCE) ((INSTANCE) == RTC)
+
+/**************************** WWDG Instances *****************************/
+#define IS_WWDG_ALL_INSTANCE(INSTANCE) ((INSTANCE) == WWDG)
+
+/****************************** USB Instances ********************************/
+#define IS_USB_ALL_INSTANCE(INSTANCE) ((INSTANCE) == USB)
+
+
+
+
+/**
+ * @}
+*/
+/******************************************************************************/
+/* For a painless codes migration between the STM32F1xx device product */
+/* lines, the aliases defined below are put in place to overcome the */
+/* differences in the interrupt handlers and IRQn definitions. */
+/* No need to update developed interrupt code when moving across */
+/* product lines within the same STM32F1 Family */
+/******************************************************************************/
+
+/* Aliases for __IRQn */
+#define ADC1_IRQn ADC1_2_IRQn
+#define DMA2_Channel4_IRQn DMA2_Channel4_5_IRQn
+#define TIM1_BRK_TIM15_IRQn TIM1_BRK_IRQn
+#define TIM1_BRK_TIM9_IRQn TIM1_BRK_IRQn
+#define TIM9_IRQn TIM1_BRK_IRQn
+#define TIM1_TRG_COM_TIM11_IRQn TIM1_TRG_COM_IRQn
+#define TIM1_TRG_COM_TIM17_IRQn TIM1_TRG_COM_IRQn
+#define TIM11_IRQn TIM1_TRG_COM_IRQn
+#define TIM10_IRQn TIM1_UP_IRQn
+#define TIM1_UP_TIM16_IRQn TIM1_UP_IRQn
+#define TIM1_UP_TIM10_IRQn TIM1_UP_IRQn
+#define TIM6_DAC_IRQn TIM6_IRQn
+#define TIM12_IRQn TIM8_BRK_IRQn
+#define TIM8_BRK_TIM12_IRQn TIM8_BRK_IRQn
+#define TIM14_IRQn TIM8_TRG_COM_IRQn
+#define TIM8_TRG_COM_TIM14_IRQn TIM8_TRG_COM_IRQn
+#define TIM8_UP_TIM13_IRQn TIM8_UP_IRQn
+#define TIM13_IRQn TIM8_UP_IRQn
+#define CEC_IRQn USBWakeUp_IRQn
+#define OTG_FS_WKUP_IRQn USBWakeUp_IRQn
+#define CAN1_TX_IRQn USB_HP_CAN1_TX_IRQn
+#define USB_HP_IRQn USB_HP_CAN1_TX_IRQn
+#define USB_LP_IRQn USB_LP_CAN1_RX0_IRQn
+#define CAN1_RX0_IRQn USB_LP_CAN1_RX0_IRQn
+
+
+/* Aliases for __IRQHandler */
+#define ADC1_IRQHandler ADC1_2_IRQHandler
+#define DMA2_Channel4_IRQHandler DMA2_Channel4_5_IRQHandler
+#define TIM1_BRK_TIM15_IRQHandler TIM1_BRK_IRQHandler
+#define TIM1_BRK_TIM9_IRQHandler TIM1_BRK_IRQHandler
+#define TIM9_IRQHandler TIM1_BRK_IRQHandler
+#define TIM1_TRG_COM_TIM11_IRQHandler TIM1_TRG_COM_IRQHandler
+#define TIM1_TRG_COM_TIM17_IRQHandler TIM1_TRG_COM_IRQHandler
+#define TIM11_IRQHandler TIM1_TRG_COM_IRQHandler
+#define TIM10_IRQHandler TIM1_UP_IRQHandler
+#define TIM1_UP_TIM16_IRQHandler TIM1_UP_IRQHandler
+#define TIM1_UP_TIM10_IRQHandler TIM1_UP_IRQHandler
+#define TIM6_DAC_IRQHandler TIM6_IRQHandler
+#define TIM12_IRQHandler TIM8_BRK_IRQHandler
+#define TIM8_BRK_TIM12_IRQHandler TIM8_BRK_IRQHandler
+#define TIM14_IRQHandler TIM8_TRG_COM_IRQHandler
+#define TIM8_TRG_COM_TIM14_IRQHandler TIM8_TRG_COM_IRQHandler
+#define TIM8_UP_TIM13_IRQHandler TIM8_UP_IRQHandler
+#define TIM13_IRQHandler TIM8_UP_IRQHandler
+#define CEC_IRQHandler USBWakeUp_IRQHandler
+#define OTG_FS_WKUP_IRQHandler USBWakeUp_IRQHandler
+#define CAN1_TX_IRQHandler USB_HP_CAN1_TX_IRQHandler
+#define USB_HP_IRQHandler USB_HP_CAN1_TX_IRQHandler
+#define USB_LP_IRQHandler USB_LP_CAN1_RX0_IRQHandler
+#define CAN1_RX0_IRQHandler USB_LP_CAN1_RX0_IRQHandler
+
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+
+#ifdef __cplusplus
+ }
+#endif /* __cplusplus */
+
+#endif /* __STM32F103xE_H */
+
+
+
+ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/darwin_controller_cpp/include/sim/stm32f1xx.h b/darwin_controller_cpp/include/sim/stm32f1xx.h
new file mode 100755
index 0000000000000000000000000000000000000000..b43c76d25954bc57e5cc1c3ebc19a7b230ed4c1d
--- /dev/null
+++ b/darwin_controller_cpp/include/sim/stm32f1xx.h
@@ -0,0 +1,238 @@
+/**
+ ******************************************************************************
+ * @file stm32f1xx.h
+ * @author MCD Application Team
+ * @version V4.0.1
+ * @date 31-July-2015
+ * @brief CMSIS STM32F1xx Device Peripheral Access Layer Header File.
+ *
+ * The file is the unique include file that the application programmer
+ * is using in the C source code, usually in main.c. This file contains:
+ * - Configuration section that allows to select:
+ * - The STM32F1xx device used in the target application
+ * - To use or not the peripheral’s drivers in application code(i.e.
+ * code will be based on direct access to peripheral’s registers
+ * rather than drivers API), this option is controlled by
+ * "#define USE_HAL_DRIVER"
+ *
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>© COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/** @addtogroup CMSIS
+ * @{
+ */
+
+/** @addtogroup stm32f1xx
+ * @{
+ */
+
+#ifndef __STM32F1XX_H
+#define __STM32F1XX_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif /* __cplusplus */
+
+/** @addtogroup Library_configuration_section
+ * @{
+ */
+
+/**
+ * @brief STM32 Family
+ */
+#if !defined (STM32F1)
+#define STM32F1
+#endif /* STM32F1 */
+
+/* Uncomment the line below according to the target STM32L device used in your
+ application
+ */
+
+#if !defined (STM32F100xB) && !defined (STM32F100xE) && !defined (STM32F101x6) && \
+ !defined (STM32F101xB) && !defined (STM32F101xE) && !defined (STM32F101xG) && !defined (STM32F102x6) && !defined (STM32F102xB) && !defined (STM32F103x6) && \
+ !defined (STM32F103xB) && !defined (STM32F103xE) && !defined (STM32F103xG) && !defined (STM32F105xC) && !defined (STM32F107xC)
+ /* #define STM32F100xB */ /*!< STM32F100C4, STM32F100R4, STM32F100C6, STM32F100R6, STM32F100C8, STM32F100R8, STM32F100V8, STM32F100CB, STM32F100RB and STM32F100VB */
+ /* #define STM32F100xE */ /*!< STM32F100RC, STM32F100VC, STM32F100ZC, STM32F100RD, STM32F100VD, STM32F100ZD, STM32F100RE, STM32F100VE and STM32F100ZE */
+ /* #define STM32F101x6 */ /*!< STM32F101C4, STM32F101R4, STM32F101T4, STM32F101C6, STM32F101R6 and STM32F101T6 Devices */
+ /* #define STM32F101xB */ /*!< STM32F101C8, STM32F101R8, STM32F101T8, STM32F101V8, STM32F101CB, STM32F101RB, STM32F101TB and STM32F101VB */
+ /* #define STM32F101xE */ /*!< STM32F101RC, STM32F101VC, STM32F101ZC, STM32F101RD, STM32F101VD, STM32F101ZD, STM32F101RE, STM32F101VE and STM32F101ZE */
+ /* #define STM32F101xG */ /*!< STM32F101RF, STM32F101VF, STM32F101ZF, STM32F101RG, STM32F101VG and STM32F101ZG */
+ /* #define STM32F102x6 */ /*!< STM32F102C4, STM32F102R4, STM32F102C6 and STM32F102R6 */
+ /* #define STM32F102xB */ /*!< STM32F102C8, STM32F102R8, STM32F102CB and STM32F102RB */
+ /* #define STM32F103x6 */ /*!< STM32F103C4, STM32F103R4, STM32F103T4, STM32F103C6, STM32F103R6 and STM32F103T6 */
+ /* #define STM32F103xB */ /*!< STM32F103C8, STM32F103R8, STM32F103T8, STM32F103V8, STM32F103CB, STM32F103RB, STM32F103TB and STM32F103VB */
+ /* #define STM32F103xE */ /*!< STM32F103RC, STM32F103VC, STM32F103ZC, STM32F103RD, STM32F103VD, STM32F103ZD, STM32F103RE, STM32F103VE and STM32F103ZE */
+ /* #define STM32F103xG */ /*!< STM32F103RF, STM32F103VF, STM32F103ZF, STM32F103RG, STM32F103VG and STM32F103ZG */
+ /* #define STM32F105xC */ /*!< STM32F105R8, STM32F105V8, STM32F105RB, STM32F105VB, STM32F105RC and STM32F105VC */
+ /* #define STM32F107xC */ /*!< STM32F107RB, STM32F107VB, STM32F107RC and STM32F107VC */
+#endif
+
+/* Tip: To avoid modifying this file each time you need to switch between these
+ devices, you can define the device in your toolchain compiler preprocessor.
+ */
+
+#if !defined (USE_HAL_DRIVER)
+/**
+ * @brief Comment the line below if you will not use the peripherals drivers.
+ In this case, these drivers will not be included and the application code will
+ be based on direct access to peripherals registers
+ */
+ /*#define USE_HAL_DRIVER */
+#endif /* USE_HAL_DRIVER */
+
+/**
+ * @brief CMSIS Device version number V4.0.1
+ */
+#define __STM32F1xx_CMSIS_DEVICE_VERSION_MAIN (0x04) /*!< [31:24] main version */
+#define __STM32F1xx_CMSIS_DEVICE_VERSION_SUB1 (0x00) /*!< [23:16] sub1 version */
+#define __STM32F1xx_CMSIS_DEVICE_VERSION_SUB2 (0x01) /*!< [15:8] sub2 version */
+#define __STM32F1xx_CMSIS_DEVICE_VERSION_RC (0x00) /*!< [7:0] release candidate */
+#define __STM32F1xx_CMSIS_DEVICE_VERSION ((__CMSIS_DEVICE_VERSION_MAIN << 24)\
+ |(__CMSIS_DEVICE_HAL_VERSION_SUB1 << 16)\
+ |(__CMSIS_DEVICE_HAL_VERSION_SUB2 << 8 )\
+ |(__CMSIS_DEVICE_HAL_VERSION_RC))
+
+/**
+ * @}
+ */
+
+/** @addtogroup Device_Included
+ * @{
+ */
+
+#if defined(STM32F100xB)
+ #include "stm32f100xb.h"
+#elif defined(STM32F100xE)
+ #include "stm32f100xe.h"
+#elif defined(STM32F101x6)
+ #include "stm32f101x6.h"
+#elif defined(STM32F101xB)
+ #include "stm32f101xb.h"
+#elif defined(STM32F101xE)
+ #include "stm32f101xe.h"
+#elif defined(STM32F101xG)
+ #include "stm32f101xg.h"
+#elif defined(STM32F102x6)
+ #include "stm32f102x6.h"
+#elif defined(STM32F102xB)
+ #include "stm32f102xb.h"
+#elif defined(STM32F103x6)
+ #include "stm32f103x6.h"
+#elif defined(STM32F103xB)
+ #include "stm32f103xb.h"
+#elif defined(STM32F103xE)
+ #include "stm32f103xe.h"
+#elif defined(STM32F103xG)
+ #include "stm32f103xg.h"
+#elif defined(STM32F105xC)
+ #include "stm32f105xc.h"
+#elif defined(STM32F107xC)
+ #include "stm32f107xc.h"
+#else
+ #error "Please select first the target STM32F1xx device used in your application (in stm32f1xx.h file)"
+#endif
+
+/**
+ * @}
+ */
+
+/** @addtogroup Exported_types
+ * @{
+ */
+typedef enum
+{
+ RESET = 0,
+ SET = !RESET
+} FlagStatus, ITStatus;
+
+typedef enum
+{
+ DISABLE = 0,
+ ENABLE = !DISABLE
+} FunctionalState;
+#define IS_FUNCTIONAL_STATE(STATE) (((STATE) == DISABLE) || ((STATE) == ENABLE))
+
+typedef enum
+{
+ ERROR = 0,
+ SUCCESS = !ERROR
+} ErrorStatus;
+
+/**
+ * @}
+ */
+
+
+/** @addtogroup Exported_macros
+ * @{
+ */
+#define SET_BIT(REG, BIT) ((REG) |= (BIT))
+
+#define CLEAR_BIT(REG, BIT) ((REG) &= ~(BIT))
+
+#define READ_BIT(REG, BIT) ((REG) & (BIT))
+
+#define CLEAR_REG(REG) ((REG) = (0x0))
+
+#define WRITE_REG(REG, VAL) ((REG) = (VAL))
+
+#define READ_REG(REG) ((REG))
+
+#define MODIFY_REG(REG, CLEARMASK, SETMASK) WRITE_REG((REG), (((READ_REG(REG)) & (~(CLEARMASK))) | (SETMASK)))
+
+#define POSITION_VAL(VAL) (__CLZ(__RBIT(VAL)))
+
+
+/**
+ * @}
+ */
+
+#if defined (USE_HAL_DRIVER)
+ #include "stm32f1xx_hal.h"
+#endif /* USE_HAL_DRIVER */
+
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* __STM32F1xx_H */
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+
+
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/darwin_controller_cpp/include/sim/system_stm32f1xx.h b/darwin_controller_cpp/include/sim/system_stm32f1xx.h
new file mode 100755
index 0000000000000000000000000000000000000000..2394c29fe56ab23b7adfc97f2c362e51399e64c5
--- /dev/null
+++ b/darwin_controller_cpp/include/sim/system_stm32f1xx.h
@@ -0,0 +1,114 @@
+/**
+ ******************************************************************************
+ * @file system_stm32f10x.h
+ * @author MCD Application Team
+ * @version V4.0.1
+ * @date 31-July-2015
+ * @brief CMSIS Cortex-M3 Device Peripheral Access Layer System Header File.
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>© COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/** @addtogroup CMSIS
+ * @{
+ */
+
+/** @addtogroup stm32f10x_system
+ * @{
+ */
+
+/**
+ * @brief Define to prevent recursive inclusion
+ */
+#ifndef __SYSTEM_STM32F10X_H
+#define __SYSTEM_STM32F10X_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/** @addtogroup STM32F10x_System_Includes
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+
+/** @addtogroup STM32F10x_System_Exported_types
+ * @{
+ */
+
+extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */
+
+/**
+ * @}
+ */
+
+/** @addtogroup STM32F10x_System_Exported_Constants
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+/** @addtogroup STM32F10x_System_Exported_Macros
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+/** @addtogroup STM32F10x_System_Exported_Functions
+ * @{
+ */
+
+extern void SystemInit(void);
+extern void SystemCoreClockUpdate(void);
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*__SYSTEM_STM32F10X_H */
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/darwin_controller_cpp/package.xml b/darwin_controller_cpp/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9fce760b828283f5b17541e8aaf9b78e050e27ab
--- /dev/null
+++ b/darwin_controller_cpp/package.xml
@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package>
+ <name>darwin_controller_cpp</name>
+ <version>0.0.0</version>
+ <description>The darwin_controller_cpp package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="sergi@todo.todo">sergi</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/darwin_controller_cpp</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>urdf</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+
+ <build_depend>controller_interface</build_depend>
+ <build_depend>hardware_interface</build_depend>
+
+ <build_depend>controller_manager</build_depend> <!--Tests-only!-->
+ <build_depend>gazebo_ros</build_depend>
+
+ <run_depend>roscpp</run_depend>
+ <run_depend>urdf</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+
+ <run_depend>controller_interface</run_depend>
+ <run_depend>hardware_interface</run_depend>
+
+ <run_depend>controller_manager</run_depend> <!--Tests-only!-->
+ <run_depend>gazebo_ros</run_depend>
+
+ <export>
+ <controller_interface plugin="${prefix}/ros_control_plugins.xml"/>
+ </export>
+</package>
diff --git a/darwin_controller_cpp/ros_control_plugins.xml b/darwin_controller_cpp/ros_control_plugins.xml
new file mode 100644
index 0000000000000000000000000000000000000000..bfaaac29381451b0c74236eea2f066702b47b1fe
--- /dev/null
+++ b/darwin_controller_cpp/ros_control_plugins.xml
@@ -0,0 +1,12 @@
+<library path="lib/libdarwin_controller_cpp">
+
+ <class name="effort_controllers/DarwinControllerCPP"
+ type="effort_controllers::DarwinControllerCPP"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ this is a description
+ </description>
+ </class>
+
+</library>
+
diff --git a/darwin_controller_cpp/src/darwin_controller_cpp.cpp b/darwin_controller_cpp/src/darwin_controller_cpp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f24c4ae4288c54a64c07ca3750f770ecb8dfb2bf
--- /dev/null
+++ b/darwin_controller_cpp/src/darwin_controller_cpp.cpp
@@ -0,0 +1,44 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics S.L. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+// Pluginlib
+#include <pluginlib/class_list_macros.h>
+
+// Project
+#include <darwin_controller_cpp.h>
+
+namespace effort_controllers
+{
+ /**
+ * \brief Joint trajectory controller that represents trajectory segments as <b>quintic splines</b> and sends
+ * commands to a \b position interface.
+ */
+ typedef darwin_controller_cpp::DarwinControllerCPP<hardware_interface::EffortJointInterface> DarwinControllerCPP;
+}
+
+PLUGINLIB_EXPORT_CLASS(effort_controllers::DarwinControllerCPP, controller_interface::ControllerBase)
diff --git a/darwin_controller_cpp/src/sim/imu.c b/darwin_controller_cpp/src/sim/imu.c
new file mode 100755
index 0000000000000000000000000000000000000000..c3a8c924a70f501d49777fedd50a624ef84d8e78
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/imu.c
@@ -0,0 +1,75 @@
+#include "imu.h"
+#include "ram.h"
+
+// gyriscope private variables
+int16_t gyro_center[3];
+int32_t gyro_data[3];
+uint8_t gyro_calibration;
+uint16_t gyro_calibration_num_samples;
+int32_t accel_data[3];
+
+// public functions
+void imu_init(void)
+{
+ gyro_center[0]=0;
+ gyro_center[1]=0;
+ gyro_center[2]=0;
+ gyro_data[0]=0;
+ gyro_data[1]=0;
+ gyro_data[2]=0;
+ gyro_calibration=0x00;
+ imu_set_calibration_samples(1024);
+ accel_data[0]=0;
+ accel_data[1]=0;
+ accel_data[2]=0;
+}
+
+void imu_start(void)
+{
+}
+
+void imu_stop(void)
+{
+}
+
+void imu_set_calibration_samples(uint16_t num_samples)
+{
+ gyro_calibration_num_samples=num_samples;
+ ram_data[DARWIN_IMU_CAL_SAMPLES_L]=gyro_calibration_num_samples%256;
+ ram_data[DARWIN_IMU_CAL_SAMPLES_H]=gyro_calibration_num_samples/256;
+}
+
+void imu_start_gyro_cal(void)
+{
+ ram_data[DARWIN_IMU_CNTRL]|=IMU_CALIBRATING;
+ gyro_center[0]=0;
+ gyro_center[1]=0;
+ gyro_center[2]=0;
+ gyro_calibration=0x01;
+}
+
+void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
+{
+ *x=gyro_data[0]-gyro_center[0];
+ *y=gyro_data[1]-gyro_center[1];
+ *z=gyro_data[2]-gyro_center[2];
+}
+
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
+{
+ *x=accel_data[0];
+ *y=accel_data[1];
+ *z=accel_data[2];
+}
+
+// operation functions
+uint8_t imu_in_range(unsigned short int address,unsigned short int length)
+{
+ return 0x01;
+}
+
+void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+
+}
+
diff --git a/darwin_controller_cpp/src/sim/stm32_cortex.c b/darwin_controller_cpp/src/sim/stm32_cortex.c
new file mode 100755
index 0000000000000000000000000000000000000000..7062a798e2dafc6d23f1bd5a3f2e30cb2f91e907
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_cortex.c
@@ -0,0 +1,66 @@
+#include "stm32f1xx_hal.h"
+
+void HAL_NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
+{
+}
+
+void HAL_NVIC_SetPriority(IRQn_Type IRQn, uint32_t PreemptPriority, uint32_t SubPriority)
+{
+}
+
+void HAL_NVIC_EnableIRQ(IRQn_Type IRQn)
+{
+}
+
+void HAL_NVIC_DisableIRQ(IRQn_Type IRQn)
+{
+}
+
+void HAL_NVIC_SystemReset(void)
+{
+}
+
+uint32_t HAL_SYSTICK_Config(uint32_t TicksNumb)
+{
+ return 0;
+}
+
+uint32_t HAL_NVIC_GetPriorityGrouping(void)
+{
+ return 0;
+}
+
+void HAL_NVIC_GetPriority(IRQn_Type IRQn, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)
+{
+}
+
+void HAL_NVIC_SetPendingIRQ(IRQn_Type IRQn)
+{
+}
+
+uint32_t HAL_NVIC_GetPendingIRQ(IRQn_Type IRQn)
+{
+ return 0;
+}
+
+void HAL_NVIC_ClearPendingIRQ(IRQn_Type IRQn)
+{
+}
+
+uint32_t HAL_NVIC_GetActive(IRQn_Type IRQn)
+{
+ return 0;
+}
+
+void HAL_SYSTICK_CLKSourceConfig(uint32_t CLKSource)
+{
+}
+
+void HAL_SYSTICK_IRQHandler(void)
+{
+}
+
+__weak void HAL_SYSTICK_Callback(void)
+{
+}
+
diff --git a/darwin_controller_cpp/src/sim/stm32_dma.c b/darwin_controller_cpp/src/sim/stm32_dma.c
new file mode 100755
index 0000000000000000000000000000000000000000..9a7f8433e0df4bfa1c714af8549d6a4f820b2635
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_dma.c
@@ -0,0 +1,52 @@
+#include "stm32f1xx_hal.h"
+
+#define HAL_TIMEOUT_DMA_ABORT ((uint32_t)1000) /* 1s */
+static void DMA_SetConfig(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, uint32_t DstAddress, uint32_t DataLength);
+
+HAL_StatusTypeDef HAL_DMA_Init(DMA_HandleTypeDef *hdma)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_DMA_DeInit(DMA_HandleTypeDef *hdma)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_DMA_Start(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, uint32_t DstAddress, uint32_t DataLength)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_DMA_Start_IT(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, uint32_t DstAddress, uint32_t DataLength)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_DMA_Abort(DMA_HandleTypeDef *hdma)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_DMA_PollForTransfer(DMA_HandleTypeDef *hdma, uint32_t CompleteLevel, uint32_t Timeout)
+{
+ return HAL_OK;
+}
+
+void HAL_DMA_IRQHandler(DMA_HandleTypeDef *hdma)
+{
+}
+
+HAL_DMA_StateTypeDef HAL_DMA_GetState(DMA_HandleTypeDef *hdma)
+{
+ return hdma->State;
+}
+
+uint32_t HAL_DMA_GetError(DMA_HandleTypeDef *hdma)
+{
+ return hdma->ErrorCode;
+}
+
+static void DMA_SetConfig(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, uint32_t DstAddress, uint32_t DataLength)
+{
+}
diff --git a/darwin_controller_cpp/src/sim/stm32_dyn_master.c b/darwin_controller_cpp/src/sim/stm32_dyn_master.c
new file mode 100644
index 0000000000000000000000000000000000000000..1364dbe89a24d4499416d8d7e211522c1d80ba0e
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_dyn_master.c
@@ -0,0 +1,151 @@
+#include "dynamixel_master.h"
+#include "action.h"
+#include "dyn_servos.h"
+
+int num_servos;
+double real_angles[PAGE_MAX_NUM_SERVOS];
+
+/* public functions */
+void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
+{
+}
+
+void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
+{
+ master->rx_timeout_ms=timeout_ms;
+}
+
+void dyn_master_set_return_level(TDynamixelMaster *master,TReturnLevel level)
+{
+ master->return_level=level;
+}
+
+TReturnLevel dyn_master_get_return_level(TDynamixelMaster *master)
+{
+ return master->return_level;
+}
+
+void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids)
+{
+ unsigned char i;
+
+ *num=0;
+ for(i=0;i<254;i++)
+ {
+ if(dyn_master_ping(master,i)==DYN_SUCCESS)// the device exists
+ {
+ ids[*num]=i;
+ (*num)++;
+ }
+ }
+}
+
+unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)
+{
+ if(master->version==DYN_VER1)
+ {
+ if(id!=0 && id<=num_servos)
+ return DYN_SUCCESS;
+ else
+ return DYN_COMM_ERROR;
+ }
+ else
+ return DYN_COMM_ERROR;
+}
+
+unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data)
+{
+ if(address==P_MODEL_NUMBER_L)
+ *data=SERVO_MX28;
+ else if(address==P_PRESENT_POSITION_L)
+ *data=((((real_angles[id-1]*180.0)/3.143159)+180.0)*4096.0)/360.0;
+ else if(address==P_CW_ANGLE_LIMIT_L)
+ *data=0;
+ else if(address==P_CCW_ANGLE_LIMIT_L)
+ *data=4095;
+ else
+ *data=0x0000;
+
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ unsigned int i=0;
+
+ for(i=0;i<length;i++)
+ data[i]=0x00;
+
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_action(TDynamixelMaster *master)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+ unsigned int i,j;
+
+ for(i=0;i<num;i++)
+ {
+ for(j=0;j<length;j++)
+ data[i].data_addr[j]=0x00;
+ }
+
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+ unsigned int i,j;
+
+ for(i=0;i<num;i++)
+ {
+ for(j=0;j<length[i];j++)
+ data[i].data_addr[j]=0x00;
+ }
+
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt)
+{
+ return DYN_SUCCESS;
+}
diff --git a/darwin_controller_cpp/src/sim/stm32_dyn_master_servos.c b/darwin_controller_cpp/src/sim/stm32_dyn_master_servos.c
new file mode 100644
index 0000000000000000000000000000000000000000..fe9c25d983c2b6fb018d3abe958dd316fcf4b074
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_dyn_master_servos.c
@@ -0,0 +1,20 @@
+#include "darwin_dyn_master.h"
+
+TDynamixelMaster darwin_dyn_master;
+
+void darwin_dyn_master_init(void)
+{
+ /* do nothing */
+ darwin_dyn_master.version=DYN_VER1;
+}
+
+void darwin_dyn_master_enable_power(void)
+{
+ /* do nothing */
+}
+
+void darwin_dyn_master_disable_power(void)
+{
+ /* do nothing */
+}
+
diff --git a/darwin_controller_cpp/src/sim/stm32_dyn_master_v2_servos.c b/darwin_controller_cpp/src/sim/stm32_dyn_master_v2_servos.c
new file mode 100644
index 0000000000000000000000000000000000000000..644fa60d65f6db90bbf75b4943b31b3779fbec36
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_dyn_master_v2_servos.c
@@ -0,0 +1,17 @@
+#include "darwin_dyn_master_v2.h"
+
+TDynamixelMaster darwin_dyn_master_v2;
+
+void darwin_dyn_master_v2_init(void)
+{
+ darwin_dyn_master_v2.version=DYN_VER2;
+}
+
+void darwin_dyn_master_v2_enable_power(void)
+{
+}
+
+void darwin_dyn_master_v2_disable_power(void)
+{
+}
+
diff --git a/darwin_controller_cpp/src/sim/stm32_dyn_slave.c b/darwin_controller_cpp/src/sim/stm32_dyn_slave.c
new file mode 100755
index 0000000000000000000000000000000000000000..5afb29113dc7031db8c6e0c84a2e8b55189db259
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_dyn_slave.c
@@ -0,0 +1,115 @@
+#include "darwin_dyn_slave.h"
+#include "darwin_time.h"
+#include "usart3.h"
+#include "ram.h"
+#include "eeprom.h"
+#include "gpio.h"
+#include "adc_dma.h"
+#include "imu.h"
+#include "motion_manager.h"
+#include "action.h"
+#include "walking.h"
+#include "joint_motion.h"
+#include "head_tracking.h"
+#include "grippers.h"
+#include "smart_charger.h"
+#include "stairs.h"
+
+// private functions
+unsigned char darwin_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ unsigned char error;
+
+ if(ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length))
+ gpio_process_read_cmd(address,length,data);
+ /* dynamixel slave internal operation registers */
+ error=ram_read_table(address,length,data);
+
+ return error;
+}
+
+unsigned char darwin_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ unsigned short int i,j;
+
+ /* dynamixel slave internal operation registers */
+ if(ram_in_range(DARWIN_ID,address,length))
+ {
+ //dyn_slave_set_address(&darwin_dyn_slave,data[DARWIN_ID-address]);
+ ram_data[DARWIN_ID]=data[DARWIN_ID-address];
+ }
+ if(ram_in_range(DARWIN_BAUD_RATE,address,length))
+ {
+ //darwin_comm_init.BaudRate=2000000/(data[DARWIN_BAUD_RATE-address]+1);
+ //usart3_config(&darwin_dyn_slave_comm,&darwin_comm_init);
+ ram_data[DARWIN_BAUD_RATE]=data[DARWIN_BAUD_RATE-address];
+ }
+ if(ram_in_range(DARWIN_RETURN_DELAY_TIME,address,length))
+ {
+ //dyn_slave_set_return_delay(&darwin_dyn_slave,data[DARWIN_RETURN_DELAY_TIME-address]);
+ ram_data[DARWIN_RETURN_DELAY_TIME]=data[DARWIN_RETURN_DELAY_TIME-address];
+ }
+ if(ram_in_range(DARWIN_RETURN_LEVEL,address,length))
+ {
+ //dyn_slave_set_return_level(&darwin_dyn_slave,data[DARWIN_RETURN_LEVEL-address]);
+ ram_data[DARWIN_RETURN_LEVEL]=data[DARWIN_RETURN_LEVEL-address];
+ }
+ // GPIO commands
+ if(gpio_in_range(address,length))
+ gpio_process_write_cmd(address,length,data);
+ // ADC commands
+ if(adc_in_range(address,length))
+ adc_process_write_cmd(address,length,data);
+ // IMU commands
+ if(imu_in_range(address,length))
+ imu_process_write_cmd(address,length,data);
+ // Manager commands
+ if(manager_in_range(address,length))
+ manager_process_write_cmd(address,length,data);
+ // action commands
+ if(action_in_range(address,length))
+ action_process_write_cmd(address,length,data);
+ // walk commands
+ if(walking_in_range(address,length))
+ walking_process_write_cmd(address,length,data);
+ // joint motion commands
+ if(joint_motion_in_range(address,length))
+ joint_motion_process_write_cmd(address,length,data);
+ // head_tracking commands
+ if(head_tracking_in_range(address,length))
+ head_tracking_process_write_cmd(address,length,data);
+ // gripper commands
+ if(grippers_in_range(address,length))
+ grippers_process_write_cmd(address,length,data);
+ // smart charger commands
+ if(smart_charger_in_range(address,length))
+ smart_charger_process_write_cmd(address,length,data);
+ if(stairs_in_range(address,length))
+ stairs_process_write_cmd(address,length,data);
+ // write eeprom
+ for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
+ EE_WriteVariable(i,data[j]);
+
+ return 0x00;
+}
+
+void darwin_on_ping(void)
+{
+}
+
+// public functions
+void darwin_dyn_slave_init(void)
+{
+ /* do nothing */
+}
+
+void darwin_dyn_slave_start(void)
+{
+ /* do nothing */
+}
+
+void darwin_dyn_slave_stop(void)
+{
+ /* do nothing */
+}
+
diff --git a/darwin_controller_cpp/src/sim/stm32_eeprom.c b/darwin_controller_cpp/src/sim/stm32_eeprom.c
new file mode 100644
index 0000000000000000000000000000000000000000..79adadc22b64b54dcb89aeb7e87c0ec8f2d41124
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_eeprom.c
@@ -0,0 +1,160 @@
+#include "stm32f1xx_hal.h"
+#include "eeprom_init.h"
+
+unsigned char eeprom_data[]={DEFAULT_DEVICE_MODEL&0xFF,
+ DEFAULT_DEVICE_MODEL>>8,
+ DEFAULT_FIRMWARE_VERSION,
+ DEFAULT_DEVICE_ID,
+ DEFAULT_BAUDRATE,
+ DEFAULT_RETURN_DELAY,
+ DEFAULT_MM_PERIOD&0xFF,
+ DEFAULT_MM_PERIOD>>8,
+ DEFAULT_BAL_KNEE_GAIN&0xFF,
+ DEFAULT_BAL_KNEE_GAIN>>8,
+ DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF,
+ DEFAULT_BAL_ANKLE_ROLL_GAIN>>8,
+ DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF,
+ DEFAULT_BAL_ANKLE_PITCH_GAIN>>8,
+ DEFAULT_BAL_HIP_ROLL_GAIN&0xFF,
+ DEFAULT_BAL_HIP_ROLL_GAIN>>8,
+ DEFAULT_RETURN_LEVEL,
+ DEFAULT_SERVO0_OFFSET,
+ DEFAULT_SERVO1_OFFSET,
+ DEFAULT_SERVO2_OFFSET,
+ DEFAULT_SERVO3_OFFSET,
+ DEFAULT_SERVO4_OFFSET,
+ DEFAULT_SERVO5_OFFSET,
+ DEFAULT_SERVO6_OFFSET,
+ DEFAULT_SERVO7_OFFSET,
+ DEFAULT_SERVO8_OFFSET,
+ DEFAULT_SERVO9_OFFSET,
+ DEFAULT_SERVO10_OFFSET,
+ DEFAULT_SERVO11_OFFSET,
+ DEFAULT_SERVO12_OFFSET,
+ DEFAULT_SERVO13_OFFSET,
+ DEFAULT_SERVO14_OFFSET,
+ DEFAULT_SERVO15_OFFSET,
+ DEFAULT_SERVO16_OFFSET,
+ DEFAULT_SERVO17_OFFSET,
+ DEFAULT_SERVO18_OFFSET,
+ DEFAULT_SERVO19_OFFSET,
+ DEFAULT_SERVO20_OFFSET,
+ DEFAULT_SERVO21_OFFSET,
+ DEFAULT_SERVO22_OFFSET,
+ DEFAULT_SERVO23_OFFSET,
+ DEFAULT_SERVO24_OFFSET,
+ DEFAULT_SERVO25_OFFSET,
+ DEFAULT_SERVO26_OFFSET,
+ DEFAULT_SERVO27_OFFSET,
+ DEFAULT_SERVO28_OFFSET,
+ DEFAULT_SERVO29_OFFSET,
+ DEFAULT_SERVO30_OFFSET,
+ DEFAULT_SERVO31_OFFSET,
+ DEFAULT_WALK_X_OFFSET&0xFF,
+ DEFAULT_WALK_Y_OFFSET,
+ DEFAULT_WALK_Z_OFFSET,
+ DEFAULT_WALK_ROLL_OFFSET,
+ DEFAULT_WALK_PITCH_OFFSET,
+ DEFAULT_WALK_YAW_OFFSET,
+ DEFAULT_WALK_HIP_PITCH_OFF&0xFF,
+ DEFAULT_WALK_HIP_PITCH_OFF>>8,
+ DEFAULT_WALK_PERIOD_TIME&0xFF,
+ DEFAULT_WALK_PERIOD_TIME>>8,
+ DEFAULT_WALK_DSP_RATIO,
+ DEFAULT_WALK_STEP_FW_BW_RATIO,
+ DEFAULT_WALK_FOOT_HEIGHT,
+ DEFAULT_WALK_SWING_RIGHT_LEFT,
+ DEFAULT_WALK_SWING_TOP_DOWN,
+ DEFAULT_WALK_PELVIS_OFFSET,
+ DEFAULT_WALK_ARM_SWING_GAIN,
+ DEFAULT_WALK_MAX_VEL,
+ DEFAULT_WALK_MAX_ROT_VEL,
+ DEFAULT_HEAD_PAN_P&0xFF,
+ DEFAULT_HEAD_PAN_P>>8,
+ DEFAULT_HEAD_PAN_I&0xFF,
+ DEFAULT_HEAD_PAN_I>>8,
+ DEFAULT_HEAD_PAN_D&0xFF,
+ DEFAULT_HEAD_PAN_D>>8,
+ DEFAULT_HEAD_PAN_I_CLAMP&0xFF,
+ DEFAULT_HEAD_PAN_I_CLAMP>>8,
+ DEFAULT_HEAD_TILT_P&0xFF,
+ DEFAULT_HEAD_TILT_P>>8,
+ DEFAULT_HEAD_TILT_I&0xFF,
+ DEFAULT_HEAD_TILT_I>>8,
+ DEFAULT_HEAD_TILT_D&0xFF,
+ DEFAULT_HEAD_TILT_D>>8,
+ DEFAULT_HEAD_TILT_I_CLAMP&0xFF,
+ DEFAULT_HEAD_TILT_I_CLAMP>>8,
+ DEFAULT_GRIPPER_LEFT_TOP_ID,
+ DEFAULT_GRIPPER_LEFT_BOT_ID,
+ DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF,
+ DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8,
+ DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF,
+ DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8,
+ DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF,
+ DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8,
+ DEFAULT_GRIPPER_RIGHT_TOP_ID,
+ DEFAULT_GRIPPER_RIGHT_BOT_ID,
+ DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF,
+ DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8,
+ DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF,
+ DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8,
+ DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF,
+ DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8,
+ DEFAULT_SMART_CHARGER_PERIOD&0xFF,
+ DEFAULT_SMART_CHARGER_PERIOD>>8,
+ DEFAULT_STAIRS_PHASE1_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE1_TIME>>8,
+ DEFAULT_STAIRS_PHASE2_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE2_TIME>>8,
+ DEFAULT_STAIRS_PHASE3_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE3_TIME>>8,
+ DEFAULT_STAIRS_PHASE4_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE4_TIME>>8,
+ DEFAULT_STAIRS_PHASE5_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE5_TIME>>8,
+ DEFAULT_STAIRS_PHASE6_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE6_TIME>>8,
+ DEFAULT_STAIRS_PHASE7_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE7_TIME>>8,
+ DEFAULT_STAIRS_PHASE8_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE8_TIME>>8,
+ DEFAULT_STAIRS_PHASE9_TIME&0xFF,
+ DEFAULT_STAIRS_PHASE9_TIME>>8,
+ DEFAULT_STAIRS_X_OFFSET&0xFF,
+ DEFAULT_STAIRS_Y_OFFSET&0xFF,
+ DEFAULT_STAIRS_Z_OFFSET&0xFF,
+ DEFAULT_STAIRS_R_OFFSET,
+ DEFAULT_STAIRS_P_OFFSET,
+ DEFAULT_STAIRS_Y_OFFSET,
+ DEFAULT_STAIRS_Y_SHIFT,
+ DEFAULT_STAIRS_X_SHIFT,
+ DEFAULT_STAIRS_Z_OVERSHOOT,
+ DEFAULT_STAIRS_Z_HEIGHT,
+ DEFAULT_STAIRS_HIP_PITCH_OFF&0xFF,
+ DEFAULT_STAIRS_HIP_PITCH_OFF>>8,
+ DEFAULT_STAIRS_R_SHIFT,
+ DEFAULT_STAIRS_P_SHIFT,
+ DEFAULT_STAIRS_A_SHIFT,
+ DEFAULT_STAIRS_Y_SPREAD,
+ DEFAULT_STAIRS_X_SHIFT_BODY};
+
+uint16_t EE_Init(void)
+{
+
+}
+
+uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data)
+{
+ *Data=eeprom_data[VirtAddress];
+
+ return 0x0000;
+}
+
+uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data)
+{
+ eeprom_data[VirtAddress]=Data;
+
+ return 0x0000;
+}
+
diff --git a/darwin_controller_cpp/src/sim/stm32_gpio.c b/darwin_controller_cpp/src/sim/stm32_gpio.c
new file mode 100755
index 0000000000000000000000000000000000000000..c253a4a7671f888e8739a6f0b7525e31277c3edc
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_gpio.c
@@ -0,0 +1,35 @@
+#include "stm32f1xx_hal.h"
+
+void HAL_GPIO_Init(GPIO_TypeDef *GPIOx, GPIO_InitTypeDef *GPIO_Init)
+{
+}
+
+void HAL_GPIO_DeInit(GPIO_TypeDef *GPIOx, uint32_t GPIO_Pin)
+{
+}
+
+GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
+{
+ return GPIO_PIN_RESET;
+}
+
+void HAL_GPIO_WritePin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState)
+{
+}
+
+void HAL_GPIO_TogglePin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
+{
+}
+
+HAL_StatusTypeDef HAL_GPIO_LockPin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
+{
+ return HAL_OK;
+}
+
+void HAL_GPIO_EXTI_IRQHandler(uint16_t GPIO_Pin)
+{
+}
+
+__weak void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
+{
+}
diff --git a/darwin_controller_cpp/src/sim/stm32_hal.c b/darwin_controller_cpp/src/sim/stm32_hal.c
new file mode 100755
index 0000000000000000000000000000000000000000..9f425c435c9e0182a80f1760edfb701c8b000ec0
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_hal.c
@@ -0,0 +1,84 @@
+#include "stm32f1xx_hal.h"
+
+HAL_StatusTypeDef HAL_Init(void)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_DeInit(void)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_MspInit(void)
+{
+}
+
+__weak void HAL_MspDeInit(void)
+{
+}
+
+__weak HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_IncTick(void)
+{
+}
+
+__weak uint32_t HAL_GetTick(void)
+{
+ return 0;
+}
+
+__weak void HAL_Delay(__IO uint32_t Delay)
+{
+}
+
+__weak void HAL_SuspendTick(void)
+{
+}
+
+__weak void HAL_ResumeTick(void)
+{
+}
+
+uint32_t HAL_GetHalVersion(void)
+{
+ return 0;
+}
+
+uint32_t HAL_GetREVID(void)
+{
+ return 0;
+}
+
+uint32_t HAL_GetDEVID(void)
+{
+ return 0;
+}
+
+void HAL_DBGMCU_EnableDBGSleepMode(void)
+{
+}
+
+void HAL_DBGMCU_DisableDBGSleepMode(void)
+{
+}
+
+void HAL_DBGMCU_EnableDBGStopMode(void)
+{
+}
+
+void HAL_DBGMCU_DisableDBGStopMode(void)
+{
+}
+
+void HAL_DBGMCU_EnableDBGStandbyMode(void)
+{
+}
+
+void HAL_DBGMCU_DisableDBGStandbyMode(void)
+{
+}
diff --git a/darwin_controller_cpp/src/sim/stm32_timer.c b/darwin_controller_cpp/src/sim/stm32_timer.c
new file mode 100755
index 0000000000000000000000000000000000000000..ddcfdb30946107d340287e45284e8ff6bd455a67
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_timer.c
@@ -0,0 +1,501 @@
+#include "stm32f1xx_hal.h"
+
+HAL_StatusTypeDef HAL_TIM_Base_Init(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Base_DeInit(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_TIM_Base_MspInit(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef *htim)
+{
+}
+
+HAL_StatusTypeDef HAL_TIM_Base_Start(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Base_Stop(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Base_Start_IT(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Base_Stop_IT(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Base_Start_DMA(TIM_HandleTypeDef *htim, uint32_t *pData, uint16_t Length)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Base_Stop_DMA(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OC_Init(TIM_HandleTypeDef* htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OC_DeInit(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_TIM_OC_MspInit(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_OC_MspDeInit(TIM_HandleTypeDef *htim)
+{
+}
+
+HAL_StatusTypeDef HAL_TIM_OC_Start(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OC_Stop(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OC_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OC_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OC_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OC_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_PWM_Init(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_PWM_DeInit(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef *htim)
+{
+}
+
+HAL_StatusTypeDef HAL_TIM_PWM_Start(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_PWM_Stop(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_PWM_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_PWM_Stop_IT (TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_PWM_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_PWM_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_IC_Init(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_IC_DeInit(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_TIM_IC_MspInit(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_IC_MspDeInit(TIM_HandleTypeDef *htim)
+{
+}
+
+HAL_StatusTypeDef HAL_TIM_IC_Start (TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_IC_Stop(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_IC_Start_IT (TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_IC_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_IC_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_IC_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OnePulse_Init(TIM_HandleTypeDef *htim, uint32_t OnePulseMode)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OnePulse_DeInit(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_TIM_OnePulse_MspInit(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_OnePulse_MspDeInit(TIM_HandleTypeDef *htim)
+{
+}
+
+HAL_StatusTypeDef HAL_TIM_OnePulse_Start(TIM_HandleTypeDef *htim, uint32_t OutputChannel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OnePulse_Stop(TIM_HandleTypeDef *htim, uint32_t OutputChannel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OnePulse_Start_IT(TIM_HandleTypeDef *htim, uint32_t OutputChannel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OnePulse_Stop_IT(TIM_HandleTypeDef *htim, uint32_t OutputChannel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Encoder_Init(TIM_HandleTypeDef *htim, TIM_Encoder_InitTypeDef* sConfig)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Encoder_DeInit(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef *htim)
+{
+}
+
+HAL_StatusTypeDef HAL_TIM_Encoder_Start(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Encoder_Stop(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Encoder_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Encoder_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Encoder_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData1, uint32_t *pData2, uint16_t Length)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_Encoder_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+void HAL_TIM_IRQHandler(TIM_HandleTypeDef *htim)
+{
+}
+
+HAL_StatusTypeDef HAL_TIM_OC_ConfigChannel(TIM_HandleTypeDef *htim, TIM_OC_InitTypeDef* sConfig, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_IC_ConfigChannel(TIM_HandleTypeDef *htim, TIM_IC_InitTypeDef* sConfig, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_PWM_ConfigChannel(TIM_HandleTypeDef *htim, TIM_OC_InitTypeDef* sConfig, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_OnePulse_ConfigChannel(TIM_HandleTypeDef *htim, TIM_OnePulse_InitTypeDef* sConfig, uint32_t OutputChannel, uint32_t InputChannel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_DMABurst_WriteStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, uint32_t BurstRequestSrc,
+ uint32_t* BurstBuffer, uint32_t BurstLength)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_DMABurst_WriteStop(TIM_HandleTypeDef *htim, uint32_t BurstRequestSrc)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_DMABurst_ReadStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, uint32_t BurstRequestSrc,
+ uint32_t *BurstBuffer, uint32_t BurstLength)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_DMABurst_ReadStop(TIM_HandleTypeDef *htim, uint32_t BurstRequestSrc)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_GenerateEvent(TIM_HandleTypeDef *htim, uint32_t EventSource)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_ConfigOCrefClear(TIM_HandleTypeDef *htim, TIM_ClearInputConfigTypeDef * sClearInputConfig, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_ConfigClockSource(TIM_HandleTypeDef *htim, TIM_ClockConfigTypeDef * sClockSourceConfig)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_ConfigTI1Input(TIM_HandleTypeDef *htim, uint32_t TI1_Selection)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchronization(TIM_HandleTypeDef *htim, TIM_SlaveConfigTypeDef * sSlaveConfig)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIM_SlaveConfigSynchronization_IT(TIM_HandleTypeDef *htim,
+ TIM_SlaveConfigTypeDef * sSlaveConfig)
+{
+ return HAL_OK;
+}
+
+uint32_t HAL_TIM_ReadCapturedValue(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return 0;
+}
+
+__weak void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_TriggerCallback(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIM_ErrorCallback(TIM_HandleTypeDef *htim)
+{
+}
+
+HAL_TIM_StateTypeDef HAL_TIM_Base_GetState(TIM_HandleTypeDef *htim)
+{
+ return htim->State;
+}
+
+HAL_TIM_StateTypeDef HAL_TIM_OC_GetState(TIM_HandleTypeDef *htim)
+{
+ return htim->State;
+}
+
+HAL_TIM_StateTypeDef HAL_TIM_PWM_GetState(TIM_HandleTypeDef *htim)
+{
+ return htim->State;
+}
+
+HAL_TIM_StateTypeDef HAL_TIM_IC_GetState(TIM_HandleTypeDef *htim)
+{
+ return htim->State;
+}
+
+HAL_TIM_StateTypeDef HAL_TIM_OnePulse_GetState(TIM_HandleTypeDef *htim)
+{
+ return htim->State;
+}
+
+HAL_TIM_StateTypeDef HAL_TIM_Encoder_GetState(TIM_HandleTypeDef *htim)
+{
+ return htim->State;
+}
+
+void TIM_DMAError(DMA_HandleTypeDef *hdma)
+{
+}
+
+void TIM_DMADelayPulseCplt(DMA_HandleTypeDef *hdma)
+{
+}
+
+void TIM_DMACaptureCplt(DMA_HandleTypeDef *hdma)
+{
+}
+
+static void TIM_DMAPeriodElapsedCplt(DMA_HandleTypeDef *hdma)
+{
+}
+
+static void TIM_DMATriggerCplt(DMA_HandleTypeDef *hdma)
+{
+}
+
+void TIM_Base_SetConfig(TIM_TypeDef *TIMx, TIM_Base_InitTypeDef *Structure)
+{
+}
+
+static void TIM_OC1_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config)
+{
+}
+
+void TIM_OC2_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config)
+{
+}
+
+static void TIM_OC3_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config)
+{
+}
+
+static void TIM_OC4_SetConfig(TIM_TypeDef *TIMx, TIM_OC_InitTypeDef *OC_Config)
+{
+}
+
+static void TIM_SlaveTimer_SetConfig(TIM_HandleTypeDef *htim,
+ TIM_SlaveConfigTypeDef * sSlaveConfig)
+{
+}
+
+void TIM_TI1_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ICSelection,
+ uint32_t TIM_ICFilter)
+{
+}
+
+static void TIM_TI1_ConfigInputStage(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ICFilter)
+{
+}
+
+static void TIM_TI2_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ICSelection,
+ uint32_t TIM_ICFilter)
+{
+}
+
+static void TIM_TI2_ConfigInputStage(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ICFilter)
+{
+}
+
+static void TIM_TI3_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ICSelection,
+ uint32_t TIM_ICFilter)
+{
+}
+
+static void TIM_TI4_SetConfig(TIM_TypeDef *TIMx, uint32_t TIM_ICPolarity, uint32_t TIM_ICSelection,
+ uint32_t TIM_ICFilter)
+{
+}
+
+static void TIM_ITRx_SetConfig(TIM_TypeDef *TIMx, uint16_t InputTriggerSource)
+{
+}
+
+static void TIM_ETR_SetConfig(TIM_TypeDef* TIMx, uint32_t TIM_ExtTRGPrescaler,
+ uint32_t TIM_ExtTRGPolarity, uint32_t ExtTRGFilter)
+{
+}
+
+void TIM_CCxChannelCmd(TIM_TypeDef* TIMx, uint32_t Channel, uint32_t ChannelState)
+{
+}
diff --git a/darwin_controller_cpp/src/sim/stm32_timer_ex.c b/darwin_controller_cpp/src/sim/stm32_timer_ex.c
new file mode 100755
index 0000000000000000000000000000000000000000..0fe7ab50b04a72a39188bf828547d2f721abcb6e
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/stm32_timer_ex.c
@@ -0,0 +1,176 @@
+#include "stm32f1xx_hal.h"
+
+HAL_StatusTypeDef HAL_TIMEx_HallSensor_Init(TIM_HandleTypeDef *htim, TIM_HallSensor_InitTypeDef* sConfig)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_HallSensor_DeInit(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_TIMEx_HallSensor_MspInit(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIMEx_HallSensor_MspDeInit(TIM_HandleTypeDef *htim)
+{
+}
+
+HAL_StatusTypeDef HAL_TIMEx_HallSensor_Start(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_HallSensor_Stop(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_HallSensor_Start_IT(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_HallSensor_Stop_IT(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_HallSensor_Start_DMA(TIM_HandleTypeDef *htim, uint32_t *pData, uint16_t Length)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_HallSensor_Stop_DMA(TIM_HandleTypeDef *htim)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OCN_Start(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OCN_Stop(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OCN_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OCN_Stop_IT(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OCN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OCN_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_PWMN_Start(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_PWMN_Stop(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_PWMN_Start_IT(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_PWMN_Stop_IT (TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_PWMN_Start_DMA(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_PWMN_Stop_DMA(TIM_HandleTypeDef *htim, uint32_t Channel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OnePulseN_Start(TIM_HandleTypeDef *htim, uint32_t OutputChannel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OnePulseN_Stop(TIM_HandleTypeDef *htim, uint32_t OutputChannel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OnePulseN_Start_IT(TIM_HandleTypeDef *htim, uint32_t OutputChannel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_OnePulseN_Stop_IT(TIM_HandleTypeDef *htim, uint32_t OutputChannel)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_ConfigCommutationEvent(TIM_HandleTypeDef *htim, uint32_t InputTrigger, uint32_t CommutationSource)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_ConfigCommutationEvent_IT(TIM_HandleTypeDef *htim, uint32_t InputTrigger, uint32_t CommutationSource)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_ConfigCommutationEvent_DMA(TIM_HandleTypeDef *htim, uint32_t InputTrigger, uint32_t CommutationSource)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_ConfigBreakDeadTime(TIM_HandleTypeDef *htim,
+ TIM_BreakDeadTimeConfigTypeDef *sBreakDeadTimeConfig)
+{
+ return HAL_OK;
+}
+
+HAL_StatusTypeDef HAL_TIMEx_MasterConfigSynchronization(TIM_HandleTypeDef *htim, TIM_MasterConfigTypeDef * sMasterConfig)
+{
+ return HAL_OK;
+}
+
+__weak void HAL_TIMEx_CommutationCallback(TIM_HandleTypeDef *htim)
+{
+}
+
+__weak void HAL_TIMEx_BreakCallback(TIM_HandleTypeDef *htim)
+{
+}
+
+void TIMEx_DMACommutationCplt(DMA_HandleTypeDef *hdma)
+{
+}
+
+HAL_TIM_StateTypeDef HAL_TIMEx_HallSensor_GetState(TIM_HandleTypeDef *htim)
+{
+ return htim->State;
+}
+
+static void TIM_CCxNChannelCmd(TIM_TypeDef* TIMx, uint32_t Channel, uint32_t ChannelNState)
+{
+}
diff --git a/darwin_controller_cpp/src/sim/usart3.c b/darwin_controller_cpp/src/sim/usart3.c
new file mode 100644
index 0000000000000000000000000000000000000000..0a95cd7e2eeeac04ad4c8636b8f1f6b7985105a9
--- /dev/null
+++ b/darwin_controller_cpp/src/sim/usart3.c
@@ -0,0 +1,340 @@
+#include "usart3.h"
+
+#define USART USART3
+#define USART_ENABLE_CLK __HAL_RCC_USART3_CLK_ENABLE()
+#define USART_IRQn USART3_IRQn
+#define USART_IRQHandler USART3_IRQHandler
+
+#define USART_TX_PIN GPIO_PIN_10
+#define USART_TX_GPIO_PORT GPIOB
+#define USART_ENABLE_TX_GPIO_CLK __HAL_RCC_GPIOB_CLK_ENABLE()
+
+#define USART_RX_PIN GPIO_PIN_11
+#define USART_RX_GPIO_PORT GPIOB
+#define USART_ENABLE_RX_GPIO_CLK __HAL_RCC_GPIOB_CLK_ENABLE()
+
+/* DMA configuration */
+#define USART_DMA DMA1
+#define USART_ENABLE_DMA_CLK __HAL_RCC_DMA1_CLK_ENABLE()
+
+#define USART_TX_DMA_CHANNEL DMA1_Channel2
+#define USART_RX_DMA_CHANNEL DMA1_Channel3
+
+#define USART_DMA_TX_IRQn DMA1_Channel2_IRQn
+#define USART_DMA_RX_IRQn DMA1_Channel3_IRQn
+#define USART_DMA_TX_IRQHandler DMA1_Channel2_IRQHandler
+#define USART_DMA_RX_IRQHandler DMA1_Channel3_IRQHandler
+
+// private variables
+UART_HandleTypeDef Uart3Handle;
+DMA_HandleTypeDef usart3_hdma_tx;
+DMA_HandleTypeDef usart3_hdma_rx;
+TComm *usart3_comm_dev;
+
+// interrupt handlers
+void USART_IRQHandler(void)
+{
+ unsigned char data,ret;
+ uint32_t source;
+
+ if(__HAL_UART_GET_FLAG(&Uart3Handle, UART_FLAG_RXNE) != RESET)
+ {
+ if(__HAL_UART_GET_IT_SOURCE(&Uart3Handle, UART_IT_RXNE) !=RESET)
+ {
+ __HAL_UART_CLEAR_FLAG(&Uart3Handle,UART_FLAG_RXNE);
+ data=(uint8_t)(Uart3Handle.Instance->DR & (uint8_t)0x00FF);
+ // call the reception function
+ if(!comm_do_irq_receive(usart3_comm_dev,data))
+ __HAL_UART_DISABLE_IT(&Uart3Handle, UART_IT_RXNE);
+ }
+ }
+ if(__HAL_UART_GET_FLAG(&Uart3Handle, UART_FLAG_TC) != RESET)
+ {
+ if(__HAL_UART_GET_IT_SOURCE(&Uart3Handle, UART_IT_TC) !=RESET)
+ {
+ __HAL_UART_CLEAR_FLAG(&Uart3Handle,UART_FLAG_TC);
+ ret=comm_do_irq_send(usart3_comm_dev,&data);
+ if(ret==0x01)
+ Uart3Handle.Instance->DR=data;
+ else if(ret==0x00)
+ __HAL_UART_DISABLE_IT(&Uart3Handle, UART_IT_TC);
+ }
+ }
+ if(__HAL_UART_GET_FLAG(&Uart3Handle, UART_FLAG_PE) != RESET)// parity error
+ {
+ if(__HAL_UART_GET_IT_SOURCE(&Uart3Handle, UART_IT_PE) !=RESET)
+ {
+ __HAL_UART_CLEAR_PEFLAG(&Uart3Handle);
+ }
+ }
+ source=__HAL_UART_GET_IT_SOURCE(&Uart3Handle, UART_IT_ERR);
+ if(__HAL_UART_GET_FLAG(&Uart3Handle, UART_FLAG_FE) != RESET)// frame error
+ {
+ if(source !=RESET)
+ {
+ __HAL_UART_CLEAR_FEFLAG(&Uart3Handle);
+ }
+ }
+ if(__HAL_UART_GET_FLAG(&Uart3Handle, UART_FLAG_NE) != RESET)// noise error
+ {
+ if(source !=RESET)
+ {
+ __HAL_UART_CLEAR_NEFLAG(&Uart3Handle);
+ }
+ }
+ if(__HAL_UART_GET_FLAG(&Uart3Handle, UART_FLAG_ORE) != RESET)// overrun error
+ {
+ if(source !=RESET)
+ {
+ __HAL_UART_CLEAR_OREFLAG(&Uart3Handle);
+ }
+ }
+}
+
+void USART_DMA_TX_IRQHandler(void)
+{
+ if(__HAL_DMA_GET_FLAG(Uart3Handle.hdmatx,__HAL_DMA_GET_TE_FLAG_INDEX(Uart3Handle.hdmatx)) != RESET)
+ {
+ if(__HAL_DMA_GET_IT_SOURCE(Uart3Handle.hdmatx, DMA_IT_TE) != RESET)
+ {
+ /* Disable the transfer error interrupt */
+ __HAL_DMA_DISABLE_IT(Uart3Handle.hdmatx, DMA_IT_TE);
+ /* Clear the transfer error flag */
+ __HAL_DMA_CLEAR_FLAG(Uart3Handle.hdmatx, __HAL_DMA_GET_TE_FLAG_INDEX(Uart3Handle.hdmatx));
+ }
+ }
+ if(__HAL_DMA_GET_FLAG(Uart3Handle.hdmatx,__HAL_DMA_GET_TC_FLAG_INDEX(Uart3Handle.hdmatx)) != RESET)
+ {
+ if(__HAL_DMA_GET_IT_SOURCE(Uart3Handle.hdmatx, DMA_IT_TC) != RESET)
+ {
+ /* Disable the half transfer interrupt */
+ __HAL_DMA_DISABLE_IT(Uart3Handle.hdmatx, DMA_IT_TC);
+ /* Clear the transfer complete flag */
+ __HAL_DMA_CLEAR_FLAG(Uart3Handle.hdmatx, __HAL_DMA_GET_TC_FLAG_INDEX(Uart3Handle.hdmatx));
+ CLEAR_BIT(Uart3Handle.Instance->CR3, (USART_CR3_DMAT | USART_CR3_DMAR));
+ HAL_DMA_Abort(Uart3Handle.hdmatx);
+ // call the user function
+ comm_do_dma_send(usart3_comm_dev);
+ }
+ }
+ if(__HAL_DMA_GET_FLAG(Uart3Handle.hdmatx, __HAL_DMA_GET_HT_FLAG_INDEX(Uart3Handle.hdmatx)) != RESET)
+ {
+ if(__HAL_DMA_GET_IT_SOURCE(Uart3Handle.hdmatx, DMA_IT_HT) != RESET)
+ {
+ /* Disable the half transfer interrupt */
+ __HAL_DMA_DISABLE_IT(Uart3Handle.hdmatx, DMA_IT_HT);
+ /* Clear the half transfer complete flag */
+ __HAL_DMA_CLEAR_FLAG(Uart3Handle.hdmatx, __HAL_DMA_GET_HT_FLAG_INDEX(Uart3Handle.hdmatx));
+ }
+ }
+}
+
+void USART_DMA_RX_IRQHandler(void)
+{
+ if(__HAL_DMA_GET_FLAG(Uart3Handle.hdmarx,__HAL_DMA_GET_TE_FLAG_INDEX(Uart3Handle.hdmarx)) != RESET)
+ {
+ if(__HAL_DMA_GET_IT_SOURCE(Uart3Handle.hdmarx, DMA_IT_TE) != RESET)
+ {
+ /* Disable the transfer error interrupt */
+ __HAL_DMA_DISABLE_IT(Uart3Handle.hdmarx, DMA_IT_TE);
+ /* Clear the transfer error flag */
+ __HAL_DMA_CLEAR_FLAG(Uart3Handle.hdmarx, __HAL_DMA_GET_TE_FLAG_INDEX(Uart3Handle.hdmarx));
+ }
+ }
+ if(__HAL_DMA_GET_FLAG(Uart3Handle.hdmarx,__HAL_DMA_GET_TC_FLAG_INDEX(Uart3Handle.hdmarx)) != RESET)
+ {
+ if(__HAL_DMA_GET_IT_SOURCE(Uart3Handle.hdmarx, DMA_IT_TC) != RESET)
+ {
+ /* Disable the transfer complete interrupt */
+ __HAL_DMA_DISABLE_IT(Uart3Handle.hdmarx, DMA_IT_TC);
+ /* Clear the transfer complete flag */
+ __HAL_DMA_CLEAR_FLAG(Uart3Handle.hdmarx, __HAL_DMA_GET_TC_FLAG_INDEX(Uart3Handle.hdmarx));
+ CLEAR_BIT(Uart3Handle.Instance->CR3, (USART_CR3_DMAT | USART_CR3_DMAR));
+ HAL_DMA_Abort(Uart3Handle.hdmarx);
+ // call the user function
+ comm_do_dma_receive(usart3_comm_dev);
+ }
+ }
+ if(__HAL_DMA_GET_FLAG(Uart3Handle.hdmarx, __HAL_DMA_GET_HT_FLAG_INDEX(Uart3Handle.hdmarx)) != RESET)
+ {
+ if(__HAL_DMA_GET_IT_SOURCE(Uart3Handle.hdmarx, DMA_IT_HT) != RESET)
+ {
+ /* Disable the half transfer interrupt */
+ __HAL_DMA_DISABLE_IT(Uart3Handle.hdmarx, DMA_IT_HT);
+ /* Clear the half transfer complete flag */
+ __HAL_DMA_CLEAR_FLAG(Uart3Handle.hdmarx, __HAL_DMA_GET_HT_FLAG_INDEX(Uart3Handle.hdmarx));
+ }
+ }
+}
+
+/* public functions*/
+void usart3_init(TComm *comm_dev,UART_InitTypeDef *conf,TUSART_IRQ_Priorities *priorities)
+{
+ GPIO_InitTypeDef GPIO_InitStructure;
+
+ /* Enable GPIO clock */
+ USART_ENABLE_TX_GPIO_CLK;
+ USART_ENABLE_RX_GPIO_CLK;
+ USART_ENABLE_DMA_CLK;
+ // configure the GPIO pins
+
+ /* Configure USART Tx and Rx as alternate function push-pull */
+ GPIO_InitStructure.Pin = USART_TX_PIN;
+ GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStructure.Pull = GPIO_PULLUP;
+ GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
+ HAL_GPIO_Init(USART_TX_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.Pin = USART_RX_PIN;
+ GPIO_InitStructure.Mode = GPIO_MODE_INPUT;
+ HAL_GPIO_Init(USART_RX_GPIO_PORT, &GPIO_InitStructure);
+
+ USART_ENABLE_CLK;
+
+ Uart3Handle.Instance = USART;
+ usart3_config(comm_dev,conf);
+
+ if(comm_dev->use_dma)
+ {
+ // configure the DMA channels
+ usart3_hdma_tx.Instance = USART_TX_DMA_CHANNEL;
+ usart3_hdma_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
+ usart3_hdma_tx.Init.PeriphInc = DMA_PINC_DISABLE;
+ usart3_hdma_tx.Init.MemInc = DMA_MINC_ENABLE;
+ usart3_hdma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ usart3_hdma_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ usart3_hdma_tx.Init.Mode = DMA_NORMAL;
+ usart3_hdma_tx.Init.Priority = DMA_PRIORITY_LOW;
+
+ HAL_DMA_Init(&usart3_hdma_tx);
+
+ /* Associate the initialized DMA handle to the UART handle */
+ __HAL_LINKDMA(&Uart3Handle, hdmatx, usart3_hdma_tx);
+
+ /* Configure the DMA handler for reception process */
+ usart3_hdma_rx.Instance = USART_RX_DMA_CHANNEL;
+ usart3_hdma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
+ usart3_hdma_rx.Init.PeriphInc = DMA_PINC_DISABLE;
+ usart3_hdma_rx.Init.MemInc = DMA_MINC_ENABLE;
+ usart3_hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ usart3_hdma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ usart3_hdma_rx.Init.Mode = DMA_NORMAL;
+ usart3_hdma_rx.Init.Priority = DMA_PRIORITY_HIGH;
+
+ HAL_DMA_Init(&usart3_hdma_rx);
+
+ /* Associate the initialized DMA handle to the the UART handle */
+ __HAL_LINKDMA(&Uart3Handle, hdmarx, usart3_hdma_rx);
+ }
+ usart3_set_priorities(comm_dev,priorities);
+
+ /* Initialize the comm structure */
+ comm_dev->send_irq=usart3_send_irq;
+ comm_dev->enable_tx_irq=usart3_enable_tx_irq;
+ comm_dev->receive_irq=usart3_receive_irq;
+ comm_dev->cancel_receive_irq=usart3_cancel_receive_irq;
+ if(comm_dev->use_dma)
+ {
+ comm_dev->send_dma=usart3_send_dma;
+ comm_dev->receive_dma=usart3_receive_dma;
+ comm_dev->cancel_receive_dma=usart3_cancel_receive_dma;
+ }
+ comm_dev->irq_send_cb=0x00000000;
+ comm_dev->irq_receive_cb=0x00000000;
+ comm_dev->dma_send_cb=0x00000000;
+ comm_dev->dma_receive_cb=0x00000000;
+
+ /* initialize internal variables */
+ usart3_comm_dev=comm_dev;
+}
+
+void usart3_config(TComm *comm_dev,UART_InitTypeDef *conf)
+{
+ Uart3Handle.Init.BaudRate = conf->BaudRate;
+ Uart3Handle.Init.WordLength = conf->WordLength;
+ Uart3Handle.Init.StopBits = conf->StopBits;
+ Uart3Handle.Init.Parity = conf->Parity;
+ Uart3Handle.Init.Mode = conf->Mode;
+ Uart3Handle.Init.HwFlowCtl = conf->HwFlowCtl;
+ Uart3Handle.Init.OverSampling = conf->OverSampling;
+ HAL_UART_Init(&Uart3Handle);
+ // set the actual baudrate of the dynamixel slave
+
+}
+
+void usart3_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities)
+{
+ HAL_NVIC_SetPriority(USART_IRQn, priorities->irq_priority,priorities->irq_subpriority);
+ HAL_NVIC_EnableIRQ(USART_IRQn);
+ if(comm_dev->use_dma)
+ {
+ HAL_NVIC_SetPriority(USART_DMA_TX_IRQn, priorities->dma_tx_priority,priorities->dma_tx_subpriority);
+ HAL_NVIC_EnableIRQ(USART_DMA_TX_IRQn);
+ HAL_NVIC_SetPriority(USART_DMA_RX_IRQn, priorities->dma_rx_priority,priorities->dma_rx_subpriority);
+ HAL_NVIC_EnableIRQ(USART_DMA_RX_IRQn);
+ }
+}
+
+/* IRQ functions */
+unsigned char usart3_send_irq(unsigned char first_byte)
+{
+ __HAL_UART_CLEAR_FLAG(&Uart3Handle,UART_FLAG_TC);
+ __HAL_UART_ENABLE_IT(&Uart3Handle, UART_IT_TC);
+ Uart3Handle.Instance->DR=first_byte;
+
+ return 0x00;
+}
+
+unsigned char usart3_enable_tx_irq(void)
+{
+ __HAL_UART_CLEAR_FLAG(&Uart3Handle,UART_FLAG_TC);
+ __HAL_UART_ENABLE_IT(&Uart3Handle, UART_IT_TC);
+
+ return 0x00;
+}
+
+unsigned char usart3_receive_irq(void)
+{
+ /* enable the rx interrupt */
+ __HAL_UART_ENABLE_IT(&Uart3Handle, UART_IT_RXNE);
+
+ return 0x00;
+}
+
+unsigned char usart3_cancel_receive_irq(void)
+{
+ /* disable the rx interrupt */
+ __HAL_UART_DISABLE_IT(&Uart3Handle, UART_IT_RXNE);
+
+ return 0x00;
+}
+
+/* DMA functions */
+unsigned char usart3_send_dma(unsigned char *data,unsigned short int length)
+{
+ HAL_DMA_Start_IT(Uart3Handle.hdmatx,(uint32_t)data,(uint32_t)&Uart3Handle.Instance->DR,length);
+ /* Clear the TC flag in the SR register by writing 0 to it */
+ __HAL_UART_CLEAR_FLAG(&Uart3Handle,UART_FLAG_TC);
+ /* Enable the DMA transfer for transmit request by setting the DMAT bit
+ in the UART CR3 register */
+ SET_BIT(Uart3Handle.Instance->CR3, USART_CR3_DMAT);
+
+ return 0x00;
+}
+
+unsigned char usart3_receive_dma(unsigned char *data,unsigned short int length)
+{
+ HAL_DMA_Start_IT(Uart3Handle.hdmarx,(uint32_t)&Uart3Handle.Instance->DR,(uint32_t)data,length);
+ SET_BIT(Uart3Handle.Instance->CR3, USART_CR3_DMAR);
+
+ return 0x00;
+}
+
+unsigned char usart3_cancel_receive_dma(void)
+{
+ CLEAR_BIT(Uart3Handle.Instance->CR3, (USART_CR3_DMAT | USART_CR3_DMAR));
+ HAL_DMA_Abort(Uart3Handle.hdmarx);
+
+ return 0x00;
+}
+
diff --git a/darwin_controller_cpp/src/xml/CMakeLists.txt b/darwin_controller_cpp/src/xml/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6560fa12984259902807c301bd3f55d4534e770b
--- /dev/null
+++ b/darwin_controller_cpp/src/xml/CMakeLists.txt
@@ -0,0 +1,45 @@
+#check the existance of the xsd library
+IF(EXISTS "/usr/include/xsd/cxx")
+ SET(HAVE_XSD TRUE PARENT_SCOPE)
+ SET(XSD_FOUND TRUE)
+ MESSAGE(STATUS "Found the XML library ... adding support for XML files")
+ FIND_LIBRARY(XSD_LIBRARY
+ NAMES xerces-c
+ PATHS /usr/lib /usr/local/lib)
+ELSE(EXISTS "/usr/include/xsd/cxx")
+ MESSAGE(STATUS "XML library not found ... it will be impossible to handle XML files")
+ENDIF(EXISTS "/usr/include/xsd/cxx")
+
+IF(XSD_FOUND)
+ SET(XSD_LIBRARY ${XSD_LIBRARY} PARENT_SCOPE)
+
+ SET(XSD_PATH ${CMAKE_CURRENT_SOURCE_DIR})
+ SET(XSD_FILES darwin_config.xsd)
+
+ IF(XSD_FILES)
+ FOREACH(xsd_file ${XSD_FILES})
+ STRING(REGEX REPLACE "xsd" "cxx" xsd_source ${xsd_file})
+ SET(XSD_SOURCES_INT ${XSD_SOURCES_INT} ${XSD_PATH}/${xsd_source})
+ SET(XSD_SOURCES ${XSD_SOURCES} ${XSD_PATH}/${xsd_source})
+ STRING(REGEX REPLACE "xsd" "hxx" xsd_header ${xsd_file})
+ SET(XSD_HEADERS_INT ${XSD_HEADERS_INT} ${XSD_PATH}/${xsd_header})
+ SET(XSD_HEADERS ${XSD_HEADERS} ${XSD_PATH}/${xsd_header})
+ SET(XSD_PATH_FILES ${XSD_PATH_FILES} ${XSD_PATH}/${xsd_file})
+ ENDFOREACH(xsd_file)
+
+ SET(XSD_SOURCES ${XSD_SOURCES_INT} PARENT_SCOPE)
+ SET(XSD_HEADERS ${XSD_HEADERS_INT} PARENT_SCOPE)
+
+ ADD_CUSTOM_TARGET(xsd_files_gen DEPENDS ${XSD_SOURCES_INT})
+ ADD_CUSTOM_COMMAND(
+ OUTPUT ${XSD_SOURCES_INT}
+ COMMAND xsdcxx cxx-tree --generate-serialization ${XSD_FILES}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+ DEPENDS ${XSD_PATH_FILES}
+ COMMENT "Parsing the xml template file ${XSD_FILES}")
+
+ INSTALL(FILES ${XSD_PATH_FILES} DESTINATION include/iridrivers/xml)
+ INSTALL(FILES ${XSD_HEADERS_INT} DESTINATION include/iridrivers/xml)
+ ENDIF(XSD_FILES)
+ENDIF(XSD_FOUND)
+
diff --git a/darwin_controller_cpp/src/xml/darwin_action.xml b/darwin_controller_cpp/src/xml/darwin_action.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9ab38f005552f7aeb999deb24b95aec09b652636
--- /dev/null
+++ b/darwin_controller_cpp/src/xml/darwin_action.xml
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+
+<darwin_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="darwin_config.xsd">
+ <servo>
+ <name>j_shoulder_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_shoulder_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_pan</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_tilt</name>
+ <motion_module>action</motion_module>
+ </servo>
+</darwin_config>
+
diff --git a/darwin_controller_cpp/src/xml/darwin_config.xml b/darwin_controller_cpp/src/xml/darwin_config.xml
new file mode 100644
index 0000000000000000000000000000000000000000..33f3cef45b544d5659ad9b81cf05b3c079b1eda9
--- /dev/null
+++ b/darwin_controller_cpp/src/xml/darwin_config.xml
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+
+<darwin_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="darwin_config.xsd">
+ <servo>
+ <name>j_shoulder_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_shoulder_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_pan</name>
+ <motion_module>head</motion_module>
+ </servo>
+ <servo>
+ <name>j_tilt</name>
+ <motion_module>head</motion_module>
+ </servo>
+</darwin_config>
+
diff --git a/darwin_controller_cpp/src/xml/darwin_config.xsd b/darwin_controller_cpp/src/xml/darwin_config.xsd
new file mode 100644
index 0000000000000000000000000000000000000000..94dde44dfaed295d354cfc8ec992c0506909bc55
--- /dev/null
+++ b/darwin_controller_cpp/src/xml/darwin_config.xsd
@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+
+<!--
+
+file : darwin_config.xsd
+author : Sergi Hernandez Juan (shernand@iri.upc.edu)
+copyright : not copyrighted - public domain
+
+-->
+
+<xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema">
+ <xsd:simpleType name="motion_module_t">
+ <xsd:restriction base="xsd:string">
+ <xsd:enumeration value="action"/>
+ <xsd:enumeration value="walking"/>
+ <xsd:enumeration value="joints"/>
+ <xsd:enumeration value="head"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:simpleType name="servo_name_t">
+ <xsd:restriction base="xsd:string">
+ <xsd:enumeration value="j_shoulder_r"/>
+ <xsd:enumeration value="j_shoulder_l"/>
+ <xsd:enumeration value="j_high_arm_r"/>
+ <xsd:enumeration value="j_high_arm_l"/>
+ <xsd:enumeration value="j_low_arm_r"/>
+ <xsd:enumeration value="j_low_arm_l"/>
+ <xsd:enumeration value="j_pelvis_r"/>
+ <xsd:enumeration value="j_pelvis_l"/>
+ <xsd:enumeration value="j_thigh1_r"/>
+ <xsd:enumeration value="j_thigh1_l"/>
+ <xsd:enumeration value="j_thigh2_r"/>
+ <xsd:enumeration value="j_thigh2_l"/>
+ <xsd:enumeration value="j_tibia_r"/>
+ <xsd:enumeration value="j_tibia_l"/>
+ <xsd:enumeration value="j_ankle1_r"/>
+ <xsd:enumeration value="j_ankle1_l"/>
+ <xsd:enumeration value="j_ankle2_r"/>
+ <xsd:enumeration value="j_ankle2_l"/>
+ <xsd:enumeration value="j_pan"/>
+ <xsd:enumeration value="j_tilt"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:complexType name="servo_config_t">
+ <xsd:sequence>
+ <xsd:element name="name" type="servo_name_t">
+ </xsd:element>
+ <xsd:element name="motion_module" type="motion_module_t">
+ </xsd:element>
+ </xsd:sequence>
+ </xsd:complexType>
+
+ <xsd:complexType name="darwin_config_t">
+ <xsd:sequence>
+ <xsd:element name="servo" type="servo_config_t" maxOccurs="unbounded">
+ </xsd:element>
+ </xsd:sequence>
+ </xsd:complexType>
+
+ <xsd:element name="darwin_config" type="darwin_config_t">
+ </xsd:element>
+
+</xsd:schema>
+
diff --git a/darwin_controller_cpp/src/xml/darwin_head.xml b/darwin_controller_cpp/src/xml/darwin_head.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1a848a78cf59e66be015eda5d3d9dbab94e975b4
--- /dev/null
+++ b/darwin_controller_cpp/src/xml/darwin_head.xml
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+
+<darwin_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="darwin_config.xsd">
+ <servo>
+ <name>j_shoulder_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_shoulder_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_r</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_l</name>
+ <motion_module>action</motion_module>
+ </servo>
+ <servo>
+ <name>j_pan</name>
+ <motion_module>head</motion_module>
+ </servo>
+ <servo>
+ <name>j_tilt</name>
+ <motion_module>head</motion_module>
+ </servo>
+</darwin_config>
+
diff --git a/darwin_controller_cpp/src/xml/darwin_joints.xml b/darwin_controller_cpp/src/xml/darwin_joints.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f79375fad8481137724efcf6270b61705f11800b
--- /dev/null
+++ b/darwin_controller_cpp/src/xml/darwin_joints.xml
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+
+<darwin_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="darwin_config.xsd">
+ <servo>
+ <name>j_shoulder_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_shoulder_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_r</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_l</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_pan</name>
+ <motion_module>joints</motion_module>
+ </servo>
+ <servo>
+ <name>j_tilt</name>
+ <motion_module>joints</motion_module>
+ </servo>
+</darwin_config>
+
diff --git a/darwin_controller_cpp/src/xml/darwin_walk.xml b/darwin_controller_cpp/src/xml/darwin_walk.xml
new file mode 100644
index 0000000000000000000000000000000000000000..7e46b8cb9d6f756307a933f23e1459c30da4e708
--- /dev/null
+++ b/darwin_controller_cpp/src/xml/darwin_walk.xml
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+
+<darwin_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="darwin_config.xsd">
+ <servo>
+ <name>j_shoulder_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_shoulder_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_high_arm_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_low_arm_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_pelvis_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh1_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_thigh2_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_tibia_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle1_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_r</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_ankle2_l</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_pan</name>
+ <motion_module>walking</motion_module>
+ </servo>
+ <servo>
+ <name>j_tilt</name>
+ <motion_module>walking</motion_module>
+ </servo>
+</darwin_config>
+
diff --git a/darwin_description/config/darwin_rviz.rviz b/darwin_description/config/darwin_rviz.rviz
index 3781f56c23b436652639f5f3510994755ddba1e5..39d7dc4275669652fb9e57475023a76ff454d021 100644
--- a/darwin_description/config/darwin_rviz.rviz
+++ b/darwin_description/config/darwin_rviz.rviz
@@ -9,7 +9,7 @@ Panels:
- /TF1/Frames1
- /TF1/Tree1
- /Environment1
- Splitter Ratio: 0.744966
+ Splitter Ratio: 0.744965971
Tree Height: 655
- Class: rviz/Selection
Name: Selection
@@ -19,7 +19,7 @@ Panels:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
- Splitter Ratio: 0.588679
+ Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
@@ -30,6 +30,8 @@ Panels:
Name: Time
SyncMode: 0
SyncSource: ""
+Toolbars:
+ toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
@@ -39,7 +41,7 @@ Visualization Manager:
Color: 160; 160; 164
Enabled: true
Line Style:
- Line Width: 0.03
+ Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
@@ -75,6 +77,11 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
+ darwin_cam_optical_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
darwin_gyro:
Alpha: 1
Show Axes: false
@@ -195,6 +202,58 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: true
+ E125:
+ Value: true
+ E175:
+ Value: true
+ E225:
+ Value: true
+ E25:
+ Value: true
+ E75:
+ Value: true
+ N125:
+ Value: true
+ N125_cam:
+ Value: true
+ N125_det:
+ Value: true
+ N175:
+ Value: true
+ N175_cam:
+ Value: true
+ N175_det:
+ Value: true
+ N25:
+ Value: true
+ N25_cam:
+ Value: true
+ N25_det:
+ Value: true
+ N75:
+ Value: true
+ N75_cam:
+ Value: true
+ N75_det:
+ Value: true
+ S125:
+ Value: true
+ S175:
+ Value: true
+ S25:
+ Value: true
+ S75:
+ Value: true
+ W125:
+ Value: true
+ W175:
+ Value: true
+ W225:
+ Value: true
+ W25:
+ Value: true
+ W75:
+ Value: true
darwin/base_footprint:
Value: true
darwin/base_link:
@@ -203,6 +262,8 @@ Visualization Manager:
Value: true
darwin/darwin_cam_link:
Value: true
+ darwin/darwin_cam_optical_link:
+ Value: true
darwin/darwin_gyro:
Value: true
darwin/darwin_imu_link:
@@ -229,6 +290,8 @@ Visualization Manager:
Value: true
darwin/neck:
Value: true
+ darwin/odom:
+ Value: true
darwin/right_arm_high:
Value: true
darwin/right_arm_low:
@@ -247,49 +310,146 @@ Visualization Manager:
Value: true
darwin/right_shoulder:
Value: true
- Marker Scale: 0.2
+ obstacle_base_fence_link:
+ Value: true
+ obstacle_base_link:
+ Value: true
+ obstacles_env_empty/E125:
+ Value: true
+ obstacles_env_empty/E175:
+ Value: true
+ obstacles_env_empty/E225:
+ Value: true
+ obstacles_env_empty/E25:
+ Value: true
+ obstacles_env_empty/E75:
+ Value: true
+ obstacles_env_empty/N125:
+ Value: true
+ obstacles_env_empty/N175:
+ Value: true
+ obstacles_env_empty/N25:
+ Value: true
+ obstacles_env_empty/N75:
+ Value: true
+ obstacles_env_empty/S125:
+ Value: true
+ obstacles_env_empty/S175:
+ Value: true
+ obstacles_env_empty/S25:
+ Value: true
+ obstacles_env_empty/S75:
+ Value: true
+ obstacles_env_empty/W125:
+ Value: true
+ obstacles_env_empty/W175:
+ Value: true
+ obstacles_env_empty/W225:
+ Value: true
+ obstacles_env_empty/W25:
+ Value: true
+ obstacles_env_empty/W75:
+ Value: true
+ obstacles_env_empty/obstacle_base_fence_link:
+ Value: true
+ obstacles_env_empty/obstacle_base_link:
+ Value: true
+ Marker Scale: 0.200000003
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
- darwin/base_link:
- darwin/darwin_accel:
+ obstacle_base_link:
+ E125:
+ {}
+ E175:
+ {}
+ E225:
{}
- darwin/darwin_gyro:
+ E25:
{}
- darwin/darwin_imu_link:
+ E75:
{}
- darwin/left_leg_pelvis_yaw:
- darwin/left_leg_pelvis_roll:
- darwin/left_leg_pelvis_pitch:
- darwin/left_leg_knee:
- darwin/left_leg_ankle_pitch:
- darwin/left_leg_ankle_roll:
+ N125:
+ {}
+ N175:
+ {}
+ N25:
+ {}
+ N75:
+ {}
+ S125:
+ {}
+ S175:
+ {}
+ S25:
+ {}
+ S75:
+ {}
+ W125:
+ {}
+ W175:
+ {}
+ W225:
+ {}
+ W25:
+ {}
+ W75:
+ {}
+ darwin/odom:
+ darwin/base_footprint:
+ darwin/base_link:
+ darwin/darwin_accel:
+ {}
+ darwin/darwin_gyro:
+ {}
+ darwin/darwin_imu_link:
+ {}
+ darwin/left_leg_pelvis_yaw:
+ darwin/left_leg_pelvis_roll:
+ darwin/left_leg_pelvis_pitch:
+ darwin/left_leg_knee:
+ darwin/left_leg_ankle_pitch:
+ darwin/left_leg_ankle_roll:
+ {}
+ darwin/left_shoulder:
+ darwin/left_arm_high:
+ darwin/left_arm_low:
{}
- darwin/left_shoulder:
- darwin/left_arm_high:
- darwin/left_arm_low:
- darwin/base_link:
- darwin/darwin_accel:
- {}
- darwin/darwin_gyro:
- {}
- darwin/neck:
- darwin/head:
- darwin/darwin_cam_link:
- {}
- darwin/right_leg_pelvis_yaw:
- darwin/right_leg_pelvis_roll:
- darwin/right_leg_pelvis_pitch:
- darwin/right_leg_knee:
- darwin/right_leg_ankle_pitch:
- darwin/right_leg_ankle_roll:
+ darwin/neck:
+ darwin/head:
+ darwin/darwin_cam_link:
+ darwin/darwin_cam_optical_link:
+ N125_cam:
+ {}
+ N125_det:
+ {}
+ N175_cam:
+ {}
+ N175_det:
+ {}
+ N25_cam:
+ {}
+ N25_det:
+ {}
+ N75_cam:
+ {}
+ N75_det:
+ {}
+ darwin/right_leg_pelvis_yaw:
+ darwin/right_leg_pelvis_roll:
+ darwin/right_leg_pelvis_pitch:
+ darwin/right_leg_knee:
+ darwin/right_leg_ankle_pitch:
+ darwin/right_leg_ankle_roll:
+ {}
+ darwin/right_shoulder:
+ darwin/right_arm_high:
+ darwin/right_arm_low:
{}
- darwin/right_shoulder:
- darwin/right_arm_high:
- darwin/right_arm_low:
- {}
+ obstacle_base_fence_link:
+ {}
Update Interval: 0
Value: true
- Alpha: 1
@@ -298,12 +458,112 @@ Visualization Manager:
Enabled: true
Links:
All Links Enabled: true
+ E125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E225:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
+ N125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ N175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ N25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ N75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W225:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ obstacle_base_fence_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ obstacle_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
Name: Environment
- Robot Description: obstacles_environment
+ Robot Description: /obstacles_env_empty/robot_description
TF Prefix: ""
Update Interval: 0
Value: true
@@ -311,7 +571,8 @@ Visualization Manager:
Enabled: true
Global Options:
Background Color: 48; 48; 48
- Fixed Frame: darwin/base_link
+ Default Light: true
+ Fixed Frame: obstacle_base_link
Frame Rate: 30
Name: root
Tools:
@@ -332,22 +593,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 4.15359
+ Distance: 11.0277748
Enable Stereo Rendering:
- Stereo Eye Separation: 0.06
+ Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 0.625211
- Y: -0.0443803
- Z: 0.189017
+ X: 1.99065769
+ Y: 0.666042745
+ Z: 1.47241056
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
Name: Current View
- Near Clip Distance: 0.01
- Pitch: 0.224797
+ Near Clip Distance: 0.00999999978
+ Pitch: 0.325200558
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
- Yaw: 5.70045
+ Yaw: 2.7204318
Saved: ~
Window Geometry:
Displays:
@@ -355,7 +619,7 @@ Window Geometry:
Height: 936
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd0000000400000000000001ce0000031efc0200000007fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000031e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000031efc0200000004fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000000280000006e0000006400fffffffb0000000a00560069006500770073010000009c000002aa000000b000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000031b000000a30000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000001d60000031e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000400000000000001ce0000031efc0200000007fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000031e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000031efc0200000004fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000000280000006e0000006100fffffffb0000000a00560069006500770073010000009c000002aa000000ad00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000031b000000a30000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000001d60000031e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
diff --git a/darwin_description/launch/charge_env.launch b/darwin_description/launch/charge_env.launch
deleted file mode 100644
index 7e24a80fe29339d03b6425c2cebc677ff2f39987..0000000000000000000000000000000000000000
--- a/darwin_description/launch/charge_env.launch
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
- <arg name="environment" default="charge_env" />
-
- <!-- Convert an xacro and put on parameter server -->
- <param name="charge_environment"
- command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/charge/$(arg environment).xacro'" />
-
- <node name="spawn_charge_station" pkg="gazebo_ros" type="spawn_model" args="-param charge_environment -urdf -model charge_station -x 1.0 -y 0.25 -z 0.03 -R 0 -P 0 -Y 3.14159"/>
-
-</launch>
diff --git a/darwin_description/launch/darwin_base.launch b/darwin_description/launch/darwin_base.launch
index fa75b58b38819bb6a68434f0cb86735cdf127c00..36cbd3d110b126c4ed21570cc64d644f3c07cf16 100644
--- a/darwin_description/launch/darwin_base.launch
+++ b/darwin_description/launch/darwin_base.launch
@@ -5,7 +5,7 @@
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/$(arg robot).xacro'" />
+ command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/$(arg robot).xacro'" />
<node pkg="robot_state_publisher" type="state_publisher" name="darwin_tf_broadcaster">
<param name="tf_prefix" type="string" value="/darwin"/>
diff --git a/darwin_description/launch/darwin_base_sim.launch b/darwin_description/launch/darwin_base_sim.launch
index 9d14760bf71bcae3d69ff8328363351bbea2ff99..e5d86f1fc812e313a4de9a385e12091618a26862 100644
--- a/darwin_description/launch/darwin_base_sim.launch
+++ b/darwin_description/launch/darwin_base_sim.launch
@@ -4,7 +4,7 @@
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/$(arg robot).xacro'" />
+ command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/$(arg robot).xacro'" />
<node pkg="robot_state_publisher" type="state_publisher" name="darwin_tf_broadcaster">
<param name="tf_prefix" type="string" value="/darwin"/>
@@ -17,5 +17,6 @@
<node name="rviz"
pkg="rviz"
type="rviz"
- args="-d $(find darwin_description)/config/darwin_rviz.rviz"/>
+ args="-d $(find darwin_nav)/config/darwin_nav.rviz"/>
+<!-- args="-d $(find darwin_description)/config/darwin_rviz.rviz"/>-->
</launch>
diff --git a/darwin_description/launch/darwin_sim.launch b/darwin_description/launch/darwin_sim.launch
index 6224f0966d7928d15d5823ff27a9fccacb160dab..0bf1a44c5e24ca6f2214943bc880aab641211abc 100644
--- a/darwin_description/launch/darwin_sim.launch
+++ b/darwin_description/launch/darwin_sim.launch
@@ -6,7 +6,16 @@
<arg name="robot" value="$(arg robot)" />
</include>
- <include file="$(find darwin_control)/launch/darwin_control.launch"/>
+ <include file="$(find darwin_control)/launch/$(arg robot)_control.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <group ns="darwin">
+ <include file="$(find darwin_driver)/launch/darwin.launch">
+ <arg name="device" value="/dev/pts/24"/>
+ <arg name="simulate" value="True"/>
+ </include>
+ </group>
</launch>
diff --git a/darwin_description/launch/darwin_test.launch b/darwin_description/launch/darwin_test.launch
index 0e5370288f2c1581f866552410b4679385c15d67..dc97347bcc47cf80c0e0aef955078915a7e2fd86 100644
--- a/darwin_description/launch/darwin_test.launch
+++ b/darwin_description/launch/darwin_test.launch
@@ -4,7 +4,7 @@
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/$(arg robot).xacro'" />
+ command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/$(arg robot).xacro'" />
<node pkg="robot_state_publisher" type="state_publisher" name="darwin_tf_broadcaster">
<param name="tf_prefix" type="string" value="/darwin"/>
diff --git a/darwin_description/launch/environment.launch b/darwin_description/launch/environment.launch
new file mode 100644
index 0000000000000000000000000000000000000000..bf5d15c835d26ebd8ac33daf5270ec3cf868ae9b
--- /dev/null
+++ b/darwin_description/launch/environment.launch
@@ -0,0 +1,16 @@
+<launch>
+
+ <group ns="charge_env">
+ <!-- Convert an xacro and put on parameter server -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro --inorder '$(find bioloid_description)/urdf/ceabot/$(arg environment).xacro'" />
+
+ <node pkg="robot_state_publisher" type="state_publisher" name="env_tf_broadcaster">
+ <param name="tf_prefix" type="string" value="/$(arg environment)"/>
+ <param name="publish_frequency" type="double" value="100.0"/>
+ </node>
+
+ <node name="spawn_charge_station" pkg="gazebo_ros" type="spawn_model" args="-param /$(arg environment)/robot_description -urdf -model $(arg environment) -x 1.0 -y 0.25 -z 0.03 -R 0 -P 0 -Y 3.14159"/>
+ </group>
+
+</launch>
diff --git a/darwin_description/meshes/bounding_boxes/hand_base_sc_bb.stl b/darwin_description/meshes/bounding_boxes/hand_base_sc_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..caf568c065f949758bc3bd2d375d510f084ab69e
Binary files /dev/null and b/darwin_description/meshes/bounding_boxes/hand_base_sc_bb.stl differ
diff --git a/darwin_description/meshes/bounding_boxes/hand_fingers_sc_bb.stl b/darwin_description/meshes/bounding_boxes/hand_fingers_sc_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e824c67409dd4375fef6d6d23fe4c33bfc83cc99
Binary files /dev/null and b/darwin_description/meshes/bounding_boxes/hand_fingers_sc_bb.stl differ
diff --git a/darwin_description/meshes/charge/connector.stl b/darwin_description/meshes/charge/connector.stl
old mode 100755
new mode 100644
index 27fa7fe0987a890003b936267a52c694aec85e58..2eaa0eb092a5e2888abbc10b424dfeaa01dd2d0b
Binary files a/darwin_description/meshes/charge/connector.stl and b/darwin_description/meshes/charge/connector.stl differ
diff --git a/darwin_description/meshes/charge/far_qr.stl b/darwin_description/meshes/charge/far_qr.stl
old mode 100755
new mode 100644
index 201896091af9f860b8c06b31436aaf4ab914b3e5..0b6f223ff101073b89220fec5f12eb1e4af1554e
Binary files a/darwin_description/meshes/charge/far_qr.stl and b/darwin_description/meshes/charge/far_qr.stl differ
diff --git a/darwin_description/meshes/charge/near_qr.stl b/darwin_description/meshes/charge/near_qr.stl
old mode 100755
new mode 100644
index c229ea9b16b0d5a466ae346d8c9129ad0c291551..0a602a61ff3031935a3ad366fc549da1e91a3a34
Binary files a/darwin_description/meshes/charge/near_qr.stl and b/darwin_description/meshes/charge/near_qr.stl differ
diff --git a/darwin_description/meshes/hand_base_sc.stl b/darwin_description/meshes/hand_base_sc.stl
new file mode 100644
index 0000000000000000000000000000000000000000..13e5ee1b5aa8fa2bc8badf42e29b171594fecd74
Binary files /dev/null and b/darwin_description/meshes/hand_base_sc.stl differ
diff --git a/darwin_description/meshes/hand_fingers_sc.stl b/darwin_description/meshes/hand_fingers_sc.stl
new file mode 100644
index 0000000000000000000000000000000000000000..9c78b604ab2e2ec4910f00af8dbcf56dc2d1b466
Binary files /dev/null and b/darwin_description/meshes/hand_fingers_sc.stl differ
diff --git a/darwin_description/meshes/left_sensor_foot.stl b/darwin_description/meshes/left_sensor_foot.stl
new file mode 100755
index 0000000000000000000000000000000000000000..4a66c1fdfdf9b25c1692d333c8e009bfc85287bc
Binary files /dev/null and b/darwin_description/meshes/left_sensor_foot.stl differ
diff --git a/darwin_description/meshes/right_sensor_foot.stl b/darwin_description/meshes/right_sensor_foot.stl
new file mode 100755
index 0000000000000000000000000000000000000000..066ac5c00776d6d4a0e687c2137ab50c5caec843
Binary files /dev/null and b/darwin_description/meshes/right_sensor_foot.stl differ
diff --git a/darwin_description/urdf/charge/charge_env.xacro b/darwin_description/urdf/charge/charge_env.xacro
index c2dc91d50fd028e9442c1216fd1fd5b5d2413bc2..f6606926ac2faf69cd41f1e0dd4674286cdbdced 100755
--- a/darwin_description/urdf/charge/charge_env.xacro
+++ b/darwin_description/urdf/charge/charge_env.xacro
@@ -1,5 +1,6 @@
<robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <xacro:include filename="$(find bioloid_description)/urdf/materials.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle_base.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/charge/charge_station.xacro" />
diff --git a/darwin_description/urdf/charge/charge_station.xacro b/darwin_description/urdf/charge/charge_station.xacro
index 8dd1e192cc61ff5545b68759f771c56b503d0538..6c38e700f6105321eaf7304117350410a9fcfd18 100644
--- a/darwin_description/urdf/charge/charge_station.xacro
+++ b/darwin_description/urdf/charge/charge_station.xacro
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find bioloid_description)/urdf/qrcodes/qrcode.xacro" />
@@ -17,6 +17,7 @@
<geometry>
<mesh filename="package://darwin_description/meshes/charge/charge_support.stl"/>
</geometry>
+ <material name="grey"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
@@ -35,15 +36,16 @@
<!-- charge station far QR -->
<link name="${name}_far_qr_link">
<inertial>
- <mass value="0.10000000"/>
+ <mass value="2.68800000"/>
<origin xyz="0.00000000 0.00000000 0.000000" rpy="0 0 0"/>
- <inertia ixx="0.00017000" ixy="0.0" ixz="0.0" iyy="0.00017000" iyz="0.0" izz="0.00017333" />
+ <inertia ixx="0.00887040" ixy="0.0" ixz="0.0" iyy="0.00887040" iyz="0.0" izz="0.00896000" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://darwin_description/meshes/charge/far_qr.stl"/>
</geometry>
+ <material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
@@ -59,45 +61,48 @@
<child link="${name}_far_qr_link"/>
</joint>
- <xacro:qrcode name="${name}_north" parent="${name}_far_qr" code="N1" size="0.07">
- <origin xyz="0.0501 0.0 0.0" rpy="-1.5707 0 1.5707" />
+ <xacro:qrcode name="${name}_north" parent="${name}_far_qr" code="N1" size="0.1">
+ <origin xyz="0.0701 0.0 0.0" rpy="0 -1.5707 0" />
</xacro:qrcode>
- <xacro:qrcode name="${name}_south" parent="${name}_far_qr" code="S1" size="0.07">
- <origin xyz="-0.0501 0.0 0.0" rpy="-1.5707 0 -1.5707" />
+ <xacro:qrcode name="${name}_south" parent="${name}_far_qr" code="S1" size="0.1">
+ <origin xyz="-0.0701 0.0 0.0" rpy="0 -1.5707 3.14159" />
</xacro:qrcode>
- <xacro:qrcode name="${name}_west" parent="${name}_far_qr" code="W1" size="0.07">
- <origin xyz="0.0 0.0501 0.0" rpy="1.5707 3.14159 0" />
+ <xacro:qrcode name="${name}_west" parent="${name}_far_qr" code="W1" size="0.1">
+ <origin xyz="0.0 0.0701 0.0" rpy="0 -1.5707 1.5707" />
</xacro:qrcode>
- <xacro:qrcode name="${name}_east" parent="${name}_far_qr" code="E1" size="0.07">
- <origin xyz="0.0 -0.0501 0.0" rpy="1.5707 3.14159 3.14159 " />
+ <xacro:qrcode name="${name}_east" parent="${name}_far_qr" code="E1" size="0.1">
+ <origin xyz="0.0 -0.0701 0.0" rpy="0 -1.5707 -1.5707" />
</xacro:qrcode>
<!-- charge station connector -->
<link name="${name}_connector_link">
<inertial>
- <mass value="0.10000000"/>
+ <mass value="0.89677868"/>
<origin xyz="0.00000000 0.00000000 0.000000" rpy="0 0 0"/>
- <inertia ixx="0.00014464" ixy="0.0" ixz="0.0" iyy="0.00014464" iyz="0.0" izz="0.00028761" />
+ <inertia ixx="0.00328683" ixy="0.0" ixz="0.0" iyy="0.00328683" iyz="0.0" izz="0.00651387" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
- <mesh filename="package://darwin_description/meshes/charge/connector.stl"/>
+ <cylinder radius="0.2" length="0.02"/>
+ <!-- <mesh filename="package://darwin_description/meshes/charge/connector.stl"/> -->
</geometry>
+ <material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
- <mesh filename="package://darwin_description/meshes/charge/connector.stl"/>
+ <cylinder radius="0.2" length="0.02"/>
+ <!-- <mesh filename="package://darwin_description/meshes/charge/connector.stl"/> -->
</geometry>
</collision>
</link>
<joint name="${name}_connector_joint" type="fixed">
- <origin xyz="0.0 0.0 0.2" rpy="0 0 0"/>
+ <origin xyz="0.0 0.0 0.22" rpy="0 0 0"/>
<parent link="${name}_support_link"/>
<child link="${name}_connector_link"/>
</joint>
@@ -105,15 +110,16 @@
<!-- charge station near_qr -->
<link name="${name}_near_qr_link">
<inertial>
- <mass value="0.10000000"/>
+ <mass value="0.77986118"/>
<origin xyz="0.00000000 0.00000000 0.000000" rpy="0 0 0"/>
- <inertia ixx="0.00008564" ixy="0.0" ixz="0.0" iyy="0.00008564" iyz="0.0" izz="0.00016968" />
+ <inertia ixx="0.00142905" ixy="0.0" ixz="0.0" iyy="0.00142905" iyz="0.0" izz="0.00265013" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://darwin_description/meshes/charge/near_qr.stl"/>
</geometry>
+ <material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
@@ -123,6 +129,38 @@
</collision>
</link>
+ <xacro:qrcode name="${name}_north_left" parent="${name}_near_qr" code="N1L" size="0.04">
+ <origin xyz="0.071 -0.05 0.0" rpy="0 -1.5707 0" />
+ </xacro:qrcode>
+
+ <xacro:qrcode name="${name}_north_right" parent="${name}_near_qr" code="N1R" size="0.04">
+ <origin xyz="0.071 0.05 0.0" rpy="0 -1.5707 0" />
+ </xacro:qrcode>
+
+ <xacro:qrcode name="${name}_south_left" parent="${name}_near_qr" code="S1L" size="0.04">
+ <origin xyz="-0.071 0.05 0.0" rpy="0 -1.5707 3.14159" />
+ </xacro:qrcode>
+
+ <xacro:qrcode name="${name}_south_right" parent="${name}_near_qr" code="S1R" size="0.04">
+ <origin xyz="-0.071 -0.05 0.0" rpy="0 -1.5707 3.14159" />
+ </xacro:qrcode>
+
+ <xacro:qrcode name="${name}_east_left" parent="${name}_near_qr" code="E1L" size="0.04">
+ <origin xyz="-0.05 -0.071 0.0" rpy="0 -1.5707 -1.5707" />
+ </xacro:qrcode>
+
+ <xacro:qrcode name="${name}_east_right" parent="${name}_near_qr" code="E1R" size="0.04">
+ <origin xyz="0.05 -0.071 0.0" rpy="0 -1.5707 -1.5707" />
+ </xacro:qrcode>
+
+ <xacro:qrcode name="${name}_west_left" parent="${name}_near_qr" code="W1L" size="0.04">
+ <origin xyz="0.05 0.071 0.0" rpy="0 -1.5707 1.5707" />
+ </xacro:qrcode>
+
+ <xacro:qrcode name="${name}_west_right" parent="${name}_near_qr" code="W1R" size="0.04">
+ <origin xyz="-0.05 0.071 0.0" rpy="0 -1.5707 1.5707" />
+ </xacro:qrcode>
+
<joint name="${name}_near_qr_joint" type="fixed">
<origin xyz="0.0 0.0 0.3" rpy="0 0 0"/>
<parent link="${name}_support_link"/>
@@ -130,7 +168,18 @@
</joint>
<gazebo reference="${name}_support_link">
-<!-- <gravity>true</gravity>
+ <gravity>true</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="${name}_far_qr_link">
+ <gravity>true</gravity>
<selfCollide>false</selfCollide>
<maxContacts>10</maxContacts>
<dampingFactor>0.2</dampingFactor>
@@ -138,15 +187,29 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>-->
</gazebo>
-<!-- <gazebo reference="${name}_support_joint">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>-->
+ <gazebo reference="${name}_connector_link">
+ <gravity>true</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="${name}_near_qr_link">
+ <gravity>true</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
</xacro:macro>
</root>
diff --git a/darwin_description/urdf/darwin.gazebo b/darwin_description/urdf/darwin.gazebo
index a21cb8a7bf9ee79ab53a2f9739ec114c575cba91..897e6d5bc4cdd8797816f9d4e34de14adf959d8c 100644
--- a/darwin_description/urdf/darwin.gazebo
+++ b/darwin_description/urdf/darwin.gazebo
@@ -52,10 +52,6 @@
<material>Gazebo/Grey</material>
</gazebo>
- <gazebo reference="left_leg_ankle_roll">
- <material>Gazebo/Grey</material>
- </gazebo>
-
<gazebo reference="right_leg_pelvis_yaw">
<material>Gazebo/Grey</material>
</gazebo>
@@ -76,10 +72,6 @@
<material>Gazebo/Grey</material>
</gazebo>
- <gazebo reference="right_leg_ankle_roll">
- <material>Gazebo/Grey</material>
- </gazebo>
-
<gazebo reference="base_link">
<gravity>false</gravity>
<selfCollide>false</selfCollide>
@@ -89,8 +81,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="neck">
@@ -102,8 +92,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="head">
@@ -115,8 +103,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="left_shoulder">
@@ -128,8 +114,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="left_arm_high">
@@ -141,8 +125,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="right_shoulder">
@@ -154,8 +136,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="right_arm_high">
@@ -167,8 +147,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="left_leg_pelvis_pitch">
@@ -180,8 +158,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="left_leg_pelvis_roll">
@@ -193,8 +169,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="left_leg_pelvis_pitch">
@@ -206,8 +180,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="left_leg_knee">
@@ -219,8 +191,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="left_leg_ankle_pitch">
@@ -232,21 +202,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="left_leg_ankle_roll">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>1.000000</mu1>
- <mu2>1.000000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="right_leg_pelvis_pitch">
@@ -258,8 +213,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="right_leg_pelvis_roll">
@@ -271,8 +224,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="right_leg_pelvis_pitch">
@@ -284,8 +235,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="right_leg_knee">
@@ -297,8 +246,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="right_leg_ankle_pitch">
@@ -310,21 +257,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="right_leg_ankle_roll">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>1.00000</mu1>
- <mu2>1.00000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="j_tilt">
@@ -392,12 +324,6 @@
<stopErp>0.0</stopErp>
</gazebo>
- <gazebo reference="j_ankle_roll_l">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
<gazebo reference="j_hip_yaw_r">
<implicitSpringDamper>true</implicitSpringDamper>
<stopCfm>0.0</stopCfm>
@@ -428,17 +354,24 @@
<stopErp>0.0</stopErp>
</gazebo>
- <gazebo reference="j_ankle_roll_r">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/darwin</robotNamespace>
+ <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+ <legacyModeNS>true</legacyModeNS>
<controlPeriod>0.0078</controlPeriod>
</plugin>
</gazebo>
+ <gazebo>
+ <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
+ <topicName>/darwin/ground_truth</topicName>
+ <frameName>world</frameName>
+ <bodyName>base_link</bodyName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <alwaysOn>true</alwaysOn>
+ <updateRate>20.0</updateRate>
+ </plugin>
+ </gazebo>
+
</robot>
diff --git a/darwin_description/urdf/darwin.xacro b/darwin_description/urdf/darwin.xacro
index 59f810d57e62d8de03656357c99634acdab27d62..00dcca5b048cb52808696c1f814e2bc361971535 100755
--- a/darwin_description/urdf/darwin.xacro
+++ b/darwin_description/urdf/darwin.xacro
@@ -1,816 +1,29 @@
<robot name="darwin" xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:include filename="$(find darwin_description)/urdf/darwin.gazebo" />
+ <xacro:include filename="$(find darwin_description)/urdf/darwin_low_res.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/sensors/darwin_imu.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/sensors/ids_xs.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/sensors/bno055.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/left_hand.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/right_hand.xacro" />
-
- <link name="base_link">
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/body.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.975599" />
- <origin xyz="0.00002469 -0.029448 -0.01576428" rpy="0 0 0"/>
- <inertia ixx="0.00175853" ixy="-0.00000031" ixz="0.00000081" iyy="0.00187076" iyz="-0.00002473" izz="0.00246393" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/body_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="neck">
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/neck.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0243577" />
- <origin xyz="-2.4722012e-006 -0.021006105 -0.0010218021" rpy="0 0 0"/>
- <inertia ixx="3.4606288e-006" ixy="-1.810192e-010" ixz="-6.1530042e-011" iyy="9.0467035e-006" iyz="1.5988071e-007" izz="1.0024371e-005" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/neck_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="head">
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/head.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.158042" />
- <origin xyz="1.4076285e-005 0.019086672 0.004621368" rpy="0 0 0"/>
- <inertia ixx="0.00015147251" ixy="-1.8688115e-007" ixz="5.4521838e-009" iyy="0.00014149723" iyz="-1.3452525e-005" izz="0.00015599345" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/head_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_shoulder">
- <visual>
- <origin xyz="0.022 -0.016 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_shoulder.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.025913" />
- <origin xyz="-0.01643844 0.01337172 0.0000000" rpy="0 0 0"/>
- <inertia ixx="0.00001012" ixy="-0.00000071" ixz="0.00000000" iyy="0.0000099" iyz="0.00000000" izz="0.00000384" />
- </inertial>
- <collision>
- <origin xyz="0.022 -0.016 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_shoulder_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_arm_high" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_arm_high.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.168377" />
- <origin xyz="-0.0000229 -0.03563612 0.00239371" rpy="0 0 0"/>
- <inertia ixx="0.00014389" ixy="0.00000009" ixz="0.00000031" iyy="0.00004108" iyz="-0.00000970" izz="0.00013383" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_arm_high_bb.stl" />
- </geometry>
- </collision>
- </link>
+ <xacro:include filename="$(find darwin_description)/urdf/left_foot.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/right_foot.xacro" />
<xacro:left_hand parent="left_arm_high">
<origin xyz="0.0 -0.060 0.016" rpy="-1.5707 0 0" />
</xacro:left_hand>
- <link name="right_shoulder" >
- <visual>
- <origin xyz="-0.022 -0.016 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_shoulder.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.025913" />
- <origin xyz="0.01643844 0.01337172 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00001012" ixy="0.00000071" ixz="0.00000000" iyy="0.00000990" iyz="0.00000000" izz="0.00000384" />
- </inertial>
- <collision>
- <origin xyz="-0.022 -0.016 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_shoulder_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_arm_high" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_arm_high.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.168377" />
- <origin xyz="0.00002296 -0.03563612 0.00239371" rpy="0 0 0"/>
- <inertia ixx="0.00014389" ixy="-0.00000009" ixz="-0.00000031" iyy="0.00004108" iyz="-0.00000970" izz="0.00013383" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_arm_high_bb.stl" />
- </geometry>
- </collision>
- </link>
-
<xacro:right_hand parent="right_arm_high">
<origin xyz="0.0 -0.06 0.016" rpy="-1.5707 0 0" />
</xacro:right_hand>
- <link name="left_leg_pelvis_yaw" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_pelvis_yaw.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.027069" />
- <origin xyz="0.00000000 0.0227787 0.00288919" rpy="0 0 0"/>
- <inertia ixx="0.00001303" ixy="0.00000000" ixz="0.00000000" iyy="0.00001210" iyz="0.00000003" izz="0.00000422" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_pelvis_yaw_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_pelvis_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_pelvis_roll.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.16710" />
- <origin xyz="0.00007233 -0.01111330 -0.02044569" rpy="0 0 0"/>
- <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010824" iyz="0.0" izz="0.00004476" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_pelvis_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_pelvis_pitch" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_pelvis_pitch.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.119043" />
- <origin xyz="0.00001568 -0.07105466 0.00142329" rpy="0 0 0"/>
- <inertia ixx="0.00008127" ixy="0.0" ixz="0.0" iyy="0.00003624" iyz="0.0" izz="0.00009704" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_pelvis_pitch_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_knee" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_knee.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0703098" />
- <origin xyz="0.00006376 -0.03683715 0.00894668" rpy="0 0 0"/>
- <inertia ixx="0.0000824" ixy="0.0" ixz="0.0" iyy="0.00005574" iyz="0.0" izz="0.00010187" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_knee_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_ankle_pitch" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_ankle_pitch.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.167108" />
- <origin xyz="-0.00002505 0.01111330 -0.02044569" rpy="0 0 0"/>
- <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010823" iyz="0.0" izz="0.00004475" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_ankle_pitch_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_ankle_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_ankle_roll.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0794462" />
- <origin xyz="0.01094328 -0.02730949 -0.00102239" rpy="0 0 0"/>
- <inertia ixx="0.00007597" ixy="0.0" ixz="0.0" iyy="0.00010548" iyz="0.0" izz="0.00003613" />
- </inertial>
- <collision>
- <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_pelvis_yaw" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_pelvis_yaw.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.027069" />
- <origin xyz="0.00000000 0.02277874 0.00013919" rpy="0 0 0"/>
- <inertia ixx="0.00001303" ixy="0.0" ixz="0.0" iyy="0.00001210" iyz="0.0" izz="0.00000422" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_pelvis_yaw_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_pelvis_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_pelvis_roll.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.167108" />
- <origin xyz="-0.00007115 -0.0111133 -0.02044569" rpy="0 0 0"/>
- <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010823" iyz="0.0" izz="0.00004475" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_pelvis_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_pelvis_pitch" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_pelvis_pitch.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.119043" />
- <origin xyz="-0.00005071 -0.07105466 0.00142329" rpy="0 0 0"/>
- <inertia ixx="0.00008127" ixy="0.0" ixz="0.0" iyy="0.00003624" iyz="0.0" izz="0.00009704" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_pelvis_pitch_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_knee" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_knee.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0703098" />
- <origin xyz="-0.00006415 -0.03683715 0.00894668" rpy="0 0 0"/>
- <inertia ixx="0.00008246" ixy="0.0" ixz="0.0" iyy="0.00005574" iyz="0.0" izz="0.00010187" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_knee_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_ankle_pitch" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_ankle_pitch.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.167108" />
- <origin xyz="-0.00007115 0.01111330 -0.02044569" rpy="0 0 0"/>
- <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010823" iyz="0.0" izz="0.00004475" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_ankle_pitch_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_ankle_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_ankle_roll.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0794462" />
- <origin xyz="-0.01094329 -0.02730949 -0.00102239" rpy="0 0 0"/>
- <inertia ixx="0.00007597" ixy="0.0" ixz="0.0" iyy="0.0001054" iyz="0.0" izz="0.00003613" />
- </inertial>
- <collision>
- <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <joint name="j_pan" type="revolute">
- <parent link="base_link"/>
- <child link="neck"/>
- <origin xyz="0.0 0.051 0.0" rpy="0 0 0" />
- <axis xyz="0 1 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_pan">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_pan">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="pan_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_tilt" type="revolute">
- <parent link="neck"/>
- <child link="head"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_tilt">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_tilt">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="tilt_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_shoulder_pitch_l" type="revolute">
- <parent link="base_link"/>
- <child link="left_shoulder"/>
- <origin xyz="0.06 0 0" rpy="0 0 0" />
- <axis xyz="1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_shoulder_pitch_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_shoulder_pitch_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="shoulder_pitch_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_shoulder_roll_l" type="revolute">
- <parent link="left_shoulder"/>
- <child link="left_arm_high"/>
- <origin xyz="0.022 -0.016 0.0" rpy="0 0 0.785398163" />
- <axis xyz="0 0 -1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_shoulder_roll_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_shoulder_roll_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="shoulder_roll_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_shoulder_pitch_r" type="revolute">
- <parent link="base_link"/>
- <child link="right_shoulder"/>
- <origin xyz="-0.06 0 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_shoulder_pitch_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_shoulder_pitch_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="shoulder_pitch_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_shoulder_roll_r" type="revolute">
- <parent link="right_shoulder"/>
- <child link="right_arm_high"/>
- <origin xyz="-0.022 -0.016 0" rpy="0 0 -0.785398163" />
- <axis xyz="0 0 -1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_shoulder_roll_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_shoulder_roll_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="shoulder_roll_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_yaw_l" type="revolute">
- <parent link="base_link"/>
- <child link="left_leg_pelvis_yaw"/>
- <origin xyz="0.037 -0.1222 -0.005" rpy="0 0 0" />
- <axis xyz="0 -1 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_yaw_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_yaw_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_yaw_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_roll_l" type="revolute">
- <parent link="left_leg_pelvis_yaw"/>
- <child link="left_leg_pelvis_roll"/>
+ <xacro:left_foot parent="left_leg_ankle_pitch">
<origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 -1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_roll_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_roll_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_roll_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
+ </xacro:left_foot>
- <joint name="j_hip_pitch_l" type="revolute">
- <parent link="left_leg_pelvis_roll"/>
- <child link="left_leg_pelvis_pitch"/>
+ <xacro:right_foot parent="right_leg_ankle_pitch">
<origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_pitch_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_pitch_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_pitch_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_knee_l" type="revolute">
- <parent link="left_leg_pelvis_pitch"/>
- <child link="left_leg_knee"/>
- <origin xyz="0 -0.093 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_knee_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_knee_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="knee_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_pitch_l" type="revolute">
- <parent link="left_leg_knee"/>
- <child link="left_leg_ankle_pitch"/>
- <origin xyz="0 -0.093 0" rpy="0 0 0" />
- <axis xyz="1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_pitch_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_pitch_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_pitch_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_roll_l" type="revolute">
- <parent link="left_leg_ankle_pitch"/>
- <child link="left_leg_ankle_roll"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_roll_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_roll_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_roll_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_yaw_r" type="revolute">
- <parent link="base_link"/>
- <child link="right_leg_pelvis_yaw"/>
- <origin xyz="-0.037 -0.1222 -0.005" rpy="0 0 0" />
- <axis xyz="0 -1 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_yaw_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_yaw_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_yaw_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_roll_r" type="revolute">
- <parent link="right_leg_pelvis_yaw"/>
- <child link="right_leg_pelvis_roll"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 -1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_roll_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_roll_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_roll_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_pitch_r" type="revolute">
- <parent link="right_leg_pelvis_roll"/>
- <child link="right_leg_pelvis_pitch"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_pitch_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_pitch_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_pitch_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_knee_r" type="revolute">
- <parent link="right_leg_pelvis_pitch"/>
- <child link="right_leg_knee"/>
- <origin xyz="0.0 -0.093 0" rpy="0 0 0" />
- <axis xyz="1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_knee_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_knee_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="knee_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_pitch_r" type="revolute">
- <parent link="right_leg_knee"/>
- <child link="right_leg_ankle_pitch"/>
- <origin xyz="0.0 -0.093 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_pitch_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_pitch_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_pitch_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_roll_r" type="revolute">
- <parent link="right_leg_ankle_pitch"/>
- <child link="right_leg_ankle_roll"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_roll_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_roll_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_roll_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
+ </xacro:right_foot>
<xacro:ids_xs name="darwin_cam" parent="head">
<origin xyz="0.0 0.005 0.045" rpy="0 -2.2777 1.5707" />
diff --git a/darwin_description/urdf/darwin_ceabot.xacro b/darwin_description/urdf/darwin_ceabot.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..dfc0a763913d404e5feaffc6f7cd3a4d4210718d
--- /dev/null
+++ b/darwin_description/urdf/darwin_ceabot.xacro
@@ -0,0 +1,36 @@
+<robot name="darwin" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/darwin_low_res.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/darwin_imu.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/ids_xs.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/bno055.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/left_hand.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/right_hand.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/left_sensor_foot.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/right_sensor_foot.xacro" />
+
+ <xacro:left_hand parent="left_arm_high">
+ <origin xyz="0.0 -0.060 0.016" rpy="-1.5707 0 0" />
+ </xacro:left_hand>
+
+ <xacro:right_hand parent="right_arm_high">
+ <origin xyz="0.0 -0.06 0.016" rpy="-1.5707 0 0" />
+ </xacro:right_hand>
+
+ <xacro:left_sensor_foot parent="left_leg_ankle_pitch" update_rate="10" range="0.01">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </xacro:left_sensor_foot>
+
+ <xacro:right_sensor_foot parent="right_leg_ankle_pitch" update_rate="10" range="0.01">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </xacro:right_sensor_foot>
+
+ <xacro:ids_xs name="darwin_cam" parent="head">
+ <origin xyz="0.0 0.005 0.045" rpy="0 -2.2777 1.5707" />
+ </xacro:ids_xs>
+
+ <xacro:bno055 name="darwin_imu" parent="base_link">
+ <origin xyz="0.0 0.0 -0.05" rpy="0 0 1.5707" />
+ </xacro:bno055>
+
+</robot>
diff --git a/darwin_description/urdf/darwin_cpp.xacro b/darwin_description/urdf/darwin_cpp.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..00dcca5b048cb52808696c1f814e2bc361971535
--- /dev/null
+++ b/darwin_description/urdf/darwin_cpp.xacro
@@ -0,0 +1,36 @@
+<robot name="darwin" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/darwin_low_res.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/darwin_imu.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/ids_xs.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/bno055.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/left_hand.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/right_hand.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/left_foot.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/right_foot.xacro" />
+
+ <xacro:left_hand parent="left_arm_high">
+ <origin xyz="0.0 -0.060 0.016" rpy="-1.5707 0 0" />
+ </xacro:left_hand>
+
+ <xacro:right_hand parent="right_arm_high">
+ <origin xyz="0.0 -0.06 0.016" rpy="-1.5707 0 0" />
+ </xacro:right_hand>
+
+ <xacro:left_foot parent="left_leg_ankle_pitch">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </xacro:left_foot>
+
+ <xacro:right_foot parent="right_leg_ankle_pitch">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </xacro:right_foot>
+
+ <xacro:ids_xs name="darwin_cam" parent="head">
+ <origin xyz="0.0 0.005 0.045" rpy="0 -2.2777 1.5707" />
+ </xacro:ids_xs>
+
+ <xacro:bno055 name="darwin_imu" parent="base_link">
+ <origin xyz="0.0 0.0 -0.05" rpy="0 0 1.5707" />
+ </xacro:bno055>
+
+</robot>
diff --git a/darwin_description/urdf/darwin_grippers.gazebo b/darwin_description/urdf/darwin_grippers.gazebo
deleted file mode 100644
index a21cb8a7bf9ee79ab53a2f9739ec114c575cba91..0000000000000000000000000000000000000000
--- a/darwin_description/urdf/darwin_grippers.gazebo
+++ /dev/null
@@ -1,444 +0,0 @@
-<?xml version="1.0"?>
-<robot>
- <gazebo>
- <static>0</static>
- </gazebo>
-
- <gazebo reference="base_link">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="neck">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="head">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="left_shoulder">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="left_arm_high">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="right_shoulder">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="right_arm_high">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="left_leg_pelvis_yaw">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="left_leg_pelvis_roll">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="left_leg_pelvis_pitch">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="left_leg_knee">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="left_leg_ankle_pitch">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="left_leg_ankle_roll">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="right_leg_pelvis_yaw">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="right_leg_pelvis_roll">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="right_leg_pelvis_pitch">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="right_leg_knee">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="right_leg_ankle_pitch">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="right_leg_ankle_roll">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- <gazebo reference="base_link">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="neck">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="head">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="left_shoulder">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="left_arm_high">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="right_shoulder">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="right_arm_high">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="left_leg_pelvis_pitch">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="left_leg_pelvis_roll">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="left_leg_pelvis_pitch">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="left_leg_knee">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="left_leg_ankle_pitch">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="left_leg_ankle_roll">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>1.000000</mu1>
- <mu2>1.000000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="right_leg_pelvis_pitch">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="right_leg_pelvis_roll">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="right_leg_pelvis_pitch">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="right_leg_knee">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="right_leg_ankle_pitch">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>0.200000</mu1>
- <mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="right_leg_ankle_roll">
- <gravity>false</gravity>
- <selfCollide>false</selfCollide>
- <maxContacts>10</maxContacts>
- <dampingFactor>0.2</dampingFactor>
- <maxVel>0.0</maxVel>
- <minDepth>0.0</minDepth>
- <mu1>1.00000</mu1>
- <mu2>1.00000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
- </gazebo>
-
- <gazebo reference="j_tilt">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
- <gazebo reference="j_pan">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_shoulder_pitch_l">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_shoulder_roll_l">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_shoulder_pitch_r">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_shoulder_roll_r">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_hip_yaw_l">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_hip_roll_l">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_hip_pitch_l">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_knee_l">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_ankle_pitch_l">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_ankle_roll_l">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_hip_yaw_r">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_hip_roll_r">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_hip_pitch_r">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_knee_r">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_ankle_pitch_r">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo reference="j_ankle_roll_r">
- <implicitSpringDamper>true</implicitSpringDamper>
- <stopCfm>0.0</stopCfm>
- <stopErp>0.0</stopErp>
- </gazebo>
-
- <gazebo>
- <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
- <robotNamespace>/darwin</robotNamespace>
- <controlPeriod>0.0078</controlPeriod>
- </plugin>
- </gazebo>
-
-</robot>
diff --git a/darwin_description/urdf/darwin_grippers.xacro b/darwin_description/urdf/darwin_grippers.xacro
index 43829c782d58039452bde7ef6ac6b5c707a6952e..a0ee89dbef92184f84b98b3e3505502b90fba3fc 100755
--- a/darwin_description/urdf/darwin_grippers.xacro
+++ b/darwin_description/urdf/darwin_grippers.xacro
@@ -1,815 +1,20 @@
<robot name="darwin" xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:include filename="$(find darwin_description)/urdf/darwin.gazebo" />
+ <xacro:include filename="$(find darwin_description)/urdf/darwin_low_res.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/sensors/darwin_imu.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/sensors/ids_xs.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/sensors/bno055.xacro" />
<xacro:include filename="$(find darwin_description)/urdf/gripper.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/left_foot.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/right_foot.xacro" />
- <link name="base_link">
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/body.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.975599" />
- <origin xyz="0.00002469 -0.029448 -0.01576428" rpy="0 0 0"/>
- <inertia ixx="0.00175853" ixy="-0.00000031" ixz="0.00000081" iyy="0.00187076" iyz="-0.00002473" izz="0.00246393" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/body_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="neck">
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/neck.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0243577" />
- <origin xyz="-2.4722012e-006 -0.021006105 -0.0010218021" rpy="0 0 0"/>
- <inertia ixx="3.4606288e-006" ixy="-1.810192e-010" ixz="-6.1530042e-011" iyy="9.0467035e-006" iyz="1.5988071e-007" izz="1.0024371e-005" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/neck_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="head">
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/head.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.158042" />
- <origin xyz="1.4076285e-005 0.019086672 0.004621368" rpy="0 0 0"/>
- <inertia ixx="0.00015147251" ixy="-1.8688115e-007" ixz="5.4521838e-009" iyy="0.00014149723" iyz="-1.3452525e-005" izz="0.00015599345" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/head_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_shoulder">
- <visual>
- <origin xyz="0.022 -0.016 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_shoulder.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.025913" />
- <origin xyz="-0.01643844 0.01337172 0.0000000" rpy="0 0 0"/>
- <inertia ixx="0.00001012" ixy="-0.00000071" ixz="0.00000000" iyy="0.0000099" iyz="0.00000000" izz="0.00000384" />
- </inertial>
- <collision>
- <origin xyz="0.022 -0.016 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_shoulder_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_arm_high" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_arm_high.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.168377" />
- <origin xyz="-0.0000229 -0.03563612 0.00239371" rpy="0 0 0"/>
- <inertia ixx="0.00014389" ixy="0.00000009" ixz="0.00000031" iyy="0.00004108" iyz="-0.00000970" izz="0.00013383" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_arm_high_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <xacro:gripper name="l" parent="left_arm_high">
- <origin xyz="0.0 -0.060 0.016" rpy="-1.5707 0 0" />
- </xacro:gripper>
-
- <link name="right_shoulder" >
- <visual>
- <origin xyz="-0.022 -0.016 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_shoulder.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.025913" />
- <origin xyz="0.01643844 0.01337172 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00001012" ixy="0.00000071" ixz="0.00000000" iyy="0.00000990" iyz="0.00000000" izz="0.00000384" />
- </inertial>
- <collision>
- <origin xyz="-0.022 -0.016 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_shoulder_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_arm_high" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_arm_high.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.168377" />
- <origin xyz="0.00002296 -0.03563612 0.00239371" rpy="0 0 0"/>
- <inertia ixx="0.00014389" ixy="-0.00000009" ixz="-0.00000031" iyy="0.00004108" iyz="-0.00000970" izz="0.00013383" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_arm_high_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <xacro:gripper name="r" parent="right_arm_high">
- <origin xyz="0.0 -0.06 0.016" rpy="-1.5707 0 0" />
- </xacro:gripper>
-
- <link name="left_leg_pelvis_yaw" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_pelvis_yaw.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.027069" />
- <origin xyz="0.00000000 0.0227787 0.00288919" rpy="0 0 0"/>
- <inertia ixx="0.00001303" ixy="0.00000000" ixz="0.00000000" iyy="0.00001210" iyz="0.00000003" izz="0.00000422" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_pelvis_yaw_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_pelvis_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_pelvis_roll.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.16710" />
- <origin xyz="0.00007233 -0.01111330 -0.02044569" rpy="0 0 0"/>
- <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010824" iyz="0.0" izz="0.00004476" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_pelvis_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_pelvis_pitch" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_pelvis_pitch.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.119043" />
- <origin xyz="0.00001568 -0.07105466 0.00142329" rpy="0 0 0"/>
- <inertia ixx="0.00008127" ixy="0.0" ixz="0.0" iyy="0.00003624" iyz="0.0" izz="0.00009704" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_pelvis_pitch_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_knee" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_knee.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0703098" />
- <origin xyz="0.00006376 -0.03683715 0.00894668" rpy="0 0 0"/>
- <inertia ixx="0.0000824" ixy="0.0" ixz="0.0" iyy="0.00005574" iyz="0.0" izz="0.00010187" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_knee_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_ankle_pitch" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_ankle_pitch.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.167108" />
- <origin xyz="-0.00002505 0.01111330 -0.02044569" rpy="0 0 0"/>
- <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010823" iyz="0.0" izz="0.00004475" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_ankle_pitch_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="left_leg_ankle_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/left_leg_ankle_roll.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0794462" />
- <origin xyz="0.01094328 -0.02730949 -0.00102239" rpy="0 0 0"/>
- <inertia ixx="0.00007597" ixy="0.0" ixz="0.0" iyy="0.00010548" iyz="0.0" izz="0.00003613" />
- </inertial>
- <collision>
- <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_pelvis_yaw" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_pelvis_yaw.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.027069" />
- <origin xyz="0.00000000 0.02277874 0.00013919" rpy="0 0 0"/>
- <inertia ixx="0.00001303" ixy="0.0" ixz="0.0" iyy="0.00001210" iyz="0.0" izz="0.00000422" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_pelvis_yaw_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_pelvis_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_pelvis_roll.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.167108" />
- <origin xyz="-0.00007115 -0.0111133 -0.02044569" rpy="0 0 0"/>
- <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010823" iyz="0.0" izz="0.00004475" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_pelvis_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_pelvis_pitch" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_pelvis_pitch.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.119043" />
- <origin xyz="-0.00005071 -0.07105466 0.00142329" rpy="0 0 0"/>
- <inertia ixx="0.00008127" ixy="0.0" ixz="0.0" iyy="0.00003624" iyz="0.0" izz="0.00009704" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_pelvis_pitch_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_knee" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_knee.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0703098" />
- <origin xyz="-0.00006415 -0.03683715 0.00894668" rpy="0 0 0"/>
- <inertia ixx="0.00008246" ixy="0.0" ixz="0.0" iyy="0.00005574" iyz="0.0" izz="0.00010187" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_knee_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_ankle_pitch" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_ankle_pitch.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.167108" />
- <origin xyz="-0.00007115 0.01111330 -0.02044569" rpy="0 0 0"/>
- <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010823" iyz="0.0" izz="0.00004475" />
- </inertial>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_ankle_pitch_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <link name="right_leg_ankle_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/right_leg_ankle_roll.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0794462" />
- <origin xyz="-0.01094329 -0.02730949 -0.00102239" rpy="0 0 0"/>
- <inertia ixx="0.00007597" ixy="0.0" ixz="0.0" iyy="0.0001054" iyz="0.0" izz="0.00003613" />
- </inertial>
- <collision>
- <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
- <joint name="j_pan" type="revolute">
- <parent link="base_link"/>
- <child link="neck"/>
- <origin xyz="0.0 0.051 0.0" rpy="0 0 0" />
- <axis xyz="0 1 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_pan">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_pan">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="pan_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_tilt" type="revolute">
- <parent link="neck"/>
- <child link="head"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_tilt">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_tilt">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="tilt_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_shoulder_pitch_l" type="revolute">
- <parent link="base_link"/>
- <child link="left_shoulder"/>
- <origin xyz="0.06 0 0" rpy="0 0 0" />
- <axis xyz="1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_shoulder_pitch_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_shoulder_pitch_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="shoulder_pitch_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_shoulder_roll_l" type="revolute">
- <parent link="left_shoulder"/>
- <child link="left_arm_high"/>
- <origin xyz="0.022 -0.016 0.0" rpy="0 0 0.785398163" />
- <axis xyz="0 0 -1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_shoulder_roll_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_shoulder_roll_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="shoulder_roll_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_shoulder_pitch_r" type="revolute">
- <parent link="base_link"/>
- <child link="right_shoulder"/>
- <origin xyz="-0.06 0 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_shoulder_pitch_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_shoulder_pitch_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="shoulder_pitch_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_shoulder_roll_r" type="revolute">
- <parent link="right_shoulder"/>
- <child link="right_arm_high"/>
- <origin xyz="-0.022 -0.016 0" rpy="0 0 -0.785398163" />
- <axis xyz="0 0 -1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_shoulder_roll_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_shoulder_roll_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="shoulder_roll_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_yaw_l" type="revolute">
- <parent link="base_link"/>
- <child link="left_leg_pelvis_yaw"/>
- <origin xyz="0.037 -0.1222 -0.005" rpy="0 0 0" />
- <axis xyz="0 -1 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_yaw_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_yaw_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_yaw_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_roll_l" type="revolute">
- <parent link="left_leg_pelvis_yaw"/>
- <child link="left_leg_pelvis_roll"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 -1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_roll_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_roll_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_roll_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_pitch_l" type="revolute">
- <parent link="left_leg_pelvis_roll"/>
- <child link="left_leg_pelvis_pitch"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_pitch_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_pitch_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_pitch_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_knee_l" type="revolute">
- <parent link="left_leg_pelvis_pitch"/>
- <child link="left_leg_knee"/>
- <origin xyz="0 -0.093 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_knee_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_knee_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="knee_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_pitch_l" type="revolute">
- <parent link="left_leg_knee"/>
- <child link="left_leg_ankle_pitch"/>
- <origin xyz="0 -0.093 0" rpy="0 0 0" />
- <axis xyz="1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_pitch_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_pitch_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_pitch_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_roll_l" type="revolute">
- <parent link="left_leg_ankle_pitch"/>
- <child link="left_leg_ankle_roll"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_roll_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_roll_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_roll_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_yaw_r" type="revolute">
- <parent link="base_link"/>
- <child link="right_leg_pelvis_yaw"/>
- <origin xyz="-0.037 -0.1222 -0.005" rpy="0 0 0" />
- <axis xyz="0 -1 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_yaw_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_yaw_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_yaw_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_roll_r" type="revolute">
- <parent link="right_leg_pelvis_yaw"/>
- <child link="right_leg_pelvis_roll"/>
+ <xacro:left_foot parent="left_leg_ankle_pitch">
<origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 -1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
+ </xacro:right_hand>
- <transmission name="tran_hip_roll_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_roll_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_roll_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_hip_pitch_r" type="revolute">
- <parent link="right_leg_pelvis_roll"/>
- <child link="right_leg_pelvis_pitch"/>
+ <xacro:right_foot parent="right_leg_ankle_pitch">
<origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_hip_pitch_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_hip_pitch_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="hip_pitch_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_knee_r" type="revolute">
- <parent link="right_leg_pelvis_pitch"/>
- <child link="right_leg_knee"/>
- <origin xyz="0.0 -0.093 0" rpy="0 0 0" />
- <axis xyz="1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_knee_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_knee_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="knee_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_pitch_r" type="revolute">
- <parent link="right_leg_knee"/>
- <child link="right_leg_ankle_pitch"/>
- <origin xyz="0.0 -0.093 0" rpy="0 0 0" />
- <axis xyz="-1 0 0" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_pitch_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_pitch_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_pitch_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_roll_r" type="revolute">
- <parent link="right_leg_ankle_pitch"/>
- <child link="right_leg_ankle_roll"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_roll_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_roll_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_roll_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
+ </xacro:right_hand>
<xacro:ids_xs name="darwin_cam" parent="head">
<origin xyz="0.0 0.005 0.045" rpy="0 -2.2777 1.5707" />
diff --git a/darwin_description/urdf/darwin_high_res.xacro b/darwin_description/urdf/darwin_high_res.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..1944cca8d103b3098d4feadc2f42c50d2d09424e
--- /dev/null
+++ b/darwin_description/urdf/darwin_high_res.xacro
@@ -0,0 +1,737 @@
+<robot name="darwin" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/darwin.gazebo" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/darwin_imu.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/ids_xs.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/bno055.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/left_hand.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/right_hand.xacro" />
+
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/body.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.975599" />
+ <origin xyz="0.00002469 -0.029448 -0.01576428" rpy="0 0 0"/>
+ <inertia ixx="0.00175853" ixy="-0.00000031" ixz="0.00000081" iyy="0.00187076" iyz="-0.00002473" izz="0.00246393" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/body_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="neck">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/neck.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.0243577" />
+ <origin xyz="-2.4722012e-006 -0.021006105 -0.0010218021" rpy="0 0 0"/>
+ <inertia ixx="3.4606288e-006" ixy="-1.810192e-010" ixz="-6.1530042e-011" iyy="9.0467035e-006" iyz="1.5988071e-007" izz="1.0024371e-005" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/neck_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="head">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/head.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.158042" />
+ <origin xyz="1.4076285e-005 0.019086672 0.004621368" rpy="0 0 0"/>
+ <inertia ixx="0.00015147251" ixy="-1.8688115e-007" ixz="5.4521838e-009" iyy="0.00014149723" iyz="-1.3452525e-005" izz="0.00015599345" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/head_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="left_shoulder">
+ <visual>
+ <origin xyz="0.022 -0.016 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/left_shoulder.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.025913" />
+ <origin xyz="-0.01643844 0.01337172 0.0000000" rpy="0 0 0"/>
+ <inertia ixx="0.00001012" ixy="-0.00000071" ixz="0.00000000" iyy="0.0000099" iyz="0.00000000" izz="0.00000384" />
+ </inertial>
+ <collision>
+ <origin xyz="0.022 -0.016 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/left_shoulder_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="left_arm_high" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/left_arm_high.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.168377" />
+ <origin xyz="-0.0000229 -0.03563612 0.00239371" rpy="0 0 0"/>
+ <inertia ixx="0.00014389" ixy="0.00000009" ixz="0.00000031" iyy="0.00004108" iyz="-0.00000970" izz="0.00013383" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/left_arm_high_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <xacro:left_hand parent="left_arm_high">
+ <origin xyz="0.0 -0.060 0.016" rpy="-1.5707 0 0" />
+ </xacro:left_hand>
+
+ <link name="right_shoulder" >
+ <visual>
+ <origin xyz="-0.022 -0.016 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/right_shoulder.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.025913" />
+ <origin xyz="0.01643844 0.01337172 0.00000000" rpy="0 0 0"/>
+ <inertia ixx="0.00001012" ixy="0.00000071" ixz="0.00000000" iyy="0.00000990" iyz="0.00000000" izz="0.00000384" />
+ </inertial>
+ <collision>
+ <origin xyz="-0.022 -0.016 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/right_shoulder_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="right_arm_high" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/right_arm_high.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.168377" />
+ <origin xyz="0.00002296 -0.03563612 0.00239371" rpy="0 0 0"/>
+ <inertia ixx="0.00014389" ixy="-0.00000009" ixz="-0.00000031" iyy="0.00004108" iyz="-0.00000970" izz="0.00013383" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/right_arm_high_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <xacro:right_hand parent="right_arm_high">
+ <origin xyz="0.0 -0.06 0.016" rpy="-1.5707 0 0" />
+ </xacro:right_hand>
+
+ <link name="left_leg_pelvis_yaw" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/left_leg_pelvis_yaw.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.027069" />
+ <origin xyz="0.00000000 0.0227787 0.00288919" rpy="0 0 0"/>
+ <inertia ixx="0.00001303" ixy="0.00000000" ixz="0.00000000" iyy="0.00001210" iyz="0.00000003" izz="0.00000422" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_pelvis_yaw_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="left_leg_pelvis_roll" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/left_leg_pelvis_roll.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.16710" />
+ <origin xyz="0.00007233 -0.01111330 -0.02044569" rpy="0 0 0"/>
+ <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010824" iyz="0.0" izz="0.00004476" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_pelvis_roll_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="left_leg_pelvis_pitch" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/left_leg_pelvis_pitch.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.119043" />
+ <origin xyz="0.00001568 -0.07105466 0.00142329" rpy="0 0 0"/>
+ <inertia ixx="0.00008127" ixy="0.0" ixz="0.0" iyy="0.00003624" iyz="0.0" izz="0.00009704" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_pelvis_pitch_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="left_leg_knee" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/left_leg_knee.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.0703098" />
+ <origin xyz="0.00006376 -0.03683715 0.00894668" rpy="0 0 0"/>
+ <inertia ixx="0.0000824" ixy="0.0" ixz="0.0" iyy="0.00005574" iyz="0.0" izz="0.00010187" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_knee_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="left_leg_ankle_pitch" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/left_leg_ankle_pitch.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.167108" />
+ <origin xyz="-0.00002505 0.01111330 -0.02044569" rpy="0 0 0"/>
+ <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010823" iyz="0.0" izz="0.00004475" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_ankle_pitch_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="right_leg_pelvis_yaw" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/right_leg_pelvis_yaw.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.027069" />
+ <origin xyz="0.00000000 0.02277874 0.00013919" rpy="0 0 0"/>
+ <inertia ixx="0.00001303" ixy="0.0" ixz="0.0" iyy="0.00001210" iyz="0.0" izz="0.00000422" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_pelvis_yaw_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="right_leg_pelvis_roll" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/right_leg_pelvis_roll.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.167108" />
+ <origin xyz="-0.00007115 -0.0111133 -0.02044569" rpy="0 0 0"/>
+ <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010823" iyz="0.0" izz="0.00004475" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_pelvis_roll_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="right_leg_pelvis_pitch" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/right_leg_pelvis_pitch.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.119043" />
+ <origin xyz="-0.00005071 -0.07105466 0.00142329" rpy="0 0 0"/>
+ <inertia ixx="0.00008127" ixy="0.0" ixz="0.0" iyy="0.00003624" iyz="0.0" izz="0.00009704" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_pelvis_pitch_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="right_leg_knee" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/right_leg_knee.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.0703098" />
+ <origin xyz="-0.00006415 -0.03683715 0.00894668" rpy="0 0 0"/>
+ <inertia ixx="0.00008246" ixy="0.0" ixz="0.0" iyy="0.00005574" iyz="0.0" izz="0.00010187" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_knee_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="right_leg_ankle_pitch" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/right_leg_ankle_pitch.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.167108" />
+ <origin xyz="-0.00007115 0.01111330 -0.02044569" rpy="0 0 0"/>
+ <inertia ixx="0.00012130" ixy="0.0" ixz="0.0" iyy="0.00010823" iyz="0.0" izz="0.00004475" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_ankle_pitch_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_pan" type="revolute">
+ <parent link="base_link"/>
+ <child link="neck"/>
+ <origin xyz="0.0 0.051 0.0" rpy="0 0 0" />
+ <axis xyz="0 1 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_pan">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_pan">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="pan_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_tilt" type="revolute">
+ <parent link="neck"/>
+ <child link="head"/>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <axis xyz="-1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_tilt">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_tilt">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="tilt_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_shoulder_pitch_l" type="revolute">
+ <parent link="base_link"/>
+ <child link="left_shoulder"/>
+ <origin xyz="0.06 0 0" rpy="0 0 0" />
+ <axis xyz="1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_shoulder_pitch_l">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_shoulder_pitch_l">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="shoulder_pitch_l_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_shoulder_roll_l" type="revolute">
+ <parent link="left_shoulder"/>
+ <child link="left_arm_high"/>
+ <origin xyz="0.022 -0.016 0.0" rpy="0 0 0.785398163" />
+ <axis xyz="0 0 -1" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_shoulder_roll_l">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_shoulder_roll_l">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="shoulder_roll_l_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_shoulder_pitch_r" type="revolute">
+ <parent link="base_link"/>
+ <child link="right_shoulder"/>
+ <origin xyz="-0.06 0 0" rpy="0 0 0" />
+ <axis xyz="-1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_shoulder_pitch_r">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_shoulder_pitch_r">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="shoulder_pitch_r_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_shoulder_roll_r" type="revolute">
+ <parent link="right_shoulder"/>
+ <child link="right_arm_high"/>
+ <origin xyz="-0.022 -0.016 0" rpy="0 0 -0.785398163" />
+ <axis xyz="0 0 -1" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_shoulder_roll_r">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_shoulder_roll_r">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="shoulder_roll_r_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_hip_yaw_l" type="revolute">
+ <parent link="base_link"/>
+ <child link="left_leg_pelvis_yaw"/>
+ <origin xyz="0.037 -0.1222 -0.005" rpy="0 0 0" />
+ <axis xyz="0 -1 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_hip_yaw_l">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_hip_yaw_l">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="hip_yaw_l_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_hip_roll_l" type="revolute">
+ <parent link="left_leg_pelvis_yaw"/>
+ <child link="left_leg_pelvis_roll"/>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <axis xyz="0 0 -1" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_hip_roll_l">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_hip_roll_l">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="hip_roll_l_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_hip_pitch_l" type="revolute">
+ <parent link="left_leg_pelvis_roll"/>
+ <child link="left_leg_pelvis_pitch"/>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <axis xyz="-1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_hip_pitch_l">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_hip_pitch_l">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="hip_pitch_l_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_knee_l" type="revolute">
+ <parent link="left_leg_pelvis_pitch"/>
+ <child link="left_leg_knee"/>
+ <origin xyz="0 -0.093 0" rpy="0 0 0" />
+ <axis xyz="-1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_knee_l">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_knee_l">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="knee_l_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_ankle_pitch_l" type="revolute">
+ <parent link="left_leg_knee"/>
+ <child link="left_leg_ankle_pitch"/>
+ <origin xyz="0 -0.093 0" rpy="0 0 0" />
+ <axis xyz="1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_ankle_pitch_l">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_ankle_pitch_l">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="ankle_pitch_l_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_hip_yaw_r" type="revolute">
+ <parent link="base_link"/>
+ <child link="right_leg_pelvis_yaw"/>
+ <origin xyz="-0.037 -0.1222 -0.005" rpy="0 0 0" />
+ <axis xyz="0 -1 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_hip_yaw_r">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_hip_yaw_r">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="hip_yaw_r_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_hip_roll_r" type="revolute">
+ <parent link="right_leg_pelvis_yaw"/>
+ <child link="right_leg_pelvis_roll"/>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <axis xyz="0 0 -1" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_hip_roll_r">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_hip_roll_r">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="hip_roll_r_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_hip_pitch_r" type="revolute">
+ <parent link="right_leg_pelvis_roll"/>
+ <child link="right_leg_pelvis_pitch"/>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <axis xyz="1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_hip_pitch_r">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_hip_pitch_r">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="hip_pitch_r_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_knee_r" type="revolute">
+ <parent link="right_leg_pelvis_pitch"/>
+ <child link="right_leg_knee"/>
+ <origin xyz="0.0 -0.093 0" rpy="0 0 0" />
+ <axis xyz="1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_knee_r">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_knee_r">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="knee_r_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <joint name="j_ankle_pitch_r" type="revolute">
+ <parent link="right_leg_knee"/>
+ <child link="right_leg_ankle_pitch"/>
+ <origin xyz="0.0 -0.093 0" rpy="0 0 0" />
+ <axis xyz="-1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_ankle_pitch_r">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_ankle_pitch_r">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="ankle_pitch_r_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <xacro:ids_xs name="darwin_cam" parent="head">
+ <origin xyz="0.0 0.005 0.045" rpy="0 -2.2777 1.5707" />
+ </xacro:ids_xs>
+
+ <xacro:bno055 name="darwin_imu" parent="base_link">
+ <origin xyz="0.0 0.0 -0.05" rpy="0 0 1.5707" />
+ </xacro:bno055>
+
+</robot>
diff --git a/darwin_description/urdf/darwin_low_res.xacro b/darwin_description/urdf/darwin_low_res.xacro
index b33b247dff4a6d8959260feea59f965ea33fbf28..c2581ce60543ceb67113758c76ac28d9889a513e 100755
--- a/darwin_description/urdf/darwin_low_res.xacro
+++ b/darwin_description/urdf/darwin_low_res.xacro
@@ -1,11 +1,6 @@
<robot name="darwin" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find darwin_description)/urdf/darwin.gazebo" />
- <xacro:include filename="$(find darwin_description)/urdf/sensors/darwin_imu.xacro" />
- <xacro:include filename="$(find darwin_description)/urdf/sensors/ids_xs.xacro" />
- <xacro:include filename="$(find darwin_description)/urdf/sensors/bno055.xacro" />
- <xacro:include filename="$(find darwin_description)/urdf/left_hand.xacro" />
- <xacro:include filename="$(find darwin_description)/urdf/right_hand.xacro" />
<link name="base_link">
<visual>
@@ -122,10 +117,6 @@
</collision>
</link>
- <xacro:left_hand parent="left_arm_high">
- <origin xyz="0.0 -0.060 0.016" rpy="-1.5707 0 0" />
- </xacro:left_hand>
-
<link name="right_shoulder" >
<visual>
<origin xyz="-0.022 -0.016 0" rpy="0 0 0" />
@@ -172,10 +163,6 @@
</collision>
</link>
- <xacro:right_hand parent="right_arm_high">
- <origin xyz="0.0 -0.06 0.016" rpy="-1.5707 0 0" />
- </xacro:right_hand>
-
<link name="left_leg_pelvis_yaw" >
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -291,29 +278,6 @@
</collision>
</link>
- <link name="left_leg_ankle_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0794462" />
- <origin xyz="0.01094328 -0.02730949 -0.00102239" rpy="0 0 0"/>
- <inertia ixx="0.00007597" ixy="0.0" ixz="0.0" iyy="0.00010548" iyz="0.0" izz="0.00003613" />
- </inertial>
- <collision>
- <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
<link name="right_leg_pelvis_yaw" >
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -429,29 +393,6 @@
</collision>
</link>
- <link name="right_leg_ankle_roll" >
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl" />
- </geometry>
- <material name="Grey">
- <color rgba="0.79 0.82 0.93 1.0"/>
- </material>
- </visual>
- <inertial>
- <mass value="0.0794462" />
- <origin xyz="-0.01094329 -0.02730949 -0.00102239" rpy="0 0 0"/>
- <inertia ixx="0.00007597" ixy="0.0" ixz="0.0" iyy="0.0001054" iyz="0.0" izz="0.00003613" />
- </inertial>
- <collision>
- <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
- <geometry>
- <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl" />
- </geometry>
- </collision>
- </link>
-
<joint name="j_pan" type="revolute">
<parent link="base_link"/>
<child link="neck"/>
@@ -464,10 +405,10 @@
<transmission name="tran_pan">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_pan">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="pan_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -484,10 +425,10 @@
<transmission name="tran_tilt">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_tilt">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="tilt_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -504,10 +445,10 @@
<transmission name="tran_shoulder_pitch_l">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_shoulder_pitch_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="shoulder_pitch_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -524,10 +465,10 @@
<transmission name="tran_shoulder_roll_l">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_shoulder_roll_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="shoulder_roll_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -544,10 +485,10 @@
<transmission name="tran_shoulder_pitch_r">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_shoulder_pitch_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="shoulder_pitch_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -564,10 +505,10 @@
<transmission name="tran_shoulder_roll_r">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_shoulder_roll_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="shoulder_roll_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -584,10 +525,10 @@
<transmission name="tran_hip_yaw_l">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_hip_yaw_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="hip_yaw_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -604,10 +545,10 @@
<transmission name="tran_hip_roll_l">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_hip_roll_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="hip_roll_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -624,10 +565,10 @@
<transmission name="tran_hip_pitch_l">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_hip_pitch_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="hip_pitch_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -644,10 +585,10 @@
<transmission name="tran_knee_l">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_knee_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="knee_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -664,30 +605,10 @@
<transmission name="tran_ankle_pitch_l">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_ankle_pitch_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="ankle_pitch_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_roll_l" type="revolute">
- <parent link="left_leg_ankle_pitch"/>
- <child link="left_leg_ankle_roll"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_roll_l">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_roll_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_roll_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -704,10 +625,10 @@
<transmission name="tran_hip_yaw_r">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_hip_yaw_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="hip_yaw_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -724,10 +645,10 @@
<transmission name="tran_hip_roll_r">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_hip_roll_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="hip_roll_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -744,10 +665,10 @@
<transmission name="tran_hip_pitch_r">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_hip_pitch_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="hip_pitch_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -764,10 +685,10 @@
<transmission name="tran_knee_r">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_knee_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="knee_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -784,40 +705,12 @@
<transmission name="tran_ankle_pitch_r">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_ankle_pitch_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="ankle_pitch_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- <mechanicalReduction>1</mechanicalReduction>
- </actuator>
- </transmission>
-
- <joint name="j_ankle_roll_r" type="revolute">
- <parent link="right_leg_ankle_pitch"/>
- <child link="right_leg_ankle_roll"/>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <axis xyz="0 0 1" />
- <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
- <dynamics damping="0.4"/>
- </joint>
-
- <transmission name="tran_ankle_roll_r">
- <type>transmission_interface/SimpleTransmission</type>
- <joint name="j_ankle_roll_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
- </joint>
- <actuator name="ankle_roll_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
- <xacro:ids_xs name="darwin_cam" parent="head">
- <origin xyz="0.0 0.005 0.045" rpy="0 -2.2777 1.5707" />
- </xacro:ids_xs>
-
- <xacro:bno055 name="darwin_imu" parent="base_link">
- <origin xyz="0.0 0.0 -0.05" rpy="0 0 1.5707" />
- </xacro:bno055>
-
</robot>
diff --git a/darwin_description/urdf/darwin_sc.xacro b/darwin_description/urdf/darwin_sc.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..7009102662021f8042845856ac5f7bb0c1384ee5
--- /dev/null
+++ b/darwin_description/urdf/darwin_sc.xacro
@@ -0,0 +1,37 @@
+<robot name="darwin" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/darwin_low_res.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/darwin_imu.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/ids_xs.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/sensors/bno055.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/gripper_sc_low_res.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/left_foot.xacro" />
+ <xacro:include filename="$(find darwin_description)/urdf/right_foot.xacro" />
+
+ <xacro:left_foot parent="left_leg_ankle_pitch">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </xacro:left_foot>
+
+ <xacro:right_foot parent="right_leg_ankle_pitch">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </xacro:right_foot>
+
+ <xacro:gripper_sc name="l" parent="left_arm_high">
+ <origin xyz="0.0 -0.060 0.016" rpy="-1.5707 0 0" />
+ <axis xyz="1 0 0" />
+ </xacro:gripper_sc>
+
+ <xacro:gripper_sc name="r" parent="right_arm_high">
+ <origin xyz="0.0 -0.06 0.016" rpy="-1.5707 0 0" />
+ <axis xyz="-1 0 0" />
+ </xacro:gripper_sc>
+
+ <xacro:ids_xs name="darwin_cam" parent="head">
+ <origin xyz="0.0 0.005 0.045" rpy="0 -2.2777 1.5707" />
+ </xacro:ids_xs>
+
+ <xacro:bno055 name="darwin_imu" parent="base_link">
+ <origin xyz="0.0 0.0 -0.05" rpy="0 0 1.5707" />
+ </xacro:bno055>
+
+</robot>
diff --git a/darwin_description/urdf/gripper.gazebo b/darwin_description/urdf/gripper.gazebo
index 54ca01f1d9a9af32ac9f6478ea0d07f402ecce5f..4fac2a895bdc5d810e965f2242b3d11b75817912 100644
--- a/darwin_description/urdf/gripper.gazebo
+++ b/darwin_description/urdf/gripper.gazebo
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="gripper_gazebo" params="name">
<gazebo reference="${name}_elbow">
@@ -16,8 +16,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="j_elbow_${name}">
@@ -26,11 +24,11 @@
<stopErp>0.0</stopErp>
</gazebo>
- <gazebo reference="${name}_wrist">
+ <gazebo reference="${name}_hand">
<material>Gazebo/Grey</material>
</gazebo>
- <gazebo reference="${name}_wrist">
+ <gazebo reference="${name}_hand">
<gravity>false</gravity>
<selfCollide>false</selfCollide>
<maxContacts>10</maxContacts>
@@ -39,8 +37,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="j_wrist_${name}">
@@ -62,11 +58,9 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
- <gazebo reference="j_finger1_seg2_${name}">
+ <gazebo reference="j_finger1_seg1_${name}">
<implicitSpringDamper>true</implicitSpringDamper>
<stopCfm>0.0</stopCfm>
<stopErp>0.0</stopErp>
@@ -85,8 +79,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="j_finger1_seg2_${name}">
@@ -108,8 +100,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="j_finger1_seg3_${name}">
@@ -118,5 +108,119 @@
<stopErp>0.0</stopErp>
</gazebo>
+ <gazebo reference="${name}_finger2_seg1">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="${name}_finger2_seg1">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="j_finger2_seg1_${name}">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ <stopCfm>0.0</stopCfm>
+ <stopErp>0.0</stopErp>
+ </gazebo>
+
+ <gazebo reference="${name}_finger2_seg2">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="${name}_finger2_seg2">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="j_finger2_seg2_${name}">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ <stopCfm>0.0</stopCfm>
+ <stopErp>0.0</stopErp>
+ </gazebo>
+
+ <gazebo reference="${name}_finger2_seg3">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="${name}_finger2_seg3">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="${name}_finger3_seg1">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="${name}_finger3_seg1">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="j_finger3_seg1_${name}">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ <stopCfm>0.0</stopCfm>
+ <stopErp>0.0</stopErp>
+ </gazebo>
+
+ <gazebo reference="${name}_finger3_seg2">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="${name}_finger3_seg2">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="j_finger3_seg2_${name}">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ <stopCfm>0.0</stopCfm>
+ <stopErp>0.0</stopErp>
+ </gazebo>
+
+ <gazebo reference="${name}_finger3_seg3">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="${name}_finger3_seg3">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
</xacro:macro>
</root>
diff --git a/darwin_description/urdf/gripper.xacro b/darwin_description/urdf/gripper.xacro
index 8149a7c9de8fa545f4c4534e8b57effa1e7b25b8..0a520867d76caea106303272fa131367bb5f5544 100755
--- a/darwin_description/urdf/gripper.xacro
+++ b/darwin_description/urdf/gripper.xacro
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find darwin_description)/urdf/gripper.gazebo" />
@@ -18,7 +18,7 @@
<inertial>
<mass value="0.00808041" />
<origin xyz="0.00000000 -0.02076698 -0.01438925" rpy="0 0 0"/>
- <inertia ixx="0.00000115" ixy="0.00000000" ixz="0.00000000" iyy="0.00000401" iyz="0.00000261" izz="0.00000587" />
+ <inertia ixx="0.000115" ixy="0.00000000" ixz="0.00000000" iyy="0.000401" iyz="0.00000261" izz="0.000587" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -40,10 +40,10 @@
<transmission name="tran_elbow_${name}">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_elbow_${name}">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="elbow_${name}_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -61,7 +61,7 @@
<inertial>
<mass value="0.06658929" />
<origin xyz="0.00000000 -0.00577359 0.04117078" rpy="0 0 0"/>
- <inertia ixx="0.00003315" ixy="0.00000000" ixz="0.00000000" iyy="0.00003331" iyz="0.00000045" izz="0.00001573" />
+ <inertia ixx="0.003315" ixy="0.00000000" ixz="0.00000000" iyy="0.003331" iyz="0.00000045" izz="0.001573" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -83,10 +83,10 @@
<transmission name="tran_wrist_${name}">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_wrist_${name}">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="wrist_${name}_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -104,8 +104,8 @@
</visual>
<inertial>
<mass value="0.00321424" />
- <origin xyz="0.00035854 0.01250000 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00000020" ixy="0.00000000" ixz="0.00000000" iyy="0.00000008" iyz="0.00000000" izz="0.00000017" />
+ <origin xyz="0.0035854 0.01250000 0.00000000" rpy="0 0 0"/>
+ <inertia ixx="0.0020" ixy="0.00000000" ixz="0.00000000" iyy="0.0008" iyz="0.00000000" izz="0.0017" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -126,11 +126,11 @@
<transmission name="tran_finger1_seg1_${name}">
<type>transmission_interface/SimpleTransmission</type>
- <joint name="j_wrist_${name}">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <joint name="j_finger1_seg1_${name}">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="wrist_${name}_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
@@ -148,7 +148,7 @@
<inertial>
<mass value="0.00321424" />
<origin xyz="0.00035854 0.01250000 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00000020" ixy="0.00000000" ixz="0.00000000" iyy="0.00000008" iyz="0.00000000" izz="0.00000017" />
+ <inertia ixx="0.0020" ixy="0.00000000" ixz="0.00000000" iyy="0.0008" iyz="0.00000000" izz="0.0017" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -181,7 +181,7 @@
<inertial>
<mass value="0.00321424" />
<origin xyz="0.00035854 0.01250000 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00000020" ixy="0.00000000" ixz="0.00000000" iyy="0.00000008" iyz="0.00000000" izz="0.00000017" />
+ <inertia ixx="0.0020" ixy="0.00000000" ixz="0.00000000" iyy="0.0008" iyz="0.00000000" izz="0.0017" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -215,7 +215,7 @@
<inertial>
<mass value="0.00321424" />
<origin xyz="0.00035854 0.01250000 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00000020" ixy="0.00000000" ixz="0.00000000" iyy="0.00000008" iyz="0.00000000" izz="0.00000017" />
+ <inertia ixx="0.0020" ixy="0.00000000" ixz="0.00000000" iyy="0.0008" iyz="0.00000000" izz="0.0017" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -248,7 +248,7 @@
<inertial>
<mass value="0.00321424" />
<origin xyz="0.00035854 0.01250000 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00000020" ixy="0.00000000" ixz="0.00000000" iyy="0.00000008" iyz="0.00000000" izz="0.00000017" />
+ <inertia ixx="0.0020" ixy="0.00000000" ixz="0.00000000" iyy="0.0008" iyz="0.00000000" izz="0.0017" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -281,7 +281,7 @@
<inertial>
<mass value="0.00321424" />
<origin xyz="0.00035854 0.01250000 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00000020" ixy="0.00000000" ixz="0.00000000" iyy="0.00000008" iyz="0.00000000" izz="0.00000017" />
+ <inertia ixx="0.0020" ixy="0.00000000" ixz="0.00000000" iyy="0.0008" iyz="0.00000000" izz="0.0017" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -315,7 +315,7 @@
<inertial>
<mass value="0.00321424" />
<origin xyz="0.00035854 0.01250000 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00000020" ixy="0.00000000" ixz="0.00000000" iyy="0.00000008" iyz="0.00000000" izz="0.00000017" />
+ <inertia ixx="0.0020" ixy="0.00000000" ixz="0.00000000" iyy="0.0008" iyz="0.00000000" izz="0.0017" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -348,7 +348,7 @@
<inertial>
<mass value="0.00321424" />
<origin xyz="0.00035854 0.01250000 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00000020" ixy="0.00000000" ixz="0.00000000" iyy="0.00000008" iyz="0.00000000" izz="0.00000017" />
+ <inertia ixx="0.0020" ixy="0.00000000" ixz="0.00000000" iyy="0.0008" iyz="0.00000000" izz="0.0017" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -381,7 +381,7 @@
<inertial>
<mass value="0.00321424" />
<origin xyz="0.00035854 0.01250000 0.00000000" rpy="0 0 0"/>
- <inertia ixx="0.00000020" ixy="0.00000000" ixz="0.00000000" iyy="0.00000008" iyz="0.00000000" izz="0.00000017" />
+ <inertia ixx="0.0020" ixy="0.00000000" ixz="0.00000000" iyy="0.0008" iyz="0.00000000" izz="0.0017" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
diff --git a/darwin_description/urdf/gripper_sc.gazebo b/darwin_description/urdf/gripper_sc.gazebo
new file mode 100644
index 0000000000000000000000000000000000000000..8265ba7c4357aee80e9bc1d453a73d724a31e7a9
--- /dev/null
+++ b/darwin_description/urdf/gripper_sc.gazebo
@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="gripper_sc_gazebo" params="name">
+ <gazebo reference="${name}_elbow">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="${name}_elbow">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="j_elbow_${name}">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ <stopCfm>0.0</stopCfm>
+ <stopErp>0.0</stopErp>
+ </gazebo>
+
+ <gazebo reference="${name}_gripper_bot">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="${name}_gripper_bot">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="j_gripper_bot_${name}">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ <stopCfm>0.0</stopCfm>
+ <stopErp>0.0</stopErp>
+ </gazebo>
+
+ <gazebo reference="${name}_gripper_top">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="${name}_gripper_top">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="j_gripper_top_${name}">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ <stopCfm>0.0</stopCfm>
+ <stopErp>0.0</stopErp>
+ </gazebo>
+
+ </xacro:macro>
+</root>
diff --git a/darwin_description/urdf/gripper_sc.xacro b/darwin_description/urdf/gripper_sc.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..60d41509163efb3934c841a2b13b1fa7ea1733f2
--- /dev/null
+++ b/darwin_description/urdf/gripper_sc.xacro
@@ -0,0 +1,140 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/gripper_sc.gazebo" />
+
+ <xacro:macro name="gripper_sc" params="name parent *origin">
+ <link name="${name}_elbow" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/hand_base_sc.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.04000000" />
+ <origin xyz="0.00000000 -0.05158190 -0.01572735" rpy="0 0 0"/>
+ <inertia ixx="0.00001939" ixy="0.00000000" ixz="-0.00000016" iyy="0.00001048" iyz="0.00000050" izz="0.00001730" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/hand_base_sc_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_elbow_${name}" type="revolute">
+ <parent link="${parent}"/>
+ <child link="${name}_elbow"/>
+ <xacro:insert_block name="origin" />
+ <axis xyz="1 0 0" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_elbow_${name}">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_elbow_${name}">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="elbow_${name}_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <link name="${name}_gripper_bot" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/hand_fingers_sc.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.00500000" />
+ <origin xyz="0.00008303 0.01262438 0.01051827" rpy="0 0 0"/>
+ <inertia ixx="0.00000409" ixy="-0.00000001" ixz="-0.00000003" iyy="0.00000191" iyz="0.00000041" izz="0.00000335" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/hand_fingers_sc_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_gripper_bot_${name}" type="revolute">
+ <parent link="${name}_elbow"/>
+ <child link="${name}_gripper_bot"/>
+ <origin xyz="0.0 -0.06865 -0.028" rpy="0 0 0"/>
+ <axis xyz="1 0 0" />
+ <limit effort="0.25" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_gripper_bot_${name}">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_gripper_bot_${name}">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="gripper_bot_${name}_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <link name="${name}_gripper_top" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/hand_fingers_sc.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.00500000" />
+ <origin xyz="0.00008303 0.01262438 0.01051827" rpy="0 0 0"/>
+ <inertia ixx="0.00000409" ixy="-0.00000001" ixz="-0.00000003" iyy="0.00000191" iyz="0.00000041" izz="0.00000335" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/hand_fingers_sc_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_gripper_top_${name}" type="revolute">
+ <parent link="${name}_elbow"/>
+ <child link="${name}_gripper_top"/>
+ <origin xyz="0.0 -0.06865 -0.00358" rpy="0 3.14159 0"/>
+ <axis xyz="1 0 0" />
+ <limit effort="0.25" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_gripper_top_${name}">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_gripper_top_${name}">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="gripper_top_${name}_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <xacro:gripper_sc_gazebo name="${name}"/>
+
+ </xacro:macro>
+</root>
diff --git a/darwin_description/urdf/gripper_sc_low_res.xacro b/darwin_description/urdf/gripper_sc_low_res.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..5bf5cd21b99923834badeb4a6b82f79b7fede153
--- /dev/null
+++ b/darwin_description/urdf/gripper_sc_low_res.xacro
@@ -0,0 +1,140 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/gripper_sc.gazebo" />
+
+ <xacro:macro name="gripper_sc" params="name parent *origin *axis">
+ <link name="${name}_elbow" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/hand_base_sc_bb.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.04000000" />
+ <origin xyz="0.00000000 -0.05158190 -0.01572735" rpy="0 0 0"/>
+ <inertia ixx="0.00001939" ixy="0.00000000" ixz="-0.00000016" iyy="0.00001048" iyz="0.00000050" izz="0.00001730" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/hand_base_sc_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_elbow_${name}" type="revolute">
+ <parent link="${parent}"/>
+ <child link="${name}_elbow"/>
+ <xacro:insert_block name="origin" />
+ <xacro:insert_block name="axis" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_elbow_${name}">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_elbow_${name}">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="elbow_${name}_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <link name="${name}_gripper_bot" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/hand_fingers_sc_bb.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.00500000" />
+ <origin xyz="0.00008303 0.01262438 0.01051827" rpy="0 0 0"/>
+ <inertia ixx="0.00000409" ixy="-0.00000001" ixz="-0.00000003" iyy="0.00000191" iyz="0.00000041" izz="0.00000335" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/hand_fingers_sc_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_gripper_bot_${name}" type="revolute">
+ <parent link="${name}_elbow"/>
+ <child link="${name}_gripper_bot"/>
+ <origin xyz="0.0 -0.06865 -0.028" rpy="0 0 0"/>
+ <axis xyz="1 0 0" />
+ <limit effort="0.25" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_gripper_bot_${name}">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_gripper_bot_${name}">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="gripper_bot_${name}_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <link name="${name}_gripper_top" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/hand_fingers_sc_bb.stl"/>
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.00500000" />
+ <origin xyz="0.00008303 0.01262438 0.01051827" rpy="0 0 0"/>
+ <inertia ixx="0.00000409" ixy="-0.00000001" ixz="-0.00000003" iyy="0.00000191" iyz="0.00000041" izz="0.00000335" />
+ </inertial>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/hand_fingers_sc_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_gripper_top_${name}" type="revolute">
+ <parent link="${name}_elbow"/>
+ <child link="${name}_gripper_top"/>
+ <origin xyz="0.0 -0.06865 -0.00358" rpy="0 3.14159 0"/>
+ <axis xyz="1 0 0" />
+ <limit effort="0.25" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_gripper_top_${name}">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_gripper_top_${name}">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="gripper_top_${name}_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <xacro:gripper_sc_gazebo name="${name}"/>
+
+ </xacro:macro>
+</root>
diff --git a/darwin_description/urdf/left_foot.gazebo b/darwin_description/urdf/left_foot.gazebo
new file mode 100644
index 0000000000000000000000000000000000000000..f0be2f7e447f76422e28e11ce2702cd0b680cd4f
--- /dev/null
+++ b/darwin_description/urdf/left_foot.gazebo
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<robot>
+ <gazebo reference="left_leg_ankle_roll">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="left_leg_ankle_roll">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>1.000000</mu1>
+ <mu2>1.000000</mu2>
+ </gazebo>
+
+ <gazebo reference="j_ankle_roll_l">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ <stopCfm>0.0</stopCfm>
+ <stopErp>0.0</stopErp>
+ </gazebo>
+
+</robot>
diff --git a/darwin_description/urdf/left_foot.xacro b/darwin_description/urdf/left_foot.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..67668eabc3bcaed97008db0953bb935a4829da14
--- /dev/null
+++ b/darwin_description/urdf/left_foot.xacro
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/left_foot.gazebo" />
+
+ <xacro:macro name="left_foot" params="parent *origin">
+ <link name="left_leg_ankle_roll" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.0794462" />
+ <origin xyz="0.01094328 -0.02730949 -0.00102239" rpy="0 0 0"/>
+ <inertia ixx="0.00007597" ixy="0.0" ixz="0.0" iyy="0.00010548" iyz="0.0" izz="0.00003613" />
+ </inertial>
+ <collision>
+ <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_ankle_roll_l" type="revolute">
+ <parent link="${parent}"/>
+ <child link="left_leg_ankle_roll"/>
+ <xacro:insert_block name="origin" />
+ <axis xyz="0 0 1" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_ankle_roll_l">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_ankle_roll_l">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="ankle_roll_l_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ </xacro:macro>
+
+</root>
diff --git a/darwin_description/urdf/left_hand.gazebo b/darwin_description/urdf/left_hand.gazebo
index 60cbc0171ade682806ab968cc7b9fd799f7478b3..2077a54e245fd734df764e1c2e3853aba7edd6d2 100644
--- a/darwin_description/urdf/left_hand.gazebo
+++ b/darwin_description/urdf/left_hand.gazebo
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo reference="left_arm_low">
<material>Gazebo/Grey</material>
@@ -15,8 +15,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="j_elbow_l">
diff --git a/darwin_description/urdf/left_hand.xacro b/darwin_description/urdf/left_hand.xacro
index 55f001d2a4558cf5074d3e99f35042eab2af5a4e..3d6f7cf53564229510c565e9e2d11f344a77ee8f 100755
--- a/darwin_description/urdf/left_hand.xacro
+++ b/darwin_description/urdf/left_hand.xacro
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find darwin_description)/urdf/left_hand.gazebo" />
@@ -40,10 +40,10 @@
<transmission name="tran_elbow_l">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_elbow_l">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="elbow_l_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
diff --git a/darwin_description/urdf/left_sensor_foot.xacro b/darwin_description/urdf/left_sensor_foot.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..fd902b7f472ac91221c80c0586b78b0874634168
--- /dev/null
+++ b/darwin_description/urdf/left_sensor_foot.xacro
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/left_foot.gazebo" />
+ <xacro:include filename="$(find bioloid_description)/urdf/sensors/cny70_ir.xacro" />
+
+ <xacro:macro name="left_sensor_foot" params="parent *origin update_rate range">
+ <link name="left_leg_ankle_roll" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/left_sensor_foot.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.07944623" />
+ <origin xyz="0.00787555 -0.03128293 -0.00010476" rpy="0 0 0"/>
+ <inertia ixx="0.00006020" ixy="-0.00000148" ixz="-0.0000010" iyy="0.00008141" iyz="0.00000086" izz="0.00002745" />
+ </inertial>
+ <collision>
+ <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/left_sensor_foot.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_ankle_roll_l" type="revolute">
+ <parent link="${parent}"/>
+ <child link="left_leg_ankle_roll"/>
+ <xacro:insert_block name="origin" />
+ <axis xyz="0 0 1" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_ankle_roll_l">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_ankle_roll_l">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="ankle_roll_l_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <xacro:cny70_ir name="left_down_right_rear" namespace="darwin" parent="left_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0185 -0.0325 0.0065" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_down_right_middle" namespace="darwin" parent="left_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0185 -0.0325 0.0275" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_down_right_front" namespace="darwin" parent="left_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.011 -0.0325 0.0485" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_down_left_rear" namespace="darwin" parent="left_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0335 -0.0325 0.0065" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_down_left_middle" namespace="darwin" parent="left_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0335 -0.0325 0.0275" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_down_left_front" namespace="darwin" parent="left_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.026 -0.0325 0.0485" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_front_right" namespace="darwin" parent="left_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0186 -0.0285 0.051" rpy="0 -1.5707 0" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_front_left" namespace="darwin" parent="left_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0336 -0.0285 0.051" rpy="-1.5707 -1.5707 0" />
+ </xacro:cny70_ir>
+
+ </xacro:macro>
+
+</root>
diff --git a/darwin_description/urdf/right_foot.gazebo b/darwin_description/urdf/right_foot.gazebo
new file mode 100644
index 0000000000000000000000000000000000000000..3682735ec532dbe0c10f51a28c1902a2356d0a6e
--- /dev/null
+++ b/darwin_description/urdf/right_foot.gazebo
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<robot>
+ <gazebo reference="right_leg_ankle_roll">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ <gazebo reference="right_leg_ankle_roll">
+ <gravity>false</gravity>
+ <selfCollide>false</selfCollide>
+ <maxContacts>10</maxContacts>
+ <dampingFactor>0.2</dampingFactor>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.0</minDepth>
+ <mu1>1.00000</mu1>
+ <mu2>1.00000</mu2>
+ </gazebo>
+
+ <gazebo reference="j_ankle_roll_r">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ <stopCfm>0.0</stopCfm>
+ <stopErp>0.0</stopErp>
+ </gazebo>
+
+</robot>
diff --git a/darwin_description/urdf/right_foot.xacro b/darwin_description/urdf/right_foot.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..eb12dc8577a4829042282979cfeb138c606e37df
--- /dev/null
+++ b/darwin_description/urdf/right_foot.xacro
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/right_foot.gazebo" />
+
+ <xacro:macro name="right_foot" params="parent *origin">
+ <link name="right_leg_ankle_roll" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.0794462" />
+ <origin xyz="-0.01094329 -0.02730949 -0.00102239" rpy="0 0 0"/>
+ <inertia ixx="0.00007597" ixy="0.0" ixz="0.0" iyy="0.0001054" iyz="0.0" izz="0.00003613" />
+ </inertial>
+ <collision>
+ <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_ankle_roll_r" type="revolute">
+ <parent link="${parent}"/>
+ <child link="right_leg_ankle_roll"/>
+ <xacro:insert_block name="origin" />
+ <axis xyz="0 0 1" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_ankle_roll_r">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_ankle_roll_r">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="ankle_roll_r_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ </xacro:macro>
+
+</root>
diff --git a/darwin_description/urdf/right_hand.gazebo b/darwin_description/urdf/right_hand.gazebo
index fc48d17c96643cb89295a06b967627e07fc36655..796338f0f2b77497d05e65373b9a39d916c1c3f6 100644
--- a/darwin_description/urdf/right_hand.gazebo
+++ b/darwin_description/urdf/right_hand.gazebo
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo reference="right_arm_low">
<material>Gazebo/Grey</material>
@@ -15,8 +15,6 @@
<minDepth>0.0</minDepth>
<mu1>0.200000</mu1>
<mu2>0.200000</mu2>
- <kp>0.0</kp>
- <kd>0.0</kd>
</gazebo>
<gazebo reference="j_elbow_r">
diff --git a/darwin_description/urdf/right_hand.xacro b/darwin_description/urdf/right_hand.xacro
index 7d4263c2cf49c2b0a43779430a872c68b0bbf4ab..ec48b6430611d2a3eaebc50a229ca47d2d7a5164 100755
--- a/darwin_description/urdf/right_hand.xacro
+++ b/darwin_description/urdf/right_hand.xacro
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find darwin_description)/urdf/right_hand.gazebo" />
@@ -40,10 +40,10 @@
<transmission name="tran_elbow_r">
<type>transmission_interface/SimpleTransmission</type>
<joint name="j_elbow_r">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="elbow_r_motor">
- <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
diff --git a/darwin_description/urdf/right_sensor_foot.xacro b/darwin_description/urdf/right_sensor_foot.xacro
new file mode 100755
index 0000000000000000000000000000000000000000..f13e4a51e1019e0c66a079008e894d05464a4a82
--- /dev/null
+++ b/darwin_description/urdf/right_sensor_foot.xacro
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find darwin_description)/urdf/right_foot.gazebo" />
+ <xacro:include filename="$(find bioloid_description)/urdf/sensors/cny70_ir.xacro" />
+
+ <xacro:macro name="right_sensor_foot" params="parent *origin update_rate range">
+ <link name="right_leg_ankle_roll" >
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/right_sensor_foot.stl" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.79 0.82 0.93 1.0"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="0.07944623" />
+ <origin xyz="-0.00825113 -0.03128300 -0.00010280" rpy="0 0 0"/>
+ <inertia ixx="0.00006020" ixy="0.00000138" ixz="0.00000006" iyy="0.00008138" iyz="0.00000086" izz="0.00002742" />
+ </inertial>
+ <collision>
+ <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://darwin_description/meshes/right_sensor_foot.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="j_ankle_roll_r" type="revolute">
+ <parent link="${parent}"/>
+ <child link="right_leg_ankle_roll"/>
+ <xacro:insert_block name="origin" />
+ <axis xyz="0 0 1" />
+ <limit effort="2.5" velocity="6.0" lower="-3.14" upper="3.14" />
+ <dynamics damping="0.4"/>
+ </joint>
+
+ <transmission name="tran_ankle_roll_r">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j_ankle_roll_r">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="ankle_roll_r_motor">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <xacro:cny70_ir name="right_down_right_rear" namespace="darwin" parent="right_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0335 -0.0325 0.0065" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_down_right_middle" namespace="darwin" parent="right_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0335 -0.0325 0.0275" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_down_right_front" namespace="darwin" parent="right_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.026 -0.0325 0.0485" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_down_left_rear" namespace="darwin" parent="right_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0185 -0.0325 0.0065" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_down_left_middle" namespace="darwin" parent="right_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0185 -0.0325 0.0275" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_down_left_front" namespace="darwin" parent="right_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.011 -0.0325 0.0485" rpy="1.5707 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_front_right" namespace="darwin" parent="right_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0336 -0.0285 0.051" rpy="0 -1.5707 0" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_front_left" namespace="darwin" parent="right_leg_ankle_roll" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0186 -0.0285 0.051" rpy="-1.5707 -1.5707 0" />
+ </xacro:cny70_ir>
+
+ </xacro:macro>
+
+</root>
diff --git a/darwin_description/urdf/sensors/bno055.gazebo b/darwin_description/urdf/sensors/bno055.gazebo
index 709e0cb509bd26fe676d7f3e2e535d5429e9dc5b..3f4045446d057dc63eb77eeff67d100502102b44 100644
--- a/darwin_description/urdf/sensors/bno055.gazebo
+++ b/darwin_description/urdf/sensors/bno055.gazebo
@@ -1,5 +1,5 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="bno055_gazebo" params="name">
<gazebo>
<plugin name="bno055_sim" filename="libhector_gazebo_ros_imu.so">
diff --git a/darwin_description/urdf/sensors/ids_xs.gazebo b/darwin_description/urdf/sensors/ids_xs.gazebo
index 57d649a6b57f6828ef8c6af2db77de99cbcc7fa8..787dec90e12c11214cbc44aa54f595364c2422a0 100644
--- a/darwin_description/urdf/sensors/ids_xs.gazebo
+++ b/darwin_description/urdf/sensors/ids_xs.gazebo
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="ids_xs_gazebo" params="name">
<gazebo reference="${name}_link">
@@ -44,7 +44,7 @@
<cameraName>darwin/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
- <frameName>darwin/${name}_link</frameName>
+ <frameName>darwin/${name}_optical_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
diff --git a/darwin_description/urdf/sensors/ids_xs.xacro b/darwin_description/urdf/sensors/ids_xs.xacro
index 619c6ca9b3c20acf8062ff045c0de2f6cc1a4818..e07bf5b471874cffe5521495246e1fa4098e7fc9 100644
--- a/darwin_description/urdf/sensors/ids_xs.xacro
+++ b/darwin_description/urdf/sensors/ids_xs.xacro
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find darwin_description)/urdf/sensors/ids_xs.gazebo" />
@@ -31,6 +31,34 @@
<parent link="${parent}"/>
<child link="${name}_link"/>
</joint>
+
+ <link name="${name}_optical_link">
+ <collision>
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.001 0.001 0.001"/>
+ </geometry>
+ </collision>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.001 0.001 0.001"/>
+ </geometry>
+ <material name="black"/>
+ </visual>
+ <inertial>
+ <mass value="0.012" />
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+ <inertia ixx="0.00000001" ixy="0" ixz="0" iyy="0.00000001" iyz="0.0" izz="0.00000001" />
+ </inertial>
+ </link>
+
+ <joint name="${name}_optical_joint" type="fixed">
+ <origin xyz="0.0 0.0 0.0" rpy="-1.5707 0 -1.5707" />
+ <parent link="${name}_link"/>
+ <child link="${name}_optical_link"/>
+ </joint>
+
<xacro:ids_xs_gazebo name="${name}"/>
</xacro:macro>
diff --git a/darwin_driver/CMakeLists.txt b/darwin_driver/CMakeLists.txt
index 8d1df67063db34e2e90b79acabcbabc4bd0ed959..c6b2276df8a5aa302c5516385b1f935add38d2a8 100644
--- a/darwin_driver/CMakeLists.txt
+++ b/darwin_driver/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_driver darwin_smart_charger_client std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver iri_action_server std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -62,7 +62,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_driver darwin_smart_charger_client std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs
+ CATKIN_DEPENDS iri_base_driver iri_action_server std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -100,7 +100,8 @@ target_link_libraries(${PROJECT_NAME} ${darwin_robot_LIBRARY})
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
-add_dependencies(${PROJECT_NAME} darwin_smart_charger_client_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} iri_base_driver_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME}_gencfg iri_base_driver_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} control_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} trajectory_msgs_generate_messages_cpp)
diff --git a/darwin_driver/cfg/DarwinDriver.cfg b/darwin_driver/cfg/DarwinDriver.cfg
index 6091edae554a77bdeafcc723bb9a44247501bdf3..8c4a562b2437b6547bdc8c4501398fec3018f09f 100755
--- a/darwin_driver/cfg/DarwinDriver.cfg
+++ b/darwin_driver/cfg/DarwinDriver.cfg
@@ -33,7 +33,7 @@
PACKAGE='darwin_driver'
-from driver_base.msg import SensorLevels
+from iri_base_driver.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
@@ -43,6 +43,7 @@ gen = ParameterGenerator()
gen.add("darwin_serial", str_t, SensorLevels.RECONFIGURE_STOP, "Darwin serial device serial number", "A4008atn")
gen.add("darwin_id", int_t, SensorLevels.RECONFIGURE_STOP, "Darwin identifier", 2, 0, 254)
gen.add("darwin_baudrate", int_t, SensorLevels.RECONFIGURE_STOP, "Darwin serial baudrate", 1000000, 9600, 3000000)
+gen.add("simulate", bool_t, SensorLevels.RECONFIGURE_STOP, "Use driver in simualtion mode", False)
gen.add("cmd_vel_timeout", double_t, SensorLevels.RECONFIGURE_STOP, "Maximum time between cmd_vel messgaes",1.0, 0.1, 5)
exit(gen.generate(PACKAGE, "DarwinDriverDriver", "DarwinDriver"))
diff --git a/darwin_driver/include/darwin_driver.h b/darwin_driver/include/darwin_driver.h
index 8fa3fc451740e74e5c636e65aa6d6867f225db8f..3fbc9217372904d6ce0a3200e07388ac32c87cd0 100644
--- a/darwin_driver/include/darwin_driver.h
+++ b/darwin_driver/include/darwin_driver.h
@@ -53,6 +53,35 @@ typedef struct
double rot_speed;
}TWalkParams;
+typedef struct
+{
+ double shift_weight_left_time;
+ double rise_right_foot_time;
+ double advance_right_foot_time;
+ double contact_right_foot_time;
+ double shift_weight_right_time;
+ double rise_left_foot_time;
+ double advance_left_foot_time;
+ double contact_left_foot_time;
+ double center_weight_time;
+ double x_offset;
+ double y_offset;
+ double z_offset;
+ double roll_offset;
+ double pitch_offset;
+ double yaw_offset;
+ double y_shift;
+ double x_shift;
+ double z_overshoot;
+ double z_height;
+ double hip_pitch_offset;
+ double roll_shift;
+ double pitch_shift;
+ double yaw_shift;
+ double y_spread;
+ double x_shift_body;
+}TStairsParams;
+
/**
* \brief IRI ROS Specific Driver Class
*
@@ -78,6 +107,7 @@ class DarwinDriver : public iri_base_driver::IriBaseDriver
// private attributes and methods
CDarwinRobot *darwin; //de darwin_robot_driver
TWalkParams walk_params;
+ TStairsParams stairs_params;
public:
/**
* \brief define config type
@@ -183,6 +213,8 @@ class DarwinDriver : public iri_base_driver::IriBaseDriver
mm_mode_t mm_get_module(unsigned char servo_id);
std::vector<double> mm_get_servo_angles(void);
void mm_load_config(std::string &filename);
+ bool mm_has_fallen(void);
+ fall_t mm_get_fallen_position(void);
// motion action interface
void action_load_page(unsigned char page_id);
unsigned char action_get_current_page(void);
@@ -198,6 +230,14 @@ class DarwinDriver : public iri_base_driver::IriBaseDriver
bool is_walking(void);
void walk(double step_x_m,double step_y_m,double step_rad);
double walk_get_timeout(void);
+ /* stairs interface */
+ void stairs_set_params(TStairsParams *params);
+ void stairs_get_params(TStairsParams *params);
+ void stairs_go_up(void);
+ void stairs_go_down(void);
+ void stairs_stop(void);
+ bool stairs_is_active(void);
+ stairs_phase_t stairs_get_phase(void);
// joint motion interface
void joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
void joints_start(joints_grp_t group);
@@ -219,15 +259,16 @@ class DarwinDriver : public iri_base_driver::IriBaseDriver
void head_get_current_target(double *pan,double *tilt);
// smart charger interface
- bool is_smart_charger_det_and_en(void);
+ bool is_smart_charger_detected(void);
+ bool is_smart_charger_enabled(void);
void smart_charger_enable(void);
void smart_charger_disable(void);
void smart_charger_set_period(double period);
double smart_charger_get_period(void);
- // unsigned char smart_charger_read_control(void);
TChargerData smart_charger_get_data(void);
void smart_charger_set_limit_current(double limit_current);
- // unsigned char smart_charger_read_control(void);
+ double smart_charger_get_limit_current(void);
+ void smart_charger_set_range_current(double min_current, double max_current);
/**
* \brief Destructor
diff --git a/darwin_driver/include/darwin_driver_node.h b/darwin_driver/include/darwin_driver_node.h
index 5057d9a8ff1da79164170437c04e9bae28cebfaa..5aed1741266b131d9b4bb11bf566652a71d18283 100644
--- a/darwin_driver/include/darwin_driver_node.h
+++ b/darwin_driver/include/darwin_driver_node.h
@@ -29,20 +29,21 @@
#include "darwin_driver.h"
// [publisher subscriber headers]
-#include <darwin_smart_charger_client/smart_charger_data.h>
+#include <humanoid_common_msgs/smart_charger_data.h> //CACA
#include <humanoid_common_msgs/leds.h>
#include <humanoid_common_msgs/buttons.h>
#include <sensor_msgs/Imu.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/JointState.h>
+#include <std_msgs/Int8.h>
// [service client headers]
-#include <darwin_smart_charger_client/set_config.h>
-#include <darwin_smart_charger_client/set_limit_current.h>
-#include <darwin_smart_charger_client/smart_charger_enable.h>
-#include <darwin_smart_charger_client/get_period.h>
-#include <darwin_smart_charger_client/set_period.h>
+#include <humanoid_common_msgs/get_stairs_params.h>
+#include <humanoid_common_msgs/set_stairs_params.h>
+#include <humanoid_common_msgs/get_smart_charger_config.h>
+#include <humanoid_common_msgs/set_smart_charger_config.h>
+//
#include <humanoid_common_msgs/set_pid.h>
#include <humanoid_common_msgs/get_pid.h>
#include <humanoid_common_msgs/get_walk_params.h>
@@ -50,6 +51,7 @@
#include <humanoid_common_msgs/set_servo_modules.h>
// [action server client headers]
+#include <humanoid_common_msgs/humanoid_stairsAction.h>
#include <control_msgs/JointTrajectoryAction.h>
#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
#include <iri_action_server/iri_action_server.h>
@@ -76,12 +78,16 @@ class DarwinDriverNode : public iri_base_driver::IriBaseNodeDriver<DarwinDriver>
{
private:
// [publisher attributes]
+// Smart charger publish data
ros::Publisher smart_charger_data_publisher_;
- darwin_smart_charger_client::smart_charger_data smart_charger_data_smart_charger_data_msg_;
+ humanoid_common_msgs::smart_charger_data smart_charger_data_smart_charger_data_msg_;
ros::Publisher buttons_publisher_;
humanoid_common_msgs::buttons buttons_buttons_msg_;
+ ros::Publisher fallen_state_publisher_;
+ std_msgs::Int8 fallen_state_msg_;
+
ros::Publisher raw_imu_publisher_;
sensor_msgs::Imu raw_imu_Imu_msg_;
@@ -110,39 +116,31 @@ class DarwinDriverNode : public iri_base_driver::IriBaseNodeDriver<DarwinDriver>
ros::Time walk_timeout;
// [service attributes]
- ros::ServiceServer smart_charger_set_config_server_;
- bool smart_charger_set_configCallback(darwin_smart_charger_client::set_config::Request &req, darwin_smart_charger_client::set_config::Response &res);
- pthread_mutex_t smart_charger_set_config_mutex_;
- void smart_charger_set_config_mutex_enter(void);
- void smart_charger_set_config_mutex_exit(void);
-
- ros::ServiceServer smart_charger_set_limit_current_server_;
- bool smart_charger_set_limit_currentCallback(darwin_smart_charger_client::set_limit_current::Request &req, darwin_smart_charger_client::set_limit_current::Response &res);
- pthread_mutex_t smart_charger_set_limit_current_mutex_;
- void smart_charger_set_limit_current_mutex_enter(void);
- void smart_charger_set_limit_current_mutex_exit(void);
-
+ ros::ServiceServer get_stairs_params_server_;
+ bool get_stairs_paramsCallback(humanoid_common_msgs::get_stairs_params::Request &req, humanoid_common_msgs::get_stairs_params::Response &res);
+ pthread_mutex_t get_stairs_params_mutex_;
+ void get_stairs_params_mutex_enter(void);
+ void get_stairs_params_mutex_exit(void);
+
+ ros::ServiceServer set_stairs_params_server_;
+ bool set_stairs_paramsCallback(humanoid_common_msgs::set_stairs_params::Request &req, humanoid_common_msgs::set_stairs_params::Response &res);
+ pthread_mutex_t set_stairs_params_mutex_;
+ void set_stairs_params_mutex_enter(void);
+ void set_stairs_params_mutex_exit(void);
+
+// Get smart charger module config
+ ros::ServiceServer get_smart_charger_config_server_;
+ bool get_smart_charger_configCallback(humanoid_common_msgs::get_smart_charger_config::Request &req, humanoid_common_msgs::get_smart_charger_config::Response &res);
+ pthread_mutex_t get_smart_charger_config_mutex_;
+ void get_smart_charger_config_mutex_enter(void);
+ void get_smart_charger_config_mutex_exit(void);
-//Enable/Disable smart charger module
- ros::ServiceServer smart_charger_enable_disable_server_;
- bool smart_charger_enable_disableCallback(darwin_smart_charger_client::smart_charger_enable::Request &req, darwin_smart_charger_client::smart_charger_enable::Response &res);
- pthread_mutex_t smart_charger_enable_disable_mutex_;
- void smart_charger_enable_disable_mutex_enter(void);
- void smart_charger_enable_disable_mutex_exit(void);
-
-//Get period of smart charger module
- ros::ServiceServer smart_charger_get_period_server_;
- bool smart_charger_get_periodCallback(darwin_smart_charger_client::get_period::Request &req, darwin_smart_charger_client::get_period::Response &res);
- pthread_mutex_t smart_charger_get_period_mutex_;
- void smart_charger_get_period_mutex_enter(void);
- void smart_charger_get_period_mutex_exit(void);
-
-//Set period of smart charger module
- ros::ServiceServer smart_charger_set_period_server_;
- bool smart_charger_set_periodCallback(darwin_smart_charger_client::set_period::Request &req, darwin_smart_charger_client::set_period::Response &res);
- pthread_mutex_t smart_charger_set_period_mutex_;
- void smart_charger_set_period_mutex_enter(void);
- void smart_charger_set_period_mutex_exit(void);
+//Set smart charger module config
+ ros::ServiceServer set_smart_charger_config_server_;
+ bool set_smart_charger_configCallback(humanoid_common_msgs::set_smart_charger_config::Request &req, humanoid_common_msgs::set_smart_charger_config::Response &res);
+ pthread_mutex_t set_smart_charger_config_mutex_;
+ void set_smart_charger_config_mutex_enter(void);
+ void set_smart_charger_config_mutex_exit(void);
ros::ServiceServer set_tilt_pid_server_;
bool set_tilt_pidCallback(humanoid_common_msgs::set_pid::Request &req, humanoid_common_msgs::set_pid::Response &res);
@@ -190,6 +188,17 @@ class DarwinDriverNode : public iri_base_driver::IriBaseNodeDriver<DarwinDriver>
// [client attributes]
// [action server attributes]
+ IriActionServer<humanoid_common_msgs::humanoid_stairsAction> climb_stairs_aserver_;
+ void climb_stairsStartCallback(const humanoid_common_msgs::humanoid_stairsGoalConstPtr& goal);
+ void climb_stairsStopCallback(void);
+ bool climb_stairsIsFinishedCallback(void);
+ bool climb_stairsHasSucceededCallback(void);
+ void climb_stairsGetResultCallback(humanoid_common_msgs::humanoid_stairsResultPtr& result);
+ void climb_stairsGetFeedbackCallback(humanoid_common_msgs::humanoid_stairsFeedbackPtr& feedback);
+ bool climb_stairs_active;
+ bool climb_stairs_succeeded;
+ bool climb_stairs_finished;
+
IriActionServer<control_msgs::JointTrajectoryAction> joint_trajectory_aserver_;
void joint_trajectoryStartCallback(const control_msgs::JointTrajectoryGoalConstPtr& goal);
void joint_trajectoryStopCallback(void);
diff --git a/darwin_driver/launch/darwin.launch b/darwin_driver/launch/darwin.launch
index fa87189cf786465ab79cc17e4b1684616e1213e3..531df83bc9781424e122068591ec2746efe3a5eb 100644
--- a/darwin_driver/launch/darwin.launch
+++ b/darwin_driver/launch/darwin.launch
@@ -1,11 +1,16 @@
<launch>
+
+ <arg name="device" default="/dev/pts/22"/>
+ <arg name="simulate" default="False"/>
+
<node name="darwin"
pkg="darwin_driver"
type="darwin_driver"
output="screen">
- <param name="darwin_serial" value="A603LOBS"/>
+ <param name="darwin_serial" value="$(arg device)"/>
<param name="darwin_baudrate" value="1000000"/>
<param name="darwin_id" value="2"/>
+ <param name="simulate" value="$(arg simulate)"/>
</node>
</launch>
diff --git a/darwin_driver/package.xml b/darwin_driver/package.xml
index 8033b64e3b0826257d3e0f873309b46eface010c..b7b0ff0153fd121016bf14e4a77bfcec2cbbb790 100644
--- a/darwin_driver/package.xml
+++ b/darwin_driver/package.xml
@@ -41,7 +41,6 @@
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_driver</build_depend>
- <build_depend>darwin_smart_charger_client</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
@@ -50,7 +49,6 @@
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>iri_base_driver</run_depend>
- <run_depend>darwin_smart_charger_client</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
diff --git a/darwin_driver/src/darwin_driver.cpp b/darwin_driver/src/darwin_driver.cpp
index c73ae06e2454ad1d861e6a768b4489c85d9e5a02..f5437902fdba6b9dda3414c2a8e426723ac58821 100644
--- a/darwin_driver/src/darwin_driver.cpp
+++ b/darwin_driver/src/darwin_driver.cpp
@@ -16,7 +16,11 @@ bool DarwinDriver::openDriver(void)
delete this->darwin;
this->darwin=NULL;
}
- this->darwin=new CDarwinRobot("Darwin",this->config_.darwin_serial,this->config_.darwin_baudrate,this->config_.darwin_id);
+ if(this->config_.simulate)
+ std::cout << "using simulation mode" << std::endl;
+ else
+ std::cout << "using real mode" << std::endl;
+ this->darwin=new CDarwinRobot("Darwin",this->config_.darwin_serial,this->config_.darwin_baudrate,this->config_.darwin_id,this->config_.simulate);
// get the current walk parameters
this->walk_params.x_offset=this->darwin->walk_get_x_offset();
this->walk_params.y_offset=this->darwin->walk_get_y_offset();
@@ -35,6 +39,32 @@ bool DarwinDriver::openDriver(void)
this->walk_params.arm_swing_gain=this->darwin->walk_get_arm_swing_gain();
this->walk_params.trans_speed=this->darwin->walk_get_trans_speed();
this->walk_params.rot_speed=this->darwin->walk_get_rot_speed();
+ // get the current stairs parameters
+ this->stairs_params.shift_weight_left_time=this->darwin->stairs_get_phase_time(SHIFT_WEIGHT_LEFT);
+ this->stairs_params.rise_right_foot_time=this->darwin->stairs_get_phase_time(RISE_RIGHT_FOOT);
+ this->stairs_params.advance_right_foot_time=this->darwin->stairs_get_phase_time(ADVANCE_RIGHT_FOOT);
+ this->stairs_params.contact_right_foot_time=this->darwin->stairs_get_phase_time(CONTACT_RIGHT_FOOT);
+ this->stairs_params.shift_weight_right_time=this->darwin->stairs_get_phase_time(SHIFT_WEIGHT_RIGHT);
+ this->stairs_params.rise_left_foot_time=this->darwin->stairs_get_phase_time(RISE_LEFT_FOOT);
+ this->stairs_params.advance_left_foot_time=this->darwin->stairs_get_phase_time(ADVANCE_LEFT_FOOT);
+ this->stairs_params.contact_left_foot_time=this->darwin->stairs_get_phase_time(CONTACT_LEFT_FOOT);
+ this->stairs_params.center_weight_time=this->darwin->stairs_get_phase_time(CENTER_WEIGHT);
+ this->stairs_params.x_offset=this->darwin->stairs_get_x_offset();
+ this->stairs_params.y_offset=this->darwin->stairs_get_y_offset();
+ this->stairs_params.z_offset=this->darwin->stairs_get_z_offset();
+ this->stairs_params.roll_offset=this->darwin->stairs_get_roll_offset();
+ this->stairs_params.pitch_offset=this->darwin->stairs_get_pitch_offset();
+ this->stairs_params.yaw_offset=this->darwin->stairs_get_yaw_offset();
+ this->stairs_params.y_shift=this->darwin->stairs_get_y_shift();
+ this->stairs_params.x_shift=this->darwin->stairs_get_x_shift();
+ this->stairs_params.z_overshoot=this->darwin->stairs_get_z_overshoot();
+ this->stairs_params.z_height=this->darwin->stairs_get_z_height();
+ this->stairs_params.hip_pitch_offset=this->darwin->stairs_get_hip_pitch_offset();
+ this->stairs_params.roll_shift=this->darwin->stairs_get_roll_shift();
+ this->stairs_params.pitch_shift=this->darwin->stairs_get_pitch_shift();
+ this->stairs_params.yaw_shift=this->darwin->stairs_get_yaw_shift();
+ this->stairs_params.y_spread=this->darwin->stairs_get_y_spread();
+ this->stairs_params.x_shift_body=this->darwin->stairs_get_x_shift_body();
return true;
}catch(CException &e){
@@ -69,6 +99,7 @@ bool DarwinDriver::startDriver(void)
ROS_INFO_STREAM("Found " << num_servos << " servos ");
// enable all servos and assign them to the action module
this->darwin->mm_enable_power();
+ this->darwin->mm_enable_power_v2();
for(i=0;i<MAX_NUM_SERVOS;i++)
{
if(present_servos&(0x00000001<<i))
@@ -80,12 +111,12 @@ bool DarwinDriver::startDriver(void)
if(!this->darwin->mm_is_running())
this->darwin->mm_start();
// move the robot to the walk ready position
-// ROS_INFO("Moving to the walk ready position ...");
-// this->darwin->action_load_page(WALK_READY);
-// this->darwin->action_start();
-// while(this->darwin->action_is_page_running())
-// usleep(100000);
-// ROS_INFO("Robot at the walk ready position");
+ ROS_INFO("Moving to the walk ready position ...");
+ this->darwin->action_load_page(WALK_READY);
+ this->darwin->action_start();
+ while(this->darwin->action_is_page_running())
+ usleep(100000);
+ ROS_INFO("Robot at the walk ready position");
// calibrate the gyro and and enable balancing
this->darwin->imu_start();
if(this->darwin->imu_is_accel_detected())
@@ -101,8 +132,8 @@ bool DarwinDriver::startDriver(void)
usleep(100000);
ROS_INFO("Gyro calibration done");
// enable balancing
-// if(!this->darwin->mm_is_balance_enabled())
-// this->darwin->mm_enable_balance();
+ if(!this->darwin->mm_is_balance_enabled())
+ this->darwin->mm_enable_balance();
}
else
ROS_INFO("Gyroscope not detected");
@@ -125,6 +156,7 @@ bool DarwinDriver::startDriver(void)
{
this->darwin->mm_stop();
this->darwin->mm_disable_power();
+ this->darwin->mm_disable_power_v2();
}
return false;
}
@@ -143,6 +175,7 @@ bool DarwinDriver::stopDriver(void)
{
this->darwin->mm_stop();
this->darwin->mm_disable_power();
+ this->darwin->mm_disable_power_v2();
}
return true;
@@ -160,13 +193,13 @@ void DarwinDriver::config_update(Config& new_cfg, uint32_t level)
// update driver with new_cfg data
switch(this->getState())
{
- case DarwinDriver::CLOSED:
+ case iri_base_driver::CLOSED:
break;
- case DarwinDriver::OPENED:
+ case iri_base_driver::OPENED:
break;
- case DarwinDriver::RUNNING:
+ case iri_base_driver::RUNNING:
break;
}
@@ -258,6 +291,22 @@ std::vector<double> DarwinDriver::mm_get_servo_angles(void)
return std::vector<double>();
}
+bool DarwinDriver::mm_has_fallen(void)
+{
+ if(this->darwin!=NULL)
+ return this->darwin->mm_has_fallen();
+ else
+ false;
+}
+
+fall_t DarwinDriver::mm_get_fallen_position(void)
+{
+ if(this->darwin!=NULL)
+ return this->darwin->mm_get_fallen_position();
+ else
+ return MM_STANDING;
+}
+
// motion action interface
void DarwinDriver::action_load_page(unsigned char page_id)
{
@@ -452,6 +501,202 @@ double DarwinDriver::walk_get_timeout(void)
return this->config_.cmd_vel_timeout;
}
+/* stairs interface */
+void DarwinDriver::stairs_set_params(TStairsParams *params)
+{
+ if(this->darwin!=NULL)
+ {
+ if(this->stairs_params.shift_weight_left_time!=params->shift_weight_left_time)
+ {
+ this->stairs_params.shift_weight_left_time=params->shift_weight_left_time;
+ this->darwin->stairs_set_phase_time(SHIFT_WEIGHT_LEFT,params->shift_weight_left_time);
+ }
+ if(this->stairs_params.rise_right_foot_time!=params->rise_right_foot_time)
+ {
+ this->stairs_params.rise_right_foot_time=params->rise_right_foot_time;
+ this->darwin->stairs_set_phase_time(RISE_RIGHT_FOOT,params->rise_right_foot_time);
+ }
+ if(this->stairs_params.advance_right_foot_time!=params->advance_right_foot_time)
+ {
+ this->stairs_params.advance_right_foot_time=params->advance_right_foot_time;
+ this->darwin->stairs_set_phase_time(ADVANCE_RIGHT_FOOT,params->advance_right_foot_time);
+ }
+ if(this->stairs_params.contact_right_foot_time!=params->contact_right_foot_time)
+ {
+ this->stairs_params.contact_right_foot_time=params->contact_right_foot_time;
+ this->darwin->stairs_set_phase_time(CONTACT_RIGHT_FOOT,params->contact_right_foot_time);
+ }
+ if(this->stairs_params.shift_weight_right_time!=params->shift_weight_right_time)
+ {
+ this->stairs_params.shift_weight_right_time=params->shift_weight_right_time;
+ this->darwin->stairs_set_phase_time(SHIFT_WEIGHT_RIGHT,params->shift_weight_right_time);
+ }
+ if(this->stairs_params.rise_left_foot_time!=params->rise_left_foot_time)
+ {
+ this->stairs_params.rise_left_foot_time=params->rise_left_foot_time;
+ this->darwin->stairs_set_phase_time(RISE_LEFT_FOOT,params->rise_left_foot_time);
+ }
+ if(this->stairs_params.advance_left_foot_time!=params->advance_left_foot_time)
+ {
+ this->stairs_params.advance_left_foot_time=params->advance_left_foot_time;
+ this->darwin->stairs_set_phase_time(ADVANCE_LEFT_FOOT,params->advance_left_foot_time);
+ }
+ if(this->stairs_params.contact_left_foot_time!=params->contact_left_foot_time)
+ {
+ this->stairs_params.contact_left_foot_time=params->contact_left_foot_time;
+ this->darwin->stairs_set_phase_time(CONTACT_LEFT_FOOT,params->contact_left_foot_time);
+ }
+ if(this->stairs_params.center_weight_time!=params->center_weight_time)
+ {
+ this->stairs_params.center_weight_time=params->center_weight_time;
+ this->darwin->stairs_set_phase_time(CENTER_WEIGHT,params->center_weight_time);
+ }
+ if(this->stairs_params.x_offset!=params->x_offset)
+ {
+ this->stairs_params.x_offset=params->x_offset;
+ this->darwin->stairs_set_x_offset(params->x_offset);
+ }
+ if(this->stairs_params.y_offset!=params->y_offset)
+ {
+ this->stairs_params.y_offset=params->y_offset;
+ this->darwin->stairs_set_y_offset(params->y_offset);
+ }
+ if(this->stairs_params.z_offset!=params->z_offset)
+ {
+ this->stairs_params.z_offset=params->z_offset;
+ this->darwin->stairs_set_z_offset(params->z_offset);
+ }
+ if(this->stairs_params.roll_offset!=params->roll_offset)
+ {
+ this->stairs_params.roll_offset=params->roll_offset;
+ this->darwin->stairs_set_roll_offset(params->roll_offset);
+ }
+ if(this->stairs_params.pitch_offset!=params->pitch_offset)
+ {
+ this->stairs_params.pitch_offset=params->pitch_offset;
+ this->darwin->stairs_set_pitch_offset(params->pitch_offset);
+ }
+ if(this->stairs_params.yaw_offset!=params->yaw_offset)
+ {
+ this->stairs_params.yaw_offset=params->yaw_offset;
+ this->darwin->stairs_set_yaw_offset(params->yaw_offset);
+ }
+ if(this->stairs_params.y_shift!=params->y_shift)
+ {
+ this->stairs_params.y_shift=params->y_shift;
+ this->darwin->stairs_set_y_shift(params->y_shift);
+ }
+ if(this->stairs_params.x_shift!=params->x_shift)
+ {
+ this->stairs_params.x_shift=params->x_shift;
+ this->darwin->stairs_set_x_shift(params->x_shift);
+ }
+ if(this->stairs_params.z_overshoot!=params->z_overshoot)
+ {
+ this->stairs_params.z_overshoot=params->z_overshoot;
+ this->darwin->stairs_set_z_overshoot(params->z_overshoot);
+ }
+ if(this->stairs_params.z_height!=params->z_height)
+ {
+ this->stairs_params.yaw_offset=params->z_height;
+ this->darwin->stairs_set_z_height(params->z_height);
+ }
+ if(this->stairs_params.hip_pitch_offset!=params->hip_pitch_offset)
+ {
+ this->stairs_params.hip_pitch_offset=params->hip_pitch_offset;
+ this->darwin->stairs_set_hip_pitch_offset(params->hip_pitch_offset);
+ }
+ if(this->stairs_params.roll_shift!=params->roll_shift)
+ {
+ this->stairs_params.roll_shift=params->roll_shift;
+ this->darwin->stairs_set_roll_shift(params->roll_shift);
+ }
+ if(this->stairs_params.pitch_shift!=params->pitch_shift)
+ {
+ this->stairs_params.pitch_shift=params->pitch_shift;
+ this->darwin->stairs_set_pitch_shift(params->pitch_shift);
+ }
+ if(this->stairs_params.yaw_shift!=params->yaw_shift)
+ {
+ this->stairs_params.yaw_shift=params->yaw_shift;
+ this->darwin->stairs_set_yaw_shift(params->yaw_shift);
+ }
+ if(this->stairs_params.y_spread!=params->y_spread)
+ {
+ this->stairs_params.y_spread=params->y_spread;
+ this->darwin->stairs_set_y_spread(params->y_spread);
+ }
+ if(this->stairs_params.x_shift_body!=params->x_shift_body)
+ {
+ this->stairs_params.x_shift_body=params->x_shift_body;
+ this->darwin->stairs_set_x_shift_body(params->x_shift_body);
+ }
+ }
+}
+
+void DarwinDriver::stairs_get_params(TStairsParams *params)
+{
+ params->shift_weight_left_time=this->stairs_params.shift_weight_left_time;
+ params->rise_right_foot_time=this->stairs_params.rise_right_foot_time;
+ params->advance_right_foot_time=this->stairs_params.advance_right_foot_time;
+ params->contact_right_foot_time=this->stairs_params.contact_right_foot_time;
+ params->shift_weight_right_time=this->stairs_params.shift_weight_right_time;
+ params->rise_left_foot_time=this->stairs_params.rise_left_foot_time;
+ params->advance_left_foot_time=this->stairs_params.advance_left_foot_time;
+ params->contact_left_foot_time=this->stairs_params.contact_left_foot_time;
+ params->center_weight_time=this->stairs_params.center_weight_time;
+ params->x_offset=this->stairs_params.x_offset;
+ params->y_offset=this->stairs_params.y_offset;
+ params->z_offset=this->stairs_params.z_offset;
+ params->roll_offset=this->stairs_params.roll_offset;
+ params->pitch_offset=this->stairs_params.pitch_offset;
+ params->yaw_offset=this->stairs_params.yaw_offset;
+ params->y_shift=this->stairs_params.y_shift;
+ params->x_shift=this->stairs_params.x_shift;
+ params->z_overshoot=this->stairs_params.z_overshoot;
+ params->z_height=this->stairs_params.z_height;
+ params->hip_pitch_offset=this->stairs_params.hip_pitch_offset;
+ params->roll_shift=this->stairs_params.roll_shift;
+ params->pitch_shift=this->stairs_params.pitch_shift;
+ params->yaw_shift=this->stairs_params.yaw_shift;
+ params->y_spread=this->stairs_params.y_spread;
+ params->x_shift_body=this->stairs_params.x_shift_body;
+}
+
+void DarwinDriver::stairs_go_up(void)
+{
+ if(this->darwin!=NULL)
+ this->darwin->stairs_go_up();
+}
+
+void DarwinDriver::stairs_go_down(void)
+{
+ if(this->darwin!=NULL)
+ this->darwin->stairs_go_down();
+}
+
+void DarwinDriver::stairs_stop(void)
+{
+ if(this->darwin!=NULL)
+ this->darwin->stairs_stop();
+}
+
+bool DarwinDriver::stairs_is_active(void)
+{
+ if(this->darwin!=NULL)
+ return this->darwin->stairs_is_active();
+ else
+ return false;
+}
+
+stairs_phase_t DarwinDriver::stairs_get_phase(void)
+{
+ if(this->darwin!=NULL)
+ return this->darwin->stairs_get_phase();
+ else
+ return (stairs_phase_t)0x00;
+}
+
// joint motion interface
void DarwinDriver::joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
{
@@ -561,10 +806,16 @@ void DarwinDriver::head_get_current_target(double *pan,double *tilt)
}
//smart charger interface
-bool DarwinDriver::is_smart_charger_det_and_en(void)
+bool DarwinDriver::is_smart_charger_detected(void)
+{
+ if(this->darwin!=NULL)
+ return this->darwin->is_smart_charger_detected();
+}
+
+bool DarwinDriver::is_smart_charger_enabled(void)
{
if(this->darwin!=NULL)
- return this->darwin->is_smart_charger_det_and_en();
+ return this->darwin->is_smart_charger_enabled();
}
void DarwinDriver::smart_charger_enable(void)
{
@@ -591,11 +842,21 @@ TChargerData DarwinDriver::smart_charger_get_data(void)
if(this->darwin!=NULL)
return this->darwin->smart_charger_get_data();
}
+double DarwinDriver::smart_charger_get_limit_current(void)
+{
+ if(this->darwin!=NULL)
+ return this->darwin->smart_charger_get_limit_current();
+}
void DarwinDriver::smart_charger_set_limit_current(double limit_current)
{
if(this->darwin!=NULL)
this->darwin->smart_charger_set_limit_current(limit_current);
}
+void DarwinDriver::smart_charger_set_range_current(double min_current, double max_current)
+{
+ if(this->darwin!=NULL)
+ this->darwin->smart_charger_set_range_current(min_current, max_current);
+}
DarwinDriver::~DarwinDriver(void)
{
diff --git a/darwin_driver/src/darwin_driver_node.cpp b/darwin_driver/src/darwin_driver_node.cpp
index aa476b1332921b533d9d95eeb42b3f7a8a55de8a..37803115b36805945e291cd5ed89dc4ac4fb182a 100644
--- a/darwin_driver/src/darwin_driver_node.cpp
+++ b/darwin_driver/src/darwin_driver_node.cpp
@@ -34,6 +34,7 @@ const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
DarwinDriverNode::DarwinDriverNode(ros::NodeHandle &nh) :
iri_base_driver::IriBaseNodeDriver<DarwinDriver>(nh),
+ climb_stairs_aserver_(public_node_handle_, "robot/climb_stairs"),
joint_trajectory_aserver_(public_node_handle_, "robot/joint_trajectory"),
head_follow_target_aserver_(public_node_handle_, "robot/head_follow_target"),
motion_action_aserver_(public_node_handle_, "robot/motion_action")
@@ -41,16 +42,10 @@ DarwinDriverNode::DarwinDriverNode(ros::NodeHandle &nh) :
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
- /* this->darwin->imu_start();
- if(this->darwin->imu_is_accel_detected())
- ROS_INFO("Accelerometer detected");
- else
- ROS_INFO("Accelerometer not detected");
- */
-
// [init publishers]
- this->smart_charger_data_publisher_ = this->public_node_handle_.advertise<darwin_smart_charger_client::smart_charger_data>("smart_charger_data", 1);
+ this->smart_charger_data_publisher_ = this->public_node_handle_.advertise<humanoid_common_msgs::smart_charger_data>("smart_charger_data", 1);
this->buttons_publisher_ = this->public_node_handle_.advertise<humanoid_common_msgs::buttons>("buttons", 1);
+ this->fallen_state_publisher_ = this->public_node_handle_.advertise<std_msgs::Int8>("robot/fallen_state", 1);
this->raw_imu_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("sensors/raw_imu", 1);
this->joint_states_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("joint_states", 1);
@@ -66,25 +61,19 @@ DarwinDriverNode::DarwinDriverNode(ros::NodeHandle &nh) :
// [init services]
-//Configuration of smart charger module
- this->smart_charger_set_config_server_ = this->public_node_handle_.advertiseService("smart_charger_set_config", &DarwinDriverNode::smart_charger_set_configCallback, this);
- pthread_mutex_init(&this->smart_charger_set_config_mutex_,NULL);
+ this->get_stairs_params_server_ = this->public_node_handle_.advertiseService("robot/get_stairs_params", &DarwinDriverNode::get_stairs_paramsCallback, this);
+ pthread_mutex_init(&this->get_stairs_params_mutex_,NULL);
-//Write smart charger limit current
- this->smart_charger_set_limit_current_server_ = this->public_node_handle_.advertiseService("smart_charger_set_limit_current", &DarwinDriverNode::smart_charger_set_limit_currentCallback, this);
- pthread_mutex_init(&this->smart_charger_set_limit_current_mutex_,NULL);
+ this->set_stairs_params_server_ = this->public_node_handle_.advertiseService("robot/set_stairs_params", &DarwinDriverNode::set_stairs_paramsCallback, this);
+ pthread_mutex_init(&this->set_stairs_params_mutex_,NULL);
-//Enable/Disable smart charger module
- this->smart_charger_enable_disable_server_ = this->public_node_handle_.advertiseService("smart_charger_enable_disable", &DarwinDriverNode::smart_charger_enable_disableCallback, this);
- pthread_mutex_init(&this->smart_charger_enable_disable_mutex_,NULL);
+// Get smart charger module configuration
+ this->get_smart_charger_config_server_ = this->public_node_handle_.advertiseService("get_smart_charger_config", &DarwinDriverNode::get_smart_charger_configCallback, this);
+ pthread_mutex_init(&this->get_smart_charger_config_mutex_,NULL);
-//Get period of smart charger module
- this->smart_charger_get_period_server_ = this->public_node_handle_.advertiseService("smart_charger_get_period", &DarwinDriverNode::smart_charger_get_periodCallback, this);
- pthread_mutex_init(&this->smart_charger_get_period_mutex_,NULL);
-
-//Set period of smart charger module
- this->smart_charger_set_period_server_ = this->public_node_handle_.advertiseService("smart_charger_set_period", &DarwinDriverNode::smart_charger_set_periodCallback, this);
- pthread_mutex_init(&this->smart_charger_set_period_mutex_,NULL);
+// Set smart charger module configuration
+ this->set_smart_charger_config_server_ = this->public_node_handle_.advertiseService("set_smart_charger_config", &DarwinDriverNode::set_smart_charger_configCallback, this);
+ pthread_mutex_init(&this->set_smart_charger_config_mutex_,NULL);
this->set_tilt_pid_server_ = this->public_node_handle_.advertiseService("robot/set_tilt_pid", &DarwinDriverNode::set_tilt_pidCallback, this);
pthread_mutex_init(&this->set_tilt_pid_mutex_,NULL);
@@ -110,6 +99,17 @@ DarwinDriverNode::DarwinDriverNode(ros::NodeHandle &nh) :
// [init clients]
// [init action servers]
+ climb_stairs_aserver_.registerStartCallback(boost::bind(&DarwinDriverNode::climb_stairsStartCallback, this, _1));
+ climb_stairs_aserver_.registerStopCallback(boost::bind(&DarwinDriverNode::climb_stairsStopCallback, this));
+ climb_stairs_aserver_.registerIsFinishedCallback(boost::bind(&DarwinDriverNode::climb_stairsIsFinishedCallback, this));
+ climb_stairs_aserver_.registerHasSucceedCallback(boost::bind(&DarwinDriverNode::climb_stairsHasSucceededCallback, this));
+ climb_stairs_aserver_.registerGetResultCallback(boost::bind(&DarwinDriverNode::climb_stairsGetResultCallback, this, _1));
+ climb_stairs_aserver_.registerGetFeedbackCallback(boost::bind(&DarwinDriverNode::climb_stairsGetFeedbackCallback, this, _1));
+ climb_stairs_aserver_.start();
+ this->climb_stairs_active=false;
+ this->climb_stairs_succeeded=false;
+ this->climb_stairs_finished=false;
+
joint_trajectory_aserver_.registerStartCallback(boost::bind(&DarwinDriverNode::joint_trajectoryStartCallback, this, _1));
joint_trajectory_aserver_.registerStopCallback(boost::bind(&DarwinDriverNode::joint_trajectoryStopCallback, this));
joint_trajectory_aserver_.registerIsFinishedCallback(boost::bind(&DarwinDriverNode::joint_trajectoryIsFinishedCallback, this));
@@ -159,33 +159,16 @@ void DarwinDriverNode::mainNodeThread(void)
// [fill msg Header if necessary]
// [fill msg structures]
- // Initialize the topic message structure
- //this->smart_charger_data_smart_charger_data_msg_.data = my_var;
-
// Initialize the topic message structure
- //this->raw_imu_Imu_msg_.data = my_var;
// [fill srv structure and make request to the server]
-
- // To finish the action server with success
- //this->head_follow_target_succeeded=true;
- //this->head_follow_target_finished=true;
- // To finish the action server with failure
- //this->head_follow_target_succeeded=false;
- //this->head_follow_target_finished=true;
-
- // IMPORTANT: it is better to use the boolean variables to control the
- // behavior of the action server instead of direclty calling the action server
- // class functions.
+
+ // [fill action structure and make request to the action server]
// [publish messages]
// Uncomment the following line to publish the topic message
- //this->raw_imu_publisher_.publish(this->raw_imu_Imu_msg_);
-
- // Initialize the topic message structure
- // initialize the joint states message
try{
this->joint_states_JointState_msg_.name.resize(MAX_NUM_SERVOS);
this->joint_states_JointState_msg_.position.resize(MAX_NUM_SERVOS);
@@ -221,7 +204,7 @@ void DarwinDriverNode::mainNodeThread(void)
}
//publish smart charger data when is detected and enabled
- if(this->driver_.is_smart_charger_det_and_en())
+ if(this->driver_.is_smart_charger_detected() && driver_.is_smart_charger_enabled())
{
//ROS_INFO("Publishing smart charger data!");
smart_charger_data=this->driver_.smart_charger_get_data();
@@ -239,8 +222,11 @@ void DarwinDriverNode::mainNodeThread(void)
this->smart_charger_data_smart_charger_data_msg_.header.stamp=ros::Time::now();
this->smart_charger_data_publisher_.publish(this->smart_charger_data_smart_charger_data_msg_);
}
-
-
+
+ // detect fall condition
+ this->fallen_state_msg_.data=(unsigned char)this->driver_.mm_get_fallen_position();
+ this->fallen_state_publisher_.publish(this->fallen_state_msg_);
+
}catch(CException &e){
ROS_WARN_STREAM(e.what());
}
@@ -374,265 +360,229 @@ void DarwinDriverNode::cmd_vel_mutex_exit(void)
/* [service callbacks] */
-bool DarwinDriverNode::smart_charger_set_configCallback(darwin_smart_charger_client::set_config::Request &req, darwin_smart_charger_client::set_config::Response &res)
+bool DarwinDriverNode::get_stairs_paramsCallback(humanoid_common_msgs::get_stairs_params::Request &req, humanoid_common_msgs::get_stairs_params::Response &res)
{
- ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: New Request Received!");
+ TStairsParams params;
+
+ ROS_INFO("DarwinDriverNode::get_stairs_paramsCallback: New Request Received!");
+ //use appropiate mutex to shared variables if necessary
try{
- //use appropiate mutex to shared variables if necessary
this->driver_.lock();
- this->smart_charger_set_config_mutex_enter();
-
- ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Processing New Request!");
-
-//Enable smart charger module
- if(req.enable){
- ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Enabling smart charger module");
- this->driver_.smart_charger_enable();
- res.success_en=true;
- }
-//Disable smart charger module
- if(req.disable && !req.enable){
- ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Disable smart charger module");
- this->driver_.smart_charger_disable();
- res.success_dis=true;
- }
-
-//Set period of smart charger module
- if(req.set_period){
- if(req.period_value > 1.3 && req.period_value < 1.7)
- {
- ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Setting period!");
- this->driver_.smart_charger_set_period(req.period_value);
- res.success_per=true;
- }
- else
- {
- ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Invalid period value");
- res.success_per=false;
- }
- }
-
-//Set limit current
- if(req.set_limit_current)
- {
- ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Setting limit current");
- this->driver_.smart_charger_set_limit_current(req.current_value);
- res.success_cur=true;
- }
+ this->get_stairs_params_mutex_enter();
+ this->driver_.stairs_get_params(¶ms);
+ res.params.PHASE1_TIME=params.shift_weight_left_time;
+ res.params.PHASE2_TIME=params.rise_right_foot_time;
+ res.params.PHASE3_TIME=params.advance_right_foot_time;
+ res.params.PHASE4_TIME=params.contact_right_foot_time;
+ res.params.PHASE5_TIME=params.shift_weight_right_time;
+ res.params.PHASE6_TIME=params.rise_left_foot_time;
+ res.params.PHASE7_TIME=params.advance_left_foot_time;
+ res.params.PHASE8_TIME=params.contact_left_foot_time;
+ res.params.PHASE9_TIME=params.center_weight_time;
+ res.params.X_OFFSET=params.x_offset;
+ res.params.Y_OFFSET=params.y_offset;
+ res.params.Z_OFFSET=params.z_offset;
+ res.params.R_OFFSET=params.roll_offset;
+ res.params.P_OFFSET=params.pitch_offset;
+ res.params.A_OFFSET=params.yaw_offset;
+ res.params.Y_SHIFT=params.y_shift;
+ res.params.X_SHIFT=params.x_shift;
+ res.params.Z_OVERSHOOT=params.z_overshoot;
+ res.params.Z_HEIGHT=params.z_height;
+ res.params.HIP_PITCH_OFFSET=params.hip_pitch_offset;
+ res.params.R_SHIFT=params.roll_shift;
+ res.params.P_SHIFT=params.pitch_shift;
+ res.params.A_SHIFT=params.yaw_shift;
+ res.params.Y_SPREAD=params.y_spread;
+ res.params.X_SHIFT_BODY=params.x_shift_body;
- //unlock previously blocked shared variables
- this->smart_charger_set_config_mutex_exit();
+ this->get_stairs_params_mutex_exit();
this->driver_.unlock();
}catch(CException &e){
ROS_WARN_STREAM(e.what());
- res.success_en=false;
- res.success_dis=false;
- res.success_per=false;
- res.success_cur=false;
- //unlock previously blocked shared variables
- this->smart_charger_set_config_mutex_exit();
+ this->get_stairs_params_mutex_exit();
this->driver_.unlock();
}
-
return true;
}
-void DarwinDriverNode::smart_charger_set_config_mutex_enter(void)
+void DarwinDriverNode::get_stairs_params_mutex_enter(void)
{
- pthread_mutex_lock(&this->smart_charger_set_config_mutex_);
+ pthread_mutex_lock(&this->get_stairs_params_mutex_);
}
-void DarwinDriverNode::smart_charger_set_config_mutex_exit(void)
+void DarwinDriverNode::get_stairs_params_mutex_exit(void)
{
- pthread_mutex_unlock(&this->smart_charger_set_config_mutex_);
+ pthread_mutex_unlock(&this->get_stairs_params_mutex_);
}
-bool DarwinDriverNode::smart_charger_set_limit_currentCallback(darwin_smart_charger_client::set_limit_current::Request &req, darwin_smart_charger_client::set_limit_current::Response &res)
+bool DarwinDriverNode::set_stairs_paramsCallback(humanoid_common_msgs::set_stairs_params::Request &req, humanoid_common_msgs::set_stairs_params::Response &res)
{
- ROS_INFO("DarwinDriverNode::smart_charger_set_limit_currentCallback: New Request Received!");
+ TStairsParams params;
- //use appropiate mutex to shared variables if necessary
- //this->driver_.lock();
- //this->smart_charger_set_limit_current_mutex_enter();
-
- //ROS_INFO("DarwinDriverNode::smart_charger_set_limit_currentCallback: Processing New Request!");
- //do operations with req and output on res
- //res.data2 = req.data1 + my_var;
-
- //unlock previously blocked shared variables
- //this->smart_charger_set_limit_current_mutex_exit();
- //this->driver_.unlock();
+ ROS_INFO("DarwinDriverNode::set_stairs_paramsCallback: New Request Received!");
try{
this->driver_.lock();
- this->smart_charger_set_limit_current_mutex_enter();
-
- if(req.set_limit_current)
- {
- ROS_INFO("DarwinDriverNode::smart_charger_set_limit_currentCallback: Set limit current");
- this->driver_.smart_charger_set_limit_current(req.limit_current);
- res.success=true;
- }
-
- //unlock previously blocked shared variables
- this->smart_charger_set_limit_current_mutex_exit();
+ this->get_stairs_params_mutex_enter();
+ params.shift_weight_left_time=req.params.PHASE1_TIME;
+ params.rise_right_foot_time=req.params.PHASE2_TIME;
+ params.advance_right_foot_time=req.params.PHASE3_TIME;
+ params.contact_right_foot_time=req.params.PHASE4_TIME;
+ params.shift_weight_right_time=req.params.PHASE5_TIME;
+ params.rise_left_foot_time=req.params.PHASE6_TIME;
+ params.advance_left_foot_time=req.params.PHASE7_TIME;
+ params.contact_left_foot_time=req.params.PHASE8_TIME;
+ params.center_weight_time=req.params.PHASE9_TIME;
+ params.x_offset=req.params.X_OFFSET;
+ params.y_offset=req.params.Y_OFFSET;
+ params.z_offset=req.params.Z_OFFSET;
+ params.roll_offset=req.params.R_OFFSET;
+ params.pitch_offset=req.params.P_OFFSET;
+ params.yaw_offset=req.params.A_OFFSET;
+ params.y_shift=req.params.Y_SHIFT;
+ params.x_shift=req.params.X_SHIFT;
+ params.z_overshoot=req.params.Z_OVERSHOOT;
+ params.z_height=req.params.Z_HEIGHT;
+ params.hip_pitch_offset=req.params.HIP_PITCH_OFFSET;
+ params.roll_shift=req.params.R_SHIFT;
+ params.pitch_shift=req.params.P_SHIFT;
+ params.yaw_shift=req.params.A_SHIFT;
+ params.y_spread=req.params.Y_SPREAD;
+ params.x_shift_body=req.params.X_SHIFT_BODY;
+ this->driver_.stairs_set_params(¶ms);
+ res.ret=true;
+ this->get_stairs_params_mutex_exit();
this->driver_.unlock();
-
}catch(CException &e){
ROS_WARN_STREAM(e.what());
- res.success=false;
- this->smart_charger_set_limit_current_mutex_exit();
+ res.ret=false;
+ this->get_stairs_params_mutex_exit();
this->driver_.unlock();
}
return true;
-}
-
-void DarwinDriverNode::smart_charger_set_limit_current_mutex_enter(void)
-{
- pthread_mutex_lock(&this->smart_charger_set_limit_current_mutex_);
-}
-
-void DarwinDriverNode::smart_charger_set_limit_current_mutex_exit(void)
-{
- pthread_mutex_unlock(&this->smart_charger_set_limit_current_mutex_);
-}
-
-bool DarwinDriverNode::smart_charger_enable_disableCallback(darwin_smart_charger_client::smart_charger_enable::Request &req, darwin_smart_charger_client::smart_charger_enable::Response &res)
-{
- ROS_INFO("DarwinDriverNode::smart_charger_enable_disableCallback: New Request Received!");
//use appropiate mutex to shared variables if necessary
- try{
- this->driver_.lock();
- this->smart_charger_enable_disable_mutex_enter();
+ //this->driver_.lock();
+ //this->set_stairs_params_mutex_enter();
- if(req.enable){
- ROS_INFO("DarwinDriverNode::smart_charger_enable_disableCallback: Enable smart charger module");
- this->driver_.smart_charger_enable();
- }else{
- ROS_INFO("DarwinDriverNode::smart_charger_enable_disableCallback: Disable smart charger module");
- this->driver_.smart_charger_disable();
- }
- res.success=true;
+ //ROS_INFO("DarwinDriverNode::set_stairs_paramsCallback: Processing New Request!");
+ //do operations with req and output on res
+ //res.data2 = req.data1 + my_var;
//unlock previously blocked shared variables
- this->smart_charger_enable_disable_mutex_exit();
- this->driver_.unlock();
-
- }catch(CException &e){
- ROS_WARN_STREAM(e.what());
- res.success=false;
- this->smart_charger_enable_disable_mutex_exit();
- this->driver_.unlock();
- }
+ //this->set_stairs_params_mutex_exit();
+ //this->driver_.unlock();
return true;
}
-void DarwinDriverNode::smart_charger_enable_disable_mutex_enter(void)
+void DarwinDriverNode::set_stairs_params_mutex_enter(void)
{
- pthread_mutex_lock(&this->smart_charger_enable_disable_mutex_);
+ pthread_mutex_lock(&this->set_stairs_params_mutex_);
}
-void DarwinDriverNode::smart_charger_enable_disable_mutex_exit(void)
+void DarwinDriverNode::set_stairs_params_mutex_exit(void)
{
- pthread_mutex_unlock(&this->smart_charger_enable_disable_mutex_);
+ pthread_mutex_unlock(&this->set_stairs_params_mutex_);
}
-bool DarwinDriverNode::smart_charger_get_periodCallback(darwin_smart_charger_client::get_period::Request &req, darwin_smart_charger_client::get_period::Response &res)
+bool DarwinDriverNode::get_smart_charger_configCallback(humanoid_common_msgs::get_smart_charger_config::Request &req, humanoid_common_msgs::get_smart_charger_config::Response &res)
{
- ROS_INFO("DarwinDriverNode::smart_charger_get_periodCallback: New Request Received!");
+ ROS_INFO("DarwinDriverNode::get_smart_charger_configCallback: New Request Received!");
//use appropiate mutex to shared variables if necessary
- try{
- this->driver_.lock();
- this->smart_charger_get_period_mutex_enter();
+ this->driver_.lock();
+ this->get_smart_charger_config_mutex_enter();
- //ROS_INFO("DarwinDriverNode::smart_charger_get_periodCallback: Processing New Request!");
- //do operations with req and output on res
- //res.data2 = req.data1 + my_var;
- //this->driver_.smart_charger_get_period((double *)&res.period);
- res.period_value = this->driver_.smart_charger_get_period();
- res.success=true;
+ ROS_INFO("DarwinDriverNode::get_smart_charger_configCallback: Processing New Request!");
+ if(this-> driver_.is_smart_charger_enabled())
+ {
+ res.charger_config.enable = true;
+ res.charger_config.disable = false;
+ }
+ else
+ {
+ res.charger_config.enable = false;
+ res.charger_config.disable = true;
+ }
+ res.charger_config.current = this->driver_.smart_charger_get_limit_current();
+ res.charger_config.period = this->driver_.smart_charger_get_period();
//unlock previously blocked shared variables
- this->smart_charger_get_period_mutex_exit();
- this->driver_.unlock();
- }catch(CException &e){
- ROS_WARN_STREAM(e.what());
- res.success=false;
- this->smart_charger_get_period_mutex_exit();
- this->driver_.unlock();
- }
-
+ this->get_smart_charger_config_mutex_exit();
+ this->driver_.unlock();
+
return true;
}
-void DarwinDriverNode::smart_charger_get_period_mutex_enter(void)
+void DarwinDriverNode::get_smart_charger_config_mutex_enter(void)
{
- pthread_mutex_lock(&this->smart_charger_get_period_mutex_);
+ pthread_mutex_lock(&this->get_smart_charger_config_mutex_);
}
-void DarwinDriverNode::smart_charger_get_period_mutex_exit(void)
+void DarwinDriverNode::get_smart_charger_config_mutex_exit(void)
{
- pthread_mutex_unlock(&this->smart_charger_get_period_mutex_);
+ pthread_mutex_unlock(&this->get_smart_charger_config_mutex_);
}
-bool DarwinDriverNode::smart_charger_set_periodCallback(darwin_smart_charger_client::set_period::Request &req, darwin_smart_charger_client::set_period::Response &res)
+bool DarwinDriverNode::set_smart_charger_configCallback(humanoid_common_msgs::set_smart_charger_config::Request &req, humanoid_common_msgs::set_smart_charger_config::Response &res)
{
- ROS_INFO("DarwinDriverNode::smart_charger_set_periodCallback: New Request Received!");
+ ROS_INFO("DarwinDriverNode::set_smart_charger_configCallback: New Request Received!");
- //use appropiate mutex to shared variables if necessary
- //this->driver_.lock();
- //this->smart_charger_set_period_mutex_enter();
-
- //ROS_INFO("DarwinDriverNode::smart_charger_set_periodCallback: Processing New Request!");
- //do operations with req and output on res
- //res.data2 = req.data1 + my_var;
-
- //unlock previously blocked shared variables
- //this->smart_charger_set_period_mutex_exit();
- //this->driver_.unlock();
-
try{
+ //use appropiate mutex to shared variables if necessary
this->driver_.lock();
- this->smart_charger_set_period_mutex_enter();
+ this->set_smart_charger_config_mutex_enter();
+
+ ROS_INFO("DarwinDriverNode::set_smart_charger_configCallback: Processing New Request!");
- if(req.period > 1.3 && req.period < 1.7)
+ //Enable smart charger module
+ if(req.charger_config.enable)
{
- this->driver_.smart_charger_set_period(req.period);
- res.ret=true;
+ ROS_INFO("DarwinDriverNode::set_smart_charger_configCallback: Enabling smart charger module");
+ this->driver_.smart_charger_enable();
+ res.success=true;
+ }
+ //Disable smart charger module
+ else if(req.charger_config.disable)
+ {
+ ROS_INFO("DarwinDriverNode::set_smart_charger_configCallback: Disable smart charger module");
+ this->driver_.smart_charger_disable();
+ res.success=true;
}
else
{
- ROS_INFO("DarwinDriverNode::smart_charger_set_periodCallback: Invalid period value");
- res.ret=false;
+ ROS_INFO("DarwinDriverNode::set_smart_charger_configCallback: Setting period and limit current!");
+ this->driver_.smart_charger_set_period(req.charger_config.period);
+ this->driver_.smart_charger_set_limit_current(req.charger_config.current);
+ res.success=true;
}
-
- //unlock previously blocked shared variables
- this->smart_charger_set_period_mutex_exit();
+
+ //unlock previously blocked shared variables
+ this->set_smart_charger_config_mutex_exit();
this->driver_.unlock();
}catch(CException &e){
ROS_WARN_STREAM(e.what());
- res.ret=false;
- this->smart_charger_set_period_mutex_exit();
+ res.success=false;
+ //unlock previously blocked shared variables
+ this->set_smart_charger_config_mutex_exit();
this->driver_.unlock();
}
-
return true;
}
-void DarwinDriverNode::smart_charger_set_period_mutex_enter(void)
+void DarwinDriverNode::set_smart_charger_config_mutex_enter(void)
{
- pthread_mutex_lock(&this->smart_charger_set_period_mutex_);
+ pthread_mutex_lock(&this->set_smart_charger_config_mutex_);
}
-void DarwinDriverNode::smart_charger_set_period_mutex_exit(void)
+void DarwinDriverNode::set_smart_charger_config_mutex_exit(void)
{
- pthread_mutex_unlock(&this->smart_charger_set_period_mutex_);
+ pthread_mutex_unlock(&this->set_smart_charger_config_mutex_);
}
bool DarwinDriverNode::set_tilt_pidCallback(humanoid_common_msgs::set_pid::Request &req, humanoid_common_msgs::set_pid::Response &res)
@@ -912,6 +862,105 @@ void DarwinDriverNode::set_servo_modules_mutex_exit(void)
/* [action callbacks] */
+void DarwinDriverNode::climb_stairsStartCallback(const humanoid_common_msgs::humanoid_stairsGoalConstPtr& goal)
+{
+ try{
+ this->driver_.lock();
+ //check goal
+ if(goal->up)
+ this->driver_.stairs_go_up();
+ else
+ this->driver_.stairs_go_down();
+ this->climb_stairs_active=true;
+ this->climb_stairs_succeeded=false;
+ this->climb_stairs_finished=false;
+ //execute goal
+ this->driver_.unlock();
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ this->climb_stairs_active=false;
+ this->climb_stairs_succeeded=false;
+ this->climb_stairs_finished=true;
+ this->driver_.unlock();
+ }
+}
+
+void DarwinDriverNode::climb_stairsStopCallback(void)
+{
+ try{
+ this->driver_.lock();
+ //stop action
+ this->driver_.stairs_stop();
+ this->climb_stairs_active=false;
+ this->driver_.unlock();
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ this->driver_.unlock();
+ }
+}
+
+bool DarwinDriverNode::climb_stairsIsFinishedCallback(void)
+{
+ bool ret = false;
+
+ try{
+ this->driver_.lock();
+ //if action has finish for any reason
+ if(this->driver_.stairs_is_active())
+ this->climb_stairs_finished=false;
+ else
+ {
+ this->climb_stairs_finished=true;
+ this->climb_stairs_succeeded=true;
+ }
+ ret = this->climb_stairs_finished;
+ this->driver_.unlock();
+ return ret;
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ this->driver_.unlock();
+ }
+}
+
+bool DarwinDriverNode::climb_stairsHasSucceededCallback(void)
+{
+ bool ret = false;
+
+ try{
+ this->driver_.lock();
+ //if goal was accomplished
+ ret = this->climb_stairs_succeeded;
+ this->climb_stairs_active=false;
+ this->driver_.unlock();
+ return ret;
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ this->driver_.unlock();
+ }
+}
+
+void DarwinDriverNode::climb_stairsGetResultCallback(humanoid_common_msgs::humanoid_stairsResultPtr& result)
+{
+ this->driver_.lock();
+ //update result data to be sent to client
+ //result->data = data;
+ this->driver_.unlock();
+}
+
+void DarwinDriverNode::climb_stairsGetFeedbackCallback(humanoid_common_msgs::humanoid_stairsFeedbackPtr& feedback)
+{
+ try{
+ this->driver_.lock();
+ //update feedback data to be sent to client
+ feedback->current_phase=this->driver_.stairs_get_phase();
+ //ROS_INFO("feedback: %s", feedback->data.c_str());
+ this->driver_.unlock();
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ this->driver_.unlock();
+ }
+}
+
void DarwinDriverNode::joint_trajectoryStartCallback(const control_msgs::JointTrajectoryGoalConstPtr& goal)
{
unsigned int i=0,j=0;
@@ -1266,11 +1315,10 @@ void DarwinDriverNode::reconfigureNodeHook(int level)
DarwinDriverNode::~DarwinDriverNode(void)
{
// [free dynamic memory]
- pthread_mutex_destroy(&this->smart_charger_set_config_mutex_);
- pthread_mutex_destroy(&this->smart_charger_set_limit_current_mutex_);
- pthread_mutex_destroy(&this->smart_charger_enable_disable_mutex_);
- pthread_mutex_destroy(&this->smart_charger_get_period_mutex_);
- pthread_mutex_destroy(&this->smart_charger_set_period_mutex_);
+ pthread_mutex_destroy(&this->get_stairs_params_mutex_);
+ pthread_mutex_destroy(&this->set_stairs_params_mutex_);
+ pthread_mutex_destroy(&this->get_smart_charger_config_mutex_);
+ pthread_mutex_destroy(&this->set_smart_charger_config_mutex_);
pthread_mutex_destroy(&this->leds_mutex_);
pthread_mutex_destroy(&this->set_tilt_pid_mutex_);
pthread_mutex_destroy(&this->set_pan_pid_mutex_);
@@ -1286,5 +1334,5 @@ DarwinDriverNode::~DarwinDriverNode(void)
/* main function */
int main(int argc,char *argv[])
{
- return driver_base::main<DarwinDriverNode>(argc, argv, "darwin_driver_node");
+ return iri_base_driver::main<DarwinDriverNode>(argc, argv, "darwin_driver_node");
}
diff --git a/darwin_gazebo/launch/darwin.launch b/darwin_gazebo/launch/darwin.launch
index b4ab0e6538ff830ccf7d31c1d5771f8db6bb3754..8857a5c39ccaef8d1621c4791d71ae9de1212495 100644
--- a/darwin_gazebo/launch/darwin.launch
+++ b/darwin_gazebo/launch/darwin.launch
@@ -14,7 +14,7 @@
</include>
<!-- Spawn a robot into Gazebo -->
- <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model darwin -x 0 -y 0 -z 0.4 -R 1.6 -P 0 -Y 0" />
+ <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model darwin -x 0 -y 0 -z 0.49 -R 1.6 -P 0 -Y 0" />
</launch>
diff --git a/darwin_nav/config/darwin_nav.rviz b/darwin_nav/config/darwin_nav.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..693ab40692e2ab0af006a7b0228a513e4c8d4a42
--- /dev/null
+++ b/darwin_nav/config/darwin_nav.rviz
@@ -0,0 +1,687 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /TF1/Frames1
+ - /TF1/Tree1
+ - /local_plan1
+ Splitter Ratio: 0.385563
+ Tree Height: 655
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.03
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ darwin_accel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ darwin_cam_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ darwin_cam_optical_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ darwin_gyro:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ darwin_imu_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ head:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_arm_high:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_arm_low:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_leg_ankle_pitch:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_leg_ankle_roll:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_leg_knee:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_leg_pelvis_pitch:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_leg_pelvis_roll:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_leg_pelvis_yaw:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_shoulder:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ neck:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_arm_high:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_arm_low:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_leg_ankle_pitch:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_leg_ankle_roll:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_leg_knee:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_leg_pelvis_pitch:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_leg_pelvis_roll:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_leg_pelvis_yaw:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_shoulder:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: /darwin
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ E125:
+ Value: true
+ E175:
+ Value: true
+ E225:
+ Value: true
+ E25:
+ Value: true
+ E75:
+ Value: true
+ N125:
+ Value: true
+ N125_cam:
+ Value: true
+ N125_det:
+ Value: true
+ N175:
+ Value: true
+ N175_cam:
+ Value: true
+ N175_det:
+ Value: true
+ N25:
+ Value: true
+ N25_cam:
+ Value: true
+ N25_det:
+ Value: true
+ N75:
+ Value: true
+ N75_cam:
+ Value: true
+ N75_det:
+ Value: true
+ S125:
+ Value: true
+ S175:
+ Value: true
+ S25:
+ Value: true
+ S75:
+ Value: true
+ W125:
+ Value: true
+ W175:
+ Value: true
+ W225:
+ Value: true
+ W25:
+ Value: true
+ W75:
+ Value: true
+ darwin/base_footprint:
+ Value: true
+ darwin/base_link:
+ Value: true
+ darwin/darwin_accel:
+ Value: true
+ darwin/darwin_cam_link:
+ Value: true
+ darwin/darwin_cam_optical_link:
+ Value: true
+ darwin/darwin_gyro:
+ Value: true
+ darwin/darwin_imu_link:
+ Value: true
+ darwin/head:
+ Value: true
+ darwin/left_arm_high:
+ Value: true
+ darwin/left_arm_low:
+ Value: true
+ darwin/left_leg_ankle_pitch:
+ Value: true
+ darwin/left_leg_ankle_roll:
+ Value: true
+ darwin/left_leg_knee:
+ Value: true
+ darwin/left_leg_pelvis_pitch:
+ Value: true
+ darwin/left_leg_pelvis_roll:
+ Value: true
+ darwin/left_leg_pelvis_yaw:
+ Value: true
+ darwin/left_shoulder:
+ Value: true
+ darwin/neck:
+ Value: true
+ darwin/odom:
+ Value: true
+ darwin/right_arm_high:
+ Value: true
+ darwin/right_arm_low:
+ Value: true
+ darwin/right_leg_ankle_pitch:
+ Value: true
+ darwin/right_leg_ankle_roll:
+ Value: true
+ darwin/right_leg_knee:
+ Value: true
+ darwin/right_leg_pelvis_pitch:
+ Value: true
+ darwin/right_leg_pelvis_roll:
+ Value: true
+ darwin/right_leg_pelvis_yaw:
+ Value: true
+ darwin/right_shoulder:
+ Value: true
+ obstacle_base_fence_link:
+ Value: true
+ obstacle_base_link:
+ Value: true
+ Marker Scale: 0.2
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ obstacle_base_link:
+ E125:
+ {}
+ E175:
+ {}
+ E225:
+ {}
+ E25:
+ {}
+ E75:
+ {}
+ N125:
+ {}
+ N175:
+ {}
+ N25:
+ {}
+ N75:
+ {}
+ S125:
+ {}
+ S175:
+ {}
+ S25:
+ {}
+ S75:
+ {}
+ W125:
+ {}
+ W175:
+ {}
+ W225:
+ {}
+ W25:
+ {}
+ W75:
+ {}
+ darwin/odom:
+ darwin/base_footprint:
+ darwin/base_link:
+ darwin/darwin_accel:
+ {}
+ darwin/darwin_gyro:
+ {}
+ darwin/darwin_imu_link:
+ {}
+ darwin/left_leg_pelvis_yaw:
+ darwin/left_leg_pelvis_roll:
+ darwin/left_leg_pelvis_pitch:
+ darwin/left_leg_knee:
+ darwin/left_leg_ankle_pitch:
+ darwin/left_leg_ankle_roll:
+ {}
+ darwin/left_shoulder:
+ darwin/left_arm_high:
+ darwin/left_arm_low:
+ {}
+ darwin/neck:
+ darwin/head:
+ darwin/darwin_cam_link:
+ darwin/darwin_cam_optical_link:
+ N125_cam:
+ {}
+ N125_det:
+ {}
+ N175_cam:
+ {}
+ N175_det:
+ {}
+ N25_cam:
+ {}
+ N25_det:
+ {}
+ N75_cam:
+ {}
+ N75_det:
+ {}
+ darwin/right_leg_pelvis_yaw:
+ darwin/right_leg_pelvis_roll:
+ darwin/right_leg_pelvis_pitch:
+ darwin/right_leg_knee:
+ darwin/right_leg_ankle_pitch:
+ darwin/right_leg_ankle_roll:
+ {}
+ darwin/right_shoulder:
+ darwin/right_arm_high:
+ darwin/right_arm_low:
+ {}
+ obstacle_base_fence_link:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ E125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E225:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ N125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ N175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ N25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ N75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W225:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ obstacle_base_fence_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ obstacle_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: Environment
+ Robot Description: obstacles_environment
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/Image
+ Enabled: false
+ Image Topic: /darwin/camera/image_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: compressed
+ Unreliable: false
+ Value: false
+ - Alpha: 0.7
+ Class: rviz/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: false
+ Name: local_costmap
+ Topic: /move_base/local_costmap/costmap
+ Unreliable: false
+ Value: false
+ - Alpha: 0.7
+ Class: rviz/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: false
+ Name: global_costmap
+ Topic: /move_base/global_costmap/costmap
+ Unreliable: false
+ Value: false
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 25; 255; 0
+ Enabled: true
+ Head Diameter: 0.3
+ Head Length: 0.2
+ Length: 0.3
+ Line Style: Lines
+ Line Width: 0.03
+ Name: global_plan
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.03
+ Shaft Diameter: 0.1
+ Shaft Length: 0.1
+ Topic: /move_base/GlobalPlanner/plan
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 0; 0; 255
+ Enabled: true
+ Head Diameter: 0.3
+ Head Length: 0.2
+ Length: 0.3
+ Line Style: Lines
+ Line Width: 0.03
+ Name: local_plan
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.03
+ Shaft Diameter: 0.1
+ Shaft Length: 0.1
+ Topic: /move_base/DWAPlannerROS/local_plan
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 255; 0; 0
+ Enabled: true
+ Head Diameter: 0.3
+ Head Length: 0.2
+ Length: 0.3
+ Line Style: Lines
+ Line Width: 0.03
+ Name: local_global_plan
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.03
+ Shaft Diameter: 0.1
+ Shaft Length: 0.1
+ Topic: /move_base/DWAPlannerROS/global_plan
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: obstacle_base_link
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /darwin/qr_global_loc/initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 5.08288
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.06
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 1.38816
+ Y: 1.12817
+ Z: 1.12501
+ Name: Current View
+ Near Clip Distance: 0.01
+ Pitch: 0.505398
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 0.375398
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 936
+ Hide Left Dock: false
+ Hide Right Dock: false
+ Image:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a0000031efc0200000008fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000031e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002190000012d0000001600ffffff000000010000010f0000031efc0200000004fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000000280000006e0000006400fffffffb0000000a00560069006500770073010000009c000002aa000000b000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000031b000000a30000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b70000003efc0100000002fb0000000800540069006d00650100000000000004b7000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002320000031e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1207
+ X: 55
+ Y: 14
diff --git a/darwin_nav/config/dwa_local_planner_params.yaml b/darwin_nav/config/dwa_local_planner_params.yaml
index 974a25acfca4ce4cd26e982e86f5dfea5a953b46..391b477659d707d2bcdeba5d06a6542c2e9fdcc4 100644
--- a/darwin_nav/config/dwa_local_planner_params.yaml
+++ b/darwin_nav/config/dwa_local_planner_params.yaml
@@ -1,18 +1,18 @@
base_local_planner: "dwa_local_planner/DWAPlannerROS"
DWAPlannerROS:
- max_trans_vel: 0.24
+ max_trans_vel: 0.1
min_trans_vel: 0.01
- max_vel_x: 0.24
- min_vel_x: -0.24
+ max_vel_x: 0.1
+ min_vel_x: -0.1
max_vel_y: 0.1
min_vel_y: -0.1
- max_rot_vel: 0.1
- min_rot_vel: 0.01
+ max_rot_vel: 0.4
+ min_rot_vel: 0.1
- sim_time: 3
+ sim_time: 10
sim_granularity: 0.025
- angular_sim_granularity: 0.1
+ angular_sim_granularity: 0.01
path_distance_bias: 32.0
goal_distance_bias: 24.0
@@ -26,19 +26,19 @@ DWAPlannerROS:
scaling_speed: 0.25
max_scaling_factor: 0.2
- vx_samples: 10
- vy_samples: 10
+ vx_samples: 21
+ vy_samples: 1
vth_samples: 20
- acc_lim_theta: 0.1
- acc_lim_x: 0.1
- acc_lim_y: 0.1
- acc_limit_trans: 0.1
+ acc_lim_theta: 0.3
+ acc_lim_x: 0.3
+ acc_lim_y: 0.3
+ acc_limit_trans: 0.3
rot_stopped_vel: 0.01
trans_stopped_vel: 0.01
- xy_goal_tolerance: 0.05
+ xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.05
#not in dynamic reconfigure
@@ -46,7 +46,7 @@ DWAPlannerROS:
publish_cost_grid_pc: false #true, useful to play with goal/path_distance_bias parameters
#move_base namespace
- latch_xy_goal_tolerance: true #false
+latch_xy_goal_tolerance: true #false
#prune_plan: true
#use_dwa: true
diff --git a/darwin_nav/launch/darwin_global_loc_sim.launch b/darwin_nav/launch/darwin_global_loc_sim.launch
index 7ed7985f83f8cf8056d7effbb6d3f9d8b1136286..b4a952c4411facdbdf9d4203cfbf144d57d5405a 100644
--- a/darwin_nav/launch/darwin_global_loc_sim.launch
+++ b/darwin_nav/launch/darwin_global_loc_sim.launch
@@ -7,7 +7,7 @@
output="screen">
<param name="qr_x" value="0.2"/>
<param name="qr_y" value="0.2"/>
- <param name="camera_frame" value="/darwin/camera_link"/>
+ <param name="camera_frame" value="/darwin/darwin_cam_link"/>
<remap from="~/camera/image_raw" to="/darwin/camera/image_raw"/>
<remap from="~/camera/camera_info" to="/darwin/camera/camera_info"/>
</node>
@@ -16,12 +16,11 @@
name="qr_global_loc"
type="qr_global_loc"
output="screen">
-<!-- <param name="qr_map_file" value="$(find qr_global_loc)/urdf/ceabot_map.urdf"/>-->
- <param name="qr_map_param" value="obstacles_environment"/>
+ <param name="qr_map_param" value="/obstacles_environment"/>
<param name="tf_prefix" value="/darwin"/>
<param name="publish_tf" value="True"/>
<param name="publish_rate" value="20"/>
- <remap from="~/qr_pose" to="/darwin/qr_detector/qr_pose"/>
+ <remap from="~/qr_pose" to="/darwin/qr_pose"/>
</node>
</group>
diff --git a/darwin_nav/launch/global_loc_sim.launch b/darwin_nav/launch/global_loc_sim.launch
index 111f7912a257890172d606c1e134fa0601063d9e..5736ec4d6945be42d1dc39a2b4601a7ec2688a0b 100644
--- a/darwin_nav/launch/global_loc_sim.launch
+++ b/darwin_nav/launch/global_loc_sim.launch
@@ -1,14 +1,14 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="robot" default="darwin" />
+ <arg name="robot" default="darwin_cpp" />
<arg name="environment" default="obstacles_env_empty" />
<include file="$(find darwin_description)/launch/darwin_sim.launch">
<arg name="robot" value="$(arg robot)" />
</include>
- <include file="$(find bioloid_description)/launch/obstacles_env.launch">
+ <include file="$(find darwin_description)/launch/environment.launch">
<arg name="environment" value="$(arg environment)" />
</include>
@@ -21,7 +21,7 @@
</node>
<!-- launch the walk client node -->
- <node name="walk_client"
+<!-- <node name="walk_client"
pkg="walk_client"
type="walk_client"
output="screen"
@@ -49,7 +49,7 @@
to="/darwin/robot/set_walk_params"/>
<remap from="/darwin/walk_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
- </node>
+ </node>-->
<!-- launch dynamic reconfigure -->
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
diff --git a/darwin_odometry/src/darwin_odometry_alg_node.cpp b/darwin_odometry/src/darwin_odometry_alg_node.cpp
index 0fd98cf9bc7a0007fd7293dcf378d1194568f8d2..b942af893abd1ddced4e393df513e63e5c605902 100644
--- a/darwin_odometry/src/darwin_odometry_alg_node.cpp
+++ b/darwin_odometry/src/darwin_odometry_alg_node.cpp
@@ -112,7 +112,7 @@ void DarwinOdometryAlgNode::joint_states_callback(const sensor_msgs::JointState:
odom_trans.transform.translation.x = 0.0;
odom_trans.transform.translation.y = 0.0;
odom_trans.transform.translation.z = odom_pos[2];
- odom_trans.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(odom_rot[0],odom_rot[1],0.0);
+ odom_trans.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(odom_rot[0],odom_rot[1],1.5707);
this->odom_broadcaster.sendTransform(odom_trans);
odom_trans.header.frame_id = this->config_.tf_prefix+"/odom";
odom_trans.child_frame_id = this->config_.tf_prefix+"/base_footprint";
@@ -120,7 +120,7 @@ void DarwinOdometryAlgNode::joint_states_callback(const sensor_msgs::JointState:
odom_trans.transform.translation.x = odom_pos[0];
odom_trans.transform.translation.y = odom_pos[1];
odom_trans.transform.translation.z = 0.0;
- odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(odom_rot[2]);
+ odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(odom_rot[2]-1.5707);
this->odom_broadcaster.sendTransform(odom_trans);
}
}catch(CException &e){
diff --git a/darwin_robot/package.xml b/darwin_robot/package.xml
index 2ad889a113567eeb31e8009da2bbfba17d263e36..cf8a634a9a3f460562ee83ce8ae070d448745b02 100644
--- a/darwin_robot/package.xml
+++ b/darwin_robot/package.xml
@@ -39,6 +39,7 @@
<!-- <run_depend>message_runtime</run_depend> -->
<run_depend>darwin_control</run_depend>
<run_depend>darwin_controller</run_depend>
+ <run_depend>darwin_controller_cpp</run_depend>
<run_depend>darwin_description</run_depend>
<run_depend>darwin_gazebo</run_depend>
<run_depend>darwin_driver</run_depend>
@@ -50,13 +51,12 @@
<run_depend>darwin_smart_charger_client</run_depend>
<run_depend>sm_qr_search</run_depend>
<run_depend>track_charge_station</run_depend>
- <!-- <run_depend>darwin_robot_driver</run_depend>-->
-<!-- <run_depend>darwin_arm_kinematics</run_depend>-->
+ <run_depend>darwin_robot_driver</run_depend>
+ <run_depend>darwin_nav</run_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
-
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
diff --git a/darwin_smart_charger_client/src/darwin_smart_charger_client_alg_node.cpp b/darwin_smart_charger_client/src/darwin_smart_charger_client_alg_node.cpp
index 0a9f213435c93761876c7eef5b08c0706c20aca2..73e6dc3611d4dfbc34f7e9c372e7b3594b9f9d64 100644
--- a/darwin_smart_charger_client/src/darwin_smart_charger_client_alg_node.cpp
+++ b/darwin_smart_charger_client/src/darwin_smart_charger_client_alg_node.cpp
@@ -160,7 +160,7 @@ void DarwinSmartChargerClientAlgNode::node_config_update(Config &config, uint32_
config.enable=false;
}
//Disable smart charger module
- if(config.disable && !config.enable)
+ else if(config.disable)
{
smart_charger_set_config_srv_.request.disable=true;
smart_charger_set_config_srv_.request.enable=false;
diff --git a/sm_qr_search/cfg/SmQrSearch.cfg b/sm_qr_search/cfg/SmQrSearch.cfg
index 2f42ab6246cf10f62a97fab7ca5b6f382f21fb5b..4ac9c9861378651bc7e07c867ffb5fd5e0756739 100755
--- a/sm_qr_search/cfg/SmQrSearch.cfg
+++ b/sm_qr_search/cfg/SmQrSearch.cfg
@@ -39,47 +39,13 @@ gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
-gen.add("start", bool_t, 0, "Start state machine", False)
-gen.add("stop", bool_t, 0, "Stop state machine", False)
-#Joint trajectory
-gen.add("load", bool_t, 0, "Load search parameters", False)
-gen.add("pan_angle_init", double_t, 0, "Initial search pan angle", -1.5, -1.5, 0.0)
-gen.add("pan_angle_end", double_t, 0, "End search pan angle", 1.5, 0.0, 1.5)
+gen.add("start", bool_t, 0, "Start state machine", False)
+gen.add("stop", bool_t, 0, "Stop state machine", False)
gen.add("tilt_angle_search", double_t, 0, "Search tilt angle", 0.8, 0.0, 1.0)
-gen.add("speed", double_t, 0, "Servo speed (rad/s)", 1, 0, 6.2832)
-gen.add("acceleration", double_t, 0, "Servo acceleration (rad/s/s)", 0.3, 0, 6.2832)
-#Walk
-gen.add("Y_SWAP_AMPLITUDE", double_t, 0, "Y swap amplitude (m)", 0.025, 0, 0.050)
-gen.add("Z_SWAP_AMPLITUDE", double_t, 0, "X swap amplitude (m)", 0.005, 0, 0.050)
-gen.add("ARM_SWING_GAIN", double_t, 0, "Arm swing gain", 1.5, 0, 10.0)
-gen.add("PELVIS_OFFSET", double_t, 0, "Pelvis offset (rad)", 0.052, 0, 0.52)
-gen.add("HIP_PITCH_OFFSET", double_t, 0, "Hip pitch offset (rad)", 0.174, 0, 0.52)
-gen.add("X_OFFSET", double_t, 0, "X offset (m)", -0.025, -0.050, 0.050)
-gen.add("Y_OFFSET", double_t, 0, "Y offset (m)", 0.0075, -0.050, 0.050)
-gen.add("Z_OFFSET", double_t, 0, "Z offset (m)", 0.035, -0.050, 0.050)
-gen.add("A_OFFSET", double_t, 0, "A offset (rad)", 0, -0.52, 0.52)
-gen.add("P_OFFSET", double_t, 0, "P offset (rad)", 0, -0.52, 0.52)
-gen.add("R_OFFSET", double_t, 0, "R offset (rad)", 0, -0.52, 0.52)
-gen.add("PERIOD_TIME", double_t, 0, "Period (s)", 0.6, 0, 1.0)
-gen.add("DSP_RATIO", double_t, 0, "Double support ratio", 0.1, 0, 1.0)
-gen.add("STEP_FB_RATIO", double_t, 0, "Step forward/backward ratio", 0.3, 0, 1.0)
-gen.add("FOOT_HEIGHT", double_t, 0, "Foot height (m)", 0.04, 0, 0.050)
-gen.add("MAX_VEL", double_t, 0, "Maximum linear velocity (m/s)", 0.01, 0, 1.0)
-gen.add("MAX_ROT_VEL", double_t, 0, "Maximum rotational velocity (rad/s)", 0.01, 0, 1.0)
-#Head tracking
-gen.add("pan_p", double_t, 0, "Desired pan kp", 0.1, 0.0, 0.28)
-gen.add("pan_i", double_t, 0, "Desired pan ki", 0.0, 0.0, 0.28)
-gen.add("pan_d", double_t, 0, "Desired pan kd", 0.0, 0.0, 0.28)
-gen.add("update_pan_pid", bool_t, 0, "Update pan pid", False)
-gen.add("pan_i_clamp", double_t, 0, "Desired pan integral limit", 0.0, 0.0, 0.28)
-gen.add("max_pan", double_t, 0, "Maximum pan angle", 3.14159, -3.14159, 3.14159)
-gen.add("min_pan", double_t, 0, "Minimum pan angle", -3.14159, -3.14159, 3.14159)
-gen.add("tilt_p", double_t, 0, "Desired tilt kp", 0.1, 0.0, 0.28)
-gen.add("tilt_i", double_t, 0, "Desired tilt ki", 0.0, 0.0, 0.28)
-gen.add("tilt_d", double_t, 0, "Desired tilt kd", 0.0, 0.0, 0.28)
-gen.add("update_tilt_pid", bool_t, 0, "Update tilt pid", False)
-gen.add("tilt_i_clamp", double_t, 0, "Desired tilt integral limit", 0.0, 0.0, 0.28)
-gen.add("max_tilt", double_t, 0, "Maximum tilt angle", 3.14159, -3.14159, 3.14159)
-gen.add("min_tilt", double_t, 0, "Minimum tilt angle", -3.14159, -3.14159, 3.14159)
+gen.add("head_speed", double_t, 0, "Servo speed (rad/s)", 1, 0, 6.2832)
+gen.add("head_acceleration", double_t, 0, "Servo acceleration (rad/s/s)", 0.3, 0, 6.2832)
+gen.add("yaw_step", double_t, 0, "Amplitud of yaw rotation (rad)", 0.1, 0, 0.2)
+gen.add("angle_rotation", double_t, 0, "Angle of rotation", 3.1415, 0.78, 3.1415)
+gen.add("error", double_t, 0, "Min error in rotation", 0.01, 0.01, 0.1)
exit(gen.generate(PACKAGE, "SmQrSearchAlgorithm", "SmQrSearch"))
diff --git a/sm_qr_search/include/sm_qr_search_alg_node.h b/sm_qr_search/include/sm_qr_search_alg_node.h
index 78f4e36f069d671f8a66be0440ed5c78166dbe16..95a8376cc0d688a3cc7bc3573dd5a4167eb2e6fb 100644
--- a/sm_qr_search/include/sm_qr_search_alg_node.h
+++ b/sm_qr_search/include/sm_qr_search_alg_node.h
@@ -33,13 +33,11 @@
// [publisher subscriber headers]
#include <nav_msgs/Odometry.h>
-#include <geometry_msgs/Twist.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <humanoid_common_msgs/tag_pose_array.h>
#include <sensor_msgs/JointState.h>
// [service client headers]
-#include <humanoid_common_msgs/set_walk_params.h>
#include <humanoid_common_msgs/set_servo_modules.h>
// [action server client headers]
@@ -50,10 +48,22 @@
//MODULES
#include <humanoid_modules/joints_module.h>
-
+#include <humanoid_modules/walk_module.h>
//states
-typedef enum {IDLE, START, SEARCH, TRACK_QR, ROTATE, WAIT_ROTATE} States;
+typedef enum {SEARCH_MODULE_IDLE,
+ SEARCH_MODULE_START,
+ SEARCH_MODULE_SEARCH,
+ SEARCH_MODULE_TRACK,
+ SEARCH_MODULE_START_ROTATE,
+ SEARCH_MODULE_ROTATE} search_module_states_t;
+//status
+typedef enum {SEARCH_MODULE_SUCCESS,
+ SEARCH_MODULE_CANCELED,
+ SEARCH_MODULE_FAIL,
+ SEARCH_MODULE_RUNNING,
+ SEARCH_MODULE_ERROR_JOINTS,
+ SEARCH_MODULE_ERROR_WALK } search_module_status_t;
/**
* \brief IRI ROS Specific Algorithm Class
@@ -63,11 +73,9 @@ class SmQrSearchAlgNode : public algorithm_base::IriBaseAlgorithm<SmQrSearchAlgo
{
private:
// [publisher attributes]
- ros::Publisher cmd_vel_publisher_;
- geometry_msgs::Twist cmd_vel_Twist_msg_;
- ros::Publisher head_target_publisher_;
- trajectory_msgs::JointTrajectoryPoint head_target_JointTrajectoryPoint_msg_;
+ // ros::Publisher head_target_publisher_;
+ // trajectory_msgs::JointTrajectoryPoint head_target_JointTrajectoryPoint_msg_;
// [subscriber attributes]
@@ -93,67 +101,66 @@ class SmQrSearchAlgNode : public algorithm_base::IriBaseAlgorithm<SmQrSearchAlgo
// [service attributes]
// [client attributes]
- ros::ServiceClient set_walk_params_client_;
- humanoid_common_msgs::set_walk_params set_walk_params_srv_;
- ros::ServiceClient set_servo_modules_client_;
- humanoid_common_msgs::set_servo_modules set_servo_modules_srv_;
+// ros::ServiceClient set_servo_modules_client_;
+// humanoid_common_msgs::set_servo_modules set_servo_modules_srv_;
// [action server attributes]
// [action client attributes]
-// actionlib::SimpleActionClient<control_msgs::JointTrajectoryAction> joint_trajectory_client_;
-// control_msgs::JointTrajectoryGoal joint_trajectory_goal_;
-// bool joint_trajectoryMakeActionRequest();
-// void joint_trajectoryDone(const actionlib::SimpleClientGoalState& state, const control_msgs::JointTrajectoryResultConstPtr& result);
-// void joint_trajectoryActive();
-// void joint_trajectoryFeedback(const control_msgs::JointTrajectoryFeedbackConstPtr& feedback);
-
- actionlib::SimpleActionClient<humanoid_common_msgs::humanoid_follow_targetAction> head_follow_target_client_;
+
+ /* actionlib::SimpleActionClient<humanoid_common_msgs::humanoid_follow_targetAction> head_follow_target_client_;
humanoid_common_msgs::humanoid_follow_targetGoal head_follow_target_goal_;
bool head_follow_targetMakeActionRequest();
void head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result);
void head_follow_targetActive();
void head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback);
-
- States state;
- bool new_goal;
- bool end;
- bool executing_trajectory;
+*/
+ search_module_status_t status;
+ search_module_states_t state;
+ bool new_search;
+ bool cancel_search;
+ bool search_success;
double current_pan_angle;
double current_tilt_angle;
- double next_pan_angle;
- double next_tilt_angle;
- double pan_angle_init;
- double pan_angle_end;
double tilt_angle_search;
+ double pan_angle_search;
//qr
double pan_angle;
double tilt_angle;
- bool tracking;
bool qr_detected;
- unsigned int counter;
//odometry
- double odom_x;
- double odom_y;
+ bool initial_odometry;
+ bool stop_rotating;
double odom_rot;
-
+ // double init_odom_rot;
+ // double actual_odom_rot;
+ double yaw_step;
+ unsigned int total_rotations;
+ double odom_target;
+ double rotate_angle;
+ double rotate_deviation (double ini, double actual);
+ double get_target (double a, double b);
+ bool start_odometry;
+ double odom_error;
+
CROSWatchdog watchdog_qr_lost;
- //CROSWatchdog watchdog_walk;
- //CROSWatchdog feedback_watchdog;
- /*
- unsigned int b[2];
- double a[2];
+
std::vector<double> speed;
- std::vector<double> accel; //2 elements with value 0.3
+ std::vector<double> accel;
std::vector<double> angle;
std::vector<unsigned int> id;
- */
+
///JOINTS MODULE
CJointsModule joints_trajectory_module;
std::vector<double> angle_feedback;
+ //WALK MODULE
+ CWalkModule walk_module;
+
+
+
/**
* \brief config variable
@@ -231,6 +238,14 @@ class SmQrSearchAlgNode : public algorithm_base::IriBaseAlgorithm<SmQrSearchAlgo
//Head tracking
void set_head_tracking_goal(double pan_target, double tilt_target);
void send_head_target(double pan, double tilt);
+ /*control functions */
+ void start_search(double angle_rotation, double error, const double head_speed=1.0, const double head_acceleration=0.3, const double yaw_step=0.1);
+ void stop_search(void);
+ void get_position(double &pan_pos, double &tilt_pos);
+ bool is_finished(void);
+ search_module_status_t get_status(void);
+ joints_module_status_t get_joints_module_status(void);
+ walk_module_status_t get_walk_module_status(void);
};
#endif
diff --git a/sm_qr_search/launch/search_sim.launch b/sm_qr_search/launch/search_sim.launch
index 0623581730748f7fd123ea7a3dad8d2be290117d..a81986d0f60f94450ad2f7537b79a8059435887d 100644
--- a/sm_qr_search/launch/search_sim.launch
+++ b/sm_qr_search/launch/search_sim.launch
@@ -1,14 +1,18 @@
<launch>
-
- <arg name="robot" default="darwin" />
- <arg name="environment" default="vision_env" />
+<!--
+<include file="$(find darwin_apps)/launch/charge/charge_sim.launch">
+ <arg name="environment" value="$(arg environment)" />
+ </include>
+ -->
+ <arg name="robot" default="darwin" />
+ <arg name="environment" default="charge_env" />
<include file="$(find darwin_description)/launch/darwin_sim.launch">
<arg name="robot" value="$(arg robot)" />
</include>
- <include file="$(find bioloid_description)/launch/vision_env.launch">
+ <include file="$(find darwin_description)/launch/charge_env.launch">
<arg name="environment" value="$(arg environment)" />
</include>
@@ -18,10 +22,11 @@
type="sm_qr_search"
output="screen"
ns="/darwin">
- <remap from="/darwin/sm_qr_search/cmd_vel"
- to="/darwin/robot/cmd_vel"/>
- <remap from="/darwin/sm_qr_search/set_walk_params"
- to="/darwin/robot/set_walk_params"/>
+ <param name="joint_trajectory/enable_timeout" value="false"/>
+ <param name="joint_trajectory/rate_hz" value="5.0"/>
+ <param name="/walk/rate_hz" value="5.0"/>
+ <param name="walk/cmd_vel_rate_hz" value="50.0"/>
+
<remap from="/darwin/sm_qr_search/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
<remap from="/darwin/head_follow_target"
@@ -32,38 +37,49 @@
to="/darwin/joint_states"/>
<remap from="/darwin/sm_qr_search/qr_pose"
to="/qr_detector/qr_pose"/>
- <remap from="/darwin/joint_trajectory"
+ <remap from="/darwin/sm_qr_search/odom"
+ to="/darwin/robot/odom" />
+<!-- Walk module -->
+ <remap from="/darwin/sm_qr_search/walk/cmd_vel"
+ to="/darwin/robot/cmd_vel"/>
+ <remap from="/darwin/sm_qr_search/walk/set_walk_params"
+ to="/darwin/robot/set_walk_params"/>
+ <remap from="/darwin/sm_qr_search/walk/get_walk_params"
+ to="/darwin/robot/get_walk_params"/>
+ <remap from="/darwin/sm_qr_search/walk/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/sm_qr_search/walk/joint_states"
+ to="/darwin/joint_states"/>
+<!-- Joints module -->
+ <remap from="/darwin/sm_qr_search/joint_trajectory/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/sm_qr_search/joint_trajectory/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/sm_qr_search/joint_trajectory/joint_trajectory_action"
to="/darwin/robot/joint_trajectory"/>
</node>
- <!-- <include file="$(find darwin_apps)/launch/xs_camera_qr.launch"/>-->
- <include file="$(find qr_detector)/launch/tracker_darwin.launch"/>
+ <node pkg="qr_detector"
+ name="qr_detector"
+ type="qr_detector"
+ output="screen">
+ <param name="qr_x" value="0.2"/>
+ <param name="qr_y" value="0.2"/>
+ <param name="camera_frame" value="/darwin/camera_link"/>d
+ <remap from="~/camera/image_raw" to="/darwin/camera/image_raw"/>
+ <remap from="~/camera/camera_info" to="/darwin/camera/camera_info"/>
+ </node>
- <node pkg="image_view"
- name="image_view"
+ <node pkg="image_view"
+ name="image_view"
type="image_view"
- args="image:=/darwin/usb_cam/image_raw">
+ args="image:=/darwin/camera/image_raw">
</node>
<!-- launch dynamic reconfigure -->
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
output="screen"/>
-
- <!-- load the controllers -->
- <node name="darwin_odometry"
- pkg="darwin_odometry"
- type="darwin_odometry"
- output="screen"
- ns="/darwin">
- <param name="left_leg_chain_file" value="$(find darwin_odometry)/config/left_leg.xml"/>
- <param name="right_leg_chain_file" value="$(find darwin_odometry)/config/right_leg.xml"/>
- <param name="tf_prefix" value="/darwin"/>
- <param name="publish_tf" value="True"/>
- <remap from="/darwin/darwin_odometry/joint_states"
- to="/darwin/joint_states"/>
- <remap from="/darwin/darwin_odometry/odom"
- to="/darwin/sm_qr_search/odom"/>
- </node>
+
</launch>
diff --git a/sm_qr_search/src/sm_qr_search_alg_node.cpp b/sm_qr_search/src/sm_qr_search_alg_node.cpp
index cf106bb11941aaa1514276e0de018c3daee16d1d..a37010f3e890ae8221de276083203f31d5dd2beb 100644
--- a/sm_qr_search/src/sm_qr_search_alg_node.cpp
+++ b/sm_qr_search/src/sm_qr_search_alg_node.cpp
@@ -10,55 +10,19 @@
#include "sm_qr_search_alg_node.h"
-/*
-#define NUM_SERVOS 32
-// joint names
-const std::string servo_names[NUM_SERVOS]={std::string("Servo0"),
- std::string("j_shoulder_pitch_r"),
- std::string("j_shoulder_pitch_l"),
- std::string("j_shoulder_roll_r"),
- std::string("j_shoulder_roll_l"),
- std::string("j_elbow_r"),
- std::string("j_elbow_l"),
- std::string("j_hip_yaw_r"),
- std::string("j_hip_yaw_l"),
- std::string("j_hip_roll_r"),
- std::string("j_hip_roll_l"),
- std::string("j_hip_pitch_r"),
- std::string("j_hip_pitch_l"),
- std::string("j_knee_r"),
- std::string("j_knee_l"),
- std::string("j_ankle_pitch_r"),
- std::string("j_ankle_pitch_l"),
- std::string("j_ankle_roll_r"),
- std::string("j_ankle_roll_l"),
- std::string("j_pan"),
- std::string("j_tilt"),
- std::string("Servo21"),
- std::string("Servo22"),
- std::string("Servo23"),
- std::string("Servo24"),
- std::string("Servo25"),
- std::string("Servo26"),
- std::string("Servo27"),
- std::string("Servo28"),
- std::string("Servo29"),
- std::string("Servo30"),
- std::string("Servo31")};
-
- */
+#define PI 3.14159
+
SmQrSearchAlgNode::SmQrSearchAlgNode(void) :
algorithm_base::IriBaseAlgorithm<SmQrSearchAlgorithm>(),
- //joint_trajectory_client_("joint_trajectory", true),
joints_trajectory_module("joint_trajectory"),
- head_follow_target_client_("head_follow_target", true)
+ walk_module("walk")
+// head_follow_target_client_("head_follow_target", true)
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
- this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
- this->head_target_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("head_target", 1);
+// this->head_target_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("head_target", 1);
// [init subscribers]
this->odom_subscriber_ = this->public_node_handle_.subscribe("odom", 1, &SmQrSearchAlgNode::odom_callback, this);
@@ -74,62 +38,58 @@ SmQrSearchAlgNode::SmQrSearchAlgNode(void) :
// [init services]
// [init clients]
- set_walk_params_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_walk_params>("set_walk_params");
- set_servo_modules_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_servo_modules>("set_servo_modules");
+// set_servo_modules_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_servo_modules>("set_servo_modules");
// [init action servers]
// [init action clients]
- this->state=IDLE;
- this->new_goal=false;
- this->end=false;
- this->executing_trajectory=false;
- this->next_pan_angle=-1.5;
- this->next_tilt_angle=0.8;
+ this->status=SEARCH_MODULE_SUCCESS;
+ this->state=SEARCH_MODULE_IDLE;
+ this->new_search=false;
+ this->cancel_search=false;
+ this->search_success=false;
+
- this->pan_angle_init=-1.5;
- this->pan_angle_end=1.5;
- this->tilt_angle_search=0.8;
this->current_pan_angle=0.0;
this->current_tilt_angle=0.0;
//qr
this->qr_detected=false;
- this->pan_angle=0.0;
- this->tilt_angle=0.0;
- this->tracking=false;
- this->counter=0;
-
-
- //actionlib::SimpleClientGoalState joint_trajectory_state(actionlib::SimpleClientGoalState::PENDING);
+ // this->pan_angle=0.0;
+ // this->tilt_angle=0.0;
+// this->tracking=false;
+ this->yaw_step=0.1;
+ this->rotate_angle = PI; //default: turn 180º
+ this->pan_angle_search = rotate_angle/2;
+ this->total_rotations = (PI/pan_angle_search)-1;
+ this->tilt_angle_search=0.8;
- //head_follow
- this->head_follow_target_goal_.pan_range.resize(2);
- this->head_follow_target_goal_.tilt_range.resize(2);
+ ROS_INFO("TOTAL ROTATIONS: %d", total_rotations);
+
+
+ this->angle.resize(2);
+ this->angle[0]=pan_angle_search;
+ this->angle[1]=tilt_angle_search;
+ this->speed.resize(2);
+ this->speed[0]=1;
+ this->speed[1]=1;
+ this->accel.resize(2);
+ this->accel[0]=0.3;
+ this->accel[0]=0.3;
+ this->id.resize(2);
+ this->id[0]=19;
+ this->id[1]=20;
- //joint_trajectory
-// this->joint_trajectory_goal_.trajectory.points.resize(1);
-// this->joint_trajectory_goal_.trajectory.joint_names.resize(2);
-// this->joint_trajectory_goal_.trajectory.points[0].positions.resize(2);
-// this->joint_trajectory_goal_.trajectory.points[0].velocities.resize(2);
-// this->joint_trajectory_goal_.trajectory.points[0].accelerations.resize(2);
-//
-//
-// //head_target
- this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
- //this->head_target_JointTrajectoryPoint_msg_.velocities.resize(2);
-
-/* this->b={19,20};
- this->a={-1,0.8};
- this->speed(2,1);
- this->accel (2,0.3); //2 elements with value 0.3
- this->angle (a, a + sizeof(a) / sizeof(double) );
- this->id (b, b + sizeof(b) / sizeof(unsigned int) );
- */
+
+//Odometry
+ this->initial_odometry=false;
+ this->stop_rotating=false;
+ this->start_odometry=false;
+ this->odom_error=0.01;
}
SmQrSearchAlgNode::~SmQrSearchAlgNode(void)
@@ -142,26 +102,16 @@ SmQrSearchAlgNode::~SmQrSearchAlgNode(void)
void SmQrSearchAlgNode::mainNodeThread(void)
{
+ unsigned int num_rotations;
+ static signed int sign;
/* // [fill msg structures]
// Initialize the topic message structure
- //this->cmd_vel_Twist_msg_.data = my_var;
// Initialize the topic message structure
//this->head_target_JointTrajectoryPoint_msg_.data = my_var;
// [fill srv structure and make request to the server]
- //set_walk_params_srv_.request.data = my_var;
- //ROS_INFO("SmQrSearchAlgNode:: Sending New Request!");
- //if (set_walk_params_client_.call(set_walk_params_srv_))
- //{
- //ROS_INFO("SmQrSearchAlgNode:: Response: %s", set_walk_params_srv_.response.result);
- //}
- //else
- //{
- //ROS_INFO("SmQrSearchAlgNode:: Failed to Call Server on topic set_walk_params ");
- //}
-
//set_servo_modules_srv_.request.data = my_var;
//ROS_INFO("SmQrSearchAlgNode:: Sending New Request!");
@@ -218,265 +168,225 @@ void SmQrSearchAlgNode::mainNodeThread(void)
// [publish messages]
// Uncomment the following line to publish the topic message
- //this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
// Uncomment the following line to publish the topic message
//this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
*/
- unsigned int b[2]={19,20};
- double a[2]={-1.5,0.8};
- std::vector<double> speed(2,1);
- std::vector<double> accel(2,0.3); //2 elements with value 0.3
- std::vector<double> angle(a, a + sizeof(a) / sizeof(double) );
- std::vector<unsigned int> id(b, b + sizeof(b) / sizeof(unsigned int) );
-
-/* unsigned int id[2]={19,20};
- double angle[2]={-1,0.8};
- double speed[2]={1,1};
- double accel[2]={0.3};
- */
-
- switch(state)
+ if(this->cancel_search)
{
- case IDLE:
- //Poner el robot a la posicion de inicio
- ROS_INFO("IDLE");
- if(new_goal)
- {
- ROS_INFO("Start");
- new_goal=false;
-// set_servo_module_head("joints"); //set head servos to joints
-// joint_trajectory_set_goal(pan_angle_init, tilt_angle_search);
-// if(joint_trajectory_start_action())
-// {
-// // ROS_INFO("Start Joint trajectory action");
-// state=POS_INIT;
-// }
- if(this->joints_trajectory_module.execute(id,angle,speed,accel))
- //if(this->joints_trajectory_module.get_status()==JOINTS_MODULE_RUNNING)
- state=START;
- // state=START;
- }
- else
- state=IDLE;
- break;
+ ROS_INFO("CSearchModule: Search canceled");
+ this->joints_trajectory_module.stop();
+ this->walk_module.stop();
+ this->state=SEARCH_MODULE_IDLE;
+ this->status=SEARCH_MODULE_CANCELED;
+ this->cancel_search=false;
+ }
+ else
+ {
+ switch(this->state)
+ {
+ case SEARCH_MODULE_IDLE: ROS_INFO("CSearchModule: state IDLE");
+ if(this->new_search)
+ {
+ ROS_INFO("CSearchModule: New Search!");
+ this->search_success=false;
+ this->new_search=false;
+ sign=-1;
+ num_rotations=0;
+ this->angle[0]=sign*pan_angle_search;
+ std::vector<double> fast_speed(2,2);
+ this->joints_trajectory_module.execute(id,angle,fast_speed,accel);
+ this->status=SEARCH_MODULE_RUNNING;
+ this->state=SEARCH_MODULE_START;
+ }
+ else
+ this->state=SEARCH_MODULE_IDLE;
+ break;
+
+
+ //Wait joint trajectory to execute
+ case SEARCH_MODULE_START: ROS_INFO("CSearchModule: state START");
+ if(!this->joints_trajectory_module.is_finished())
+ {
+ ROS_INFO("CSearchModule: Going to initial position");
+ state=SEARCH_MODULE_START;
+ }
+ else
+ {
+ if(this->joints_trajectory_module.get_status()==JOINTS_MODULE_SUCCESS)
+ {
+ ROS_INFO("CSearchModule: Arrived to initial position");
+ this->angle[0]=pan_angle_search; //1.5
+ this->joints_trajectory_module.execute(this->id,this->angle,this->speed,this->accel); //start search (go to end position
+ this->state=SEARCH_MODULE_SEARCH;
+ }
+ else
+ {
+ ROS_INFO("CSearchModule: Failed in arriving to initial position");
+ this->status=SEARCH_MODULE_ERROR_JOINTS;
+ this->state=SEARCH_MODULE_IDLE;
+ }
+ }
+ break;
- //Wait joint trajectory to execute
- case START:
- ROS_INFO("Start Search");
- //if(!this->joints_trajectory_module.is_finished()){
- if(this->joints_trajectory_module.get_status()==JOINTS_MODULE_RUNNING){
- ROS_INFO("Going to initial position");
- state=START;
-
- }else{
- angle[0]=1.5; //Change pan to end
- if(this->joints_trajectory_module.execute(id,angle,speed,accel))
- state=SEARCH;
-
- // joint_trajectory_set_goal(pan_angle_end, tilt_angle_search);
- // if(joint_trajectory_start_action())
- // {
- // ROS_INFO("Start search");
- // state=SEARCH;
- //}else
- //ROS_INFO("New trajectory canceled");
- }
- break;
+ case SEARCH_MODULE_SEARCH: ROS_INFO("CSearchModule: state SEARCH");
+ if(qr_detected)
+ {
+ ROS_INFO("CSearchmodule: QR detected!");
+ this->joints_trajectory_module.stop();
+ this->search_success=true;
+ this->status=SEARCH_MODULE_SUCCESS;
+ this->state=SEARCH_MODULE_IDLE;
+ /* set_servo_module_head("head");
+ this->tracking=true;
+ set_head_tracking_goal(this->current_pan_angle, this->current_tilt_angle);
+ //set_head_tracking_goal(angle_feedback[0], angle_feedback[1]);
+ if(head_follow_targetMakeActionRequest())
+ {
+ state=SEARCH_MODULE_TRACK;
+ }
+ */
+ }
+ else
+ {
+ if(!this->joints_trajectory_module.is_finished())
+ {
+ ROS_INFO("CSearchModule: Searching...");
+ this->state=SEARCH_MODULE_SEARCH;
+ angle_feedback=this->joints_trajectory_module.get_current_angle();
+ }
+ else
+ {
+ if(this->joints_trajectory_module.get_status()==JOINTS_MODULE_SUCCESS)
+ {
+ ROS_INFO("CSearchModule: ");
+ angle_feedback=this->joints_trajectory_module.get_current_angle();
+ //ROS_INFO("Angulo pan: %f", angle_feedback[0]);
+ this->state=SEARCH_MODULE_START_ROTATE;
+ }
+ else
+ {
+ ROS_INFO("CSearchModule: Failed in moving head joints");
+ this->status=SEARCH_MODULE_ERROR_JOINTS;
+ this->state=SEARCH_MODULE_IDLE;
+ }
+ }
+ }
+ break;
+
+ //Maquina de estados tracking
+ case SEARCH_MODULE_TRACK: ROS_INFO("CSearchModule: state TRACK");
+ if(this->watchdog_qr_lost.is_active())
+ {
+ ROS_INFO("QR lost");
+ // this->tracking=false;
+ // this->head_follow_target_client_.cancelGoal();
+ // this->angle[0]=pan_angle_init;
+ // angle[0]=-1.5;
+ this->joints_trajectory_module.execute(this->id, this->angle, this->speed, this->accel);
+ state=SEARCH_MODULE_START;
+ }
+ else
+ {
+ this->state=SEARCH_MODULE_TRACK;
+ }
+ break;
- case SEARCH:
- ROS_INFO("SEARCH");
- if(qr_detected){
- ROS_INFO("QR detected");
- this->joints_trajectory_module.stop();
- //this->joint_trajectory_client_.cancelGoal();
- set_servo_module_head("head");
- this->tracking=true;
- set_head_tracking_goal(this->current_pan_angle, this->current_tilt_angle);
- //set_head_tracking_goal(angle_feedback[0], angle_feedback[1]);
- if(head_follow_targetMakeActionRequest())
- {
- state=TRACK_QR;
- }
- }else{
- if(this->joints_trajectory_module.is_finished()){
- ROS_INFO("End of search");
- state=IDLE;
- }else{
- angle_feedback=this->joints_trajectory_module.get_current_angle();
- //std::cout << "Current pan: " << angle_feedback[0] << std::endl;
- //std::cout << "Current tilt: " <<angle_feedback[1] << std::endl;
- //ROS_INFO("current pan angle: %f", this->current_pan_angle);
- }
- }
-
- break;
-
- case TRACK_QR:
- ROS_INFO("TRACK QR");
- if(this->watchdog_qr_lost.is_active())
- {
- ROS_INFO("QR lost");
- this->tracking=false;
- this->head_follow_target_client_.cancelGoal();
- angle[0]=-1.5;
- //this->joints_trajectory_module.execute_goal(19,-1,1,0.3);
- if( this->joints_trajectory_module.execute(id, angle, speed, accel))
-// set_servo_module_head("joints");
-// joint_trajectory_set_goal(pan_angle_init, tilt_angle_search);
-// if(joint_trajectory_start_action())
-// {
- // ROS_INFO("Start Joint trajectory action");
- state=START;
- // }
- }
- //}
- else if(end)
- {
- this->end=false;
- this->tracking=false;
- this->qr_detected=false;
- this->head_follow_target_client_.cancelGoal();
- state=IDLE;
- }
- else
- {
- counter=0;
- state=TRACK_QR;
- }
- break;
+ case SEARCH_MODULE_START_ROTATE: ROS_INFO("CSearchModule: state START ROTATION");
+ if(num_rotations==total_rotations)
+ {
+ ROS_INFO("CSearchModule: End of search. Unsuccessful");
+ this->status=SEARCH_MODULE_FAIL;
+ this->state=SEARCH_MODULE_IDLE;
+ }
+ else
+ {
+ num_rotations++;
+ // ROS_INFO("num rotations: %d", num_rotations);
+ // walk_module.add_whole_body();
+ initial_odometry=true;
+ walk_module.set_steps_size(0.0,0.0,this->yaw_step);
+ this->state=SEARCH_MODULE_ROTATE;
+ }
+ break;
- /* case ROTATE:
- ROS_INFO("Set servos to 'walk'");
- set_servo_module("walk");
- ROS_INFO("Set Walk params");
- if(set_walk_params(void))
- {
- ROS_INFO("Turning around");
- send_cmd_vel(0,0,1);
- //sleep(20);
- state=WAIT_ROTATE;
- }
- break;
-
- case WAIT_ROTATE:
- if(watchdog_rotate.is_active())
- {
- send_cmd_vel(0,0,0);
- ROS_INFO("Set head servos to 'joints'");
- set_servo_module_head("joints");
- joint_trajectory_set_goal(pan_angle_init, tilt_angle_search);
- if(joint_trajectory_start_action())
- {
- state=POS_INIT;
- }
- }
- else
- {
- state=WAIT_ROTATE;
- break;
- */
-
-
-
- /* case ACTION:
- ROS_INFO("NavModule: nav_action_wait");
- if(nav_module->use_nav_timeout && nav_module->nav_timeout.timed_out())
- {
- ROS_ERROR("NavModule: goal timed out");
- // cancel the current action
- this->nav_access.exit();
- this->head_follow_target_client_.cancelGoal();
- this->nav_access.enter();
- // stop the timer
- nav_module->nav_timeout.stop();
- // set the error
- nav_module->state=HANDLE_ERROR;
- }joint_states
- else if(this->->feedback_watchdog.is_active())
- {
- ROS_ERROR("NavModule: goal feedback watchdog, %fs", nav_module->config.move_feedback_watchdog);
- // cancel the current action
- nav_module->nav_access.exit();
- this->move_base_client->cancelGoal();joint_states
- nav_module->nav_access.enter();
- // set the error
- nav_module->status=NAV_MODULE_FB_WATCHDOG;
- this->state=HANDLE_ERROR;
- }
- else
- {
- nav_module->nav_access.exit();
- joint_trajectory_state=nav_module->move_base_client->getState();
- nav_module->nav_access.enter();
- if(joint_trajectory_state!=actionlib::SimpleClientGoalState::ACTIVE)
- {
- if(joint_trajectory_state==actionlib::SimpleClientGoalState::ABORTED)
- {
- ROS_ERROR("NavModule: goal aborted");
- nav_module->status=NAV_MODULE_ABORTED;
- }
- else if(joint_trajectory_state==actionlib::SimpleClientGoalState::PREEMPTED)
- {
- ROS_WARN("NavModule: goal preempted");
- nav_module->status=NAV_MODULE_PREEMPTED;
- }
- else if(joint_trajectory_state==actionlib::SimpleClientGoalState::SUCCEEDED)
- {
- ROS_DEBUG("NavModule: goal successfull");
- nav_module->status=NAV_MODULE_SUCCESS;
- }
-
- nav_module->running=false;
- nav_module->state=nav_idle;
- }
- else
- {
- ROS_DEBUG("NavModule: goal active");
- nav_module->state=nav_action_wait;
- }
- }
- if(nav_module->cancel_pending)
- {
- nav_module->cancel_pending=false;
- nav_module->nav_access.exit();
- nav_module->move_base_client->cancelGoal();
- nav_module->nav_access.enter();
- }
+ case SEARCH_MODULE_ROTATE: ROS_INFO("CSearchModule: State ROTATE");
+ start_odometry=true;
+ if(stop_rotating)
+ {
+ ROS_INFO("CSearchModule: Stop rotating");
+ stop_rotating=false;
+ start_odometry=false;
+ this->walk_module.stop();
+ this->angle[0]=sign*pan_angle_search;
+ sign=sign*(-1);
+ this->joints_trajectory_module.execute(this->id, this->angle, this->speed, this->accel);
+ this->state=SEARCH_MODULE_SEARCH;
+ }
+ else
+ state=SEARCH_MODULE_ROTATE;
break;
- */
+
+ }
+ }
+}
-
-
-
+double SmQrSearchAlgNode::rotate_deviation (double ini, double actual)
+{
+ double inc = actual - ini;
+ if (inc<(-PI)) inc+=(2*PI);
+ else if (inc>PI) inc-=(2*PI);
+ return fabs(inc);
}
+double SmQrSearchAlgNode::get_target (double a, double b)
+{
+ double res = a + b;
+ if (res < 0 ) res+= (2*PI);
+ else if (res>(2*PI))
+ res-=(2*PI);
+ return res;
}
-/* [subscriber callbacks] */
void SmQrSearchAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
- //ROS_INFO("SmQrSearchAlgNode::odom_callback: New Message Received");
-
- //use appropiate mutex to shared variables if necessary
+ double init_odom_rot, actual_odom_rot;
+
//this->alg_.lock();
this->odom_mutex_enter();
- odom_x=msg->pose.pose.position.x;
- odom_y=msg->pose.pose.position.y;
- //odom_rot=msg->pose.pose.orientation;
- odom_rot=tf::getYaw(msg->pose.pose.orientation);
- //ROS_INFO("Orientacion Odometria: %f", odom_rot);
+ //save initial odometry
+ if(initial_odometry)
+ {
+ init_odom_rot=tf::getYaw(msg->pose.pose.orientation);
+ init_odom_rot = init_odom_rot + rotate_angle; //change range (0-2pi) //paso del rango (-pi)-(pi) al rango 0-(2pi)
+ odom_target=get_target(init_odom_rot, PI); //calculate target (segun el angulo que depende de total_rotations) //pi para 1 vuelta 180º
+ initial_odometry=false;
+ }
+
+ else if(start_odometry)
+ {
+ odom_rot=tf::getYaw(msg->pose.pose.orientation); //get actual odometry
+ actual_odom_rot = odom_rot + PI; //change range (0-2pi) //paso del rango (-pi)-(pi) al rango 0-(2pi)
+ if((rotate_deviation(actual_odom_rot,odom_target))<odom_error) //Calculate difference
+ stop_rotating=true;
+ // ROS_INFO("TARGET: %f", odom_target);
+ // ROS_INFO("ACTUAL: %f", actual_odom_rot);
+ // ROS_INFO("diferencia: %f", rotate_deviation(actual_odom_rot,odom_target));
+ }
//unlock previously blocked shared variables
//this->alg_.unlock();
this->odom_mutex_exit();
}
+/* [subscriber callbacks] */
+
+
void SmQrSearchAlgNode::odom_mutex_enter(void)
{
pthread_mutex_lock(&this->odom_mutex_);
@@ -503,22 +413,20 @@ void SmQrSearchAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_ar
//angle_feedback=this->joints_trajectory_module.get_current_angle();
- this->pan_angle=this->current_pan_angle+atan2(-msg->tags[0].position.x,msg->tags[0].position.z);
- this->tilt_angle=this->current_tilt_angle+atan2(-msg->tags[0].position.y,msg->tags[0].position.z);
+ this->pan_angle=this->current_pan_angle+atan2(msg->tags[0].position.x,msg->tags[0].position.z);
+ this->tilt_angle=this->current_tilt_angle+atan2(msg->tags[0].position.y,msg->tags[0].position.z);
ROS_INFO("Next target pan angle: %f (%f,%f)",this->pan_angle,this->current_pan_angle,atan2(msg->tags[0].position.x,msg->tags[0].position.z));
ROS_INFO("Next target tilt angle: %f (%f,%f)",this->tilt_angle,this->current_tilt_angle,atan2(msg->tags[0].position.y,msg->tags[0].position.z));
-
+ /*
if(this->tracking)
{
send_head_target(pan_angle, tilt_angle);
}
-
+ */
this->watchdog_qr_lost.reset(ros::Duration(5));
- counter=0;
}else{
//ROS_INFO("QR not detected");
qr_detected=false;
- counter++;
}
@@ -530,6 +438,7 @@ void SmQrSearchAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_ar
}
+
void SmQrSearchAlgNode::qr_pose_mutex_enter(void)
{
pthread_mutex_lock(&this->qr_pose_mutex_);
@@ -539,7 +448,9 @@ void SmQrSearchAlgNode::qr_pose_mutex_exit(void)
{
pthread_mutex_unlock(&this->qr_pose_mutex_);
}
- //Se utiliza en el joints_module
+
+
+
void SmQrSearchAlgNode::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
{
//ROS_INFO("SmQrSearchAlgNode::joint_states_callback: New Message Received");
@@ -585,50 +496,6 @@ void SmQrSearchAlgNode::joint_states_mutex_exit(void)
/* [action callbacks] */
/*
-void SmQrSearchAlgNode::joint_trajectoryDone(const actionlib::SimpleClientGoalState& state, const control_msgs::JointTrajectoryResultConstPtr& result)
-{
- alg_.lock();
- if( state == actionlib::SimpleClientGoalState::SUCCEEDED )
- {
- //ROS_INFO("SmQrSearchAlgNode::joint_trajectoryDone: Goal Achieved!");
- this->executing_trajectory=false;
- //next_pan_angle=
- }
- else
- ROS_INFO("SmQrSearchAlgNode::joint_trajectoryDone: %s", state.toString().c_str());
-
- //copy & work with requested result
- alg_.unlock();
-}
-
-void SmQrSearchAlgNode::joint_trajectoryActive()
-{
- alg_.lock();
- //ROS_INFO("SmQrSearchAlgNode::joint_trajectoryActive: Goal just went active!");
- alg_.unlock();
-}
-
-void SmQrSearchAlgNode::joint_trajectoryFeedback(const control_msgs::JointTrajectoryFeedbackConstPtr& feedback)
-{
- alg_.lock();
- //ROS_INFO("SmQrSearchAlgNode::joint_trajectoryFeedback: Got Feedback!");
-
- bool feedback_is_ok = true;
-
- //this->feedback_watchdog.reset(ros::Duration(2));
-
- //analyze feedback
- //my_var = feedback->var;
-
- //if feedback is not what expected, cancel requested goal
- if( !feedback_is_ok )
- {
- joint_trajectory_client_.cancelGoal();
- //ROS_INFO("SmQrSearchAlgNode::joint_trajectoryFeedback: Cancelling Action!");
- }
- alg_.unlock();
-}
-*/
void SmQrSearchAlgNode::head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result)
{
alg_.lock();
@@ -666,38 +533,10 @@ void SmQrSearchAlgNode::head_follow_targetFeedback(const humanoid_common_msgs::h
}
alg_.unlock();
}
-
+*/
/* [action requests] */
/*
-bool SmQrSearchAlgNode::joint_trajectoryMakeActionRequest()
-{
- // IMPORTANT: Please note that all mutex used in the client callback functions
- // must be unlocked before calling any of the client class functions from an
- // other thread (MainNodeThread).
- // this->alg_.unlock();
- if(joint_trajectory_client_.isServerConnected())
- {
- //ROS_DEBUG("SmQrSearchAlgNode::joint_trajectoryMakeActionRequest: Server is Available!");
- //send a goal to the action server
- //joint_trajectory_goal_.data = my_desired_goal;
- joint_trajectory_client_.sendGoal(joint_trajectory_goal_,
- boost::bind(&SmQrSearchAlgNode::joint_trajectoryDone, this, _1, _2),
- boost::bind(&SmQrSearchAlgNode::joint_trajectoryActive, this),
- boost::bind(&SmQrSearchAlgNode::joint_trajectoryFeedback, this, _1));
- // this->alg_.lock();
- // ROS_DEBUG("SmQrSearchAlgNode::MakeActionRequest: Goal Sent.");
- this->executing_trajectory=true;
- return true;
- }
- else
- {
- // this->alg_.lock();
- ROS_DEBUG("SmQrSearchAlgNode::joint_trajectoryMakeActionRequest: HRI server is not connected");
- return false;
- }
-}
-*/
bool SmQrSearchAlgNode::head_follow_targetMakeActionRequest()
{
// IMPORTANT: Please note that all mutex used in the client callback functions
@@ -724,43 +563,97 @@ bool SmQrSearchAlgNode::head_follow_targetMakeActionRequest()
return false;
}
}
-
+*/
void SmQrSearchAlgNode::node_config_update(Config &config, uint32_t level)
{
this->alg_.lock();
-
+ unsigned int i;
+
if(config.start)
{
- ROS_INFO("START State Machine");
- new_goal=true;
- config.start=false;
+ if(!this->is_finished())
+ ROS_INFO("Search already executing");
+ else
+ {
+ ROS_INFO("START Search");
+ this->start_search(config.angle_rotation, config.error);
+ config.start=false;
+ }
}
if(config.stop)
{
- ROS_INFO("STOP State Machine");
- end=true;
+ ROS_INFO("STOP Search");
+ this->stop_search();
config.stop=false;
}
- if(config.load)
- {
- ROS_INFO("Load parameters");
- pan_angle_init=config.pan_angle_init;
- pan_angle_end=config.pan_angle_end;
- tilt_angle_search=config.tilt_angle_search;
- config.load=false;
- }
this->config_=config;
this->alg_.unlock();
}
+void SmQrSearchAlgNode::start_search(double angle_rotation, double error, const double head_speed, const double head_acceleration, const double yaw_step)
+{
+ this->rotate_angle = angle_rotation; //Angle of rotation
+ this->pan_angle_search = this->rotate_angle/2; //pan angle to search
+ this->tilt_angle_search = -this->pan_angle_search; //tilt angle to search
+ this->total_rotations = (PI/this->pan_angle_search)-1; //number of iterations
+ this->odom_error = error; //min error in rotation
+ ROS_INFO("Angle rotation: %f, Total rotations: %d, Pan angle: %f", rotate_angle, total_rotations, pan_angle_search);
+ for(int i=0;i<2;i++)
+ {
+ this->speed[i]=head_speed;
+ this->accel[i]=head_acceleration;
+ }
+ this->yaw_step=yaw_step; //Yaw amplitud to rotate
+ ROS_INFO("speed: %f, accel: %f", speed[0], accel[0]);
+
+
+ this->new_search=true;
+}
+
+void SmQrSearchAlgNode::stop_search(void)
+{
+ if(!this->state==SEARCH_MODULE_IDLE)
+ this->cancel_search=true;
+}
+
+bool SmQrSearchAlgNode::is_finished(void)
+{
+ if(this->state==SEARCH_MODULE_IDLE && this->new_search==false)
+ return true;
+ else
+ return false;
+}
+
+void SmQrSearchAlgNode::get_position(double &pan_pos, double &tilt_pos)
+{
+ if(this->search_success)
+ {
+ pan_pos=this->current_pan_angle;
+ tilt_pos=this->current_tilt_angle;
+ }
+}
+search_module_status_t SmQrSearchAlgNode::get_status(void)
+{
+ return this->status;
+}
+joints_module_status_t SmQrSearchAlgNode::get_joints_module_status(void)
+{
+ return this->joints_trajectory_module.get_status();
+}
+walk_module_status_t SmQrSearchAlgNode::get_walk_module_status(void)
+{
+ return this->walk_module.get_status();
+}
+
void SmQrSearchAlgNode::addNodeDiagnostics(void)
{
}
//-------------------------
+/*
bool SmQrSearchAlgNode::set_servo_module_head(std::string module)
{
this->set_servo_modules_srv_.request.names.resize(2);
@@ -798,71 +691,12 @@ bool SmQrSearchAlgNode::set_servo_module(std::string module)
}
-//Walk
-bool SmQrSearchAlgNode::set_walk_params(void)
-{
- set_walk_params_srv_.request.params.Y_SWAP_AMPLITUDE = config_.Y_SWAP_AMPLITUDE;
- set_walk_params_srv_.request.params.Z_SWAP_AMPLITUDE = config_.Z_SWAP_AMPLITUDE;
- set_walk_params_srv_.request.params.ARM_SWING_GAIN = config_.ARM_SWING_GAIN;
- set_walk_params_srv_.request.params.PELVIS_OFFSET = config_.PELVIS_OFFSET;
- set_walk_params_srv_.request.params.HIP_PITCH_OFFSET = config_.HIP_PITCH_OFFSET;
- set_walk_params_srv_.request.params.X_OFFSET = config_.X_OFFSET;
- set_walk_params_srv_.request.params.Y_OFFSET = config_.Y_OFFSET;
- set_walk_params_srv_.request.params.Z_OFFSET = config_.Z_OFFSET;
- set_walk_params_srv_.request.params.A_OFFSET = config_.A_OFFSET;
- set_walk_params_srv_.request.params.P_OFFSET = config_.P_OFFSET;
- set_walk_params_srv_.request.params.R_OFFSET = config_.R_OFFSET;
- set_walk_params_srv_.request.params.PERIOD_TIME = config_.PERIOD_TIME;
- set_walk_params_srv_.request.params.DSP_RATIO = config_.DSP_RATIO;
- set_walk_params_srv_.request.params.STEP_FB_RATIO = config_.STEP_FB_RATIO;
- set_walk_params_srv_.request.params.FOOT_HEIGHT = config_.FOOT_HEIGHT;
- set_walk_params_srv_.request.params.MAX_VEL = config_.MAX_VEL;
- set_walk_params_srv_.request.params.MAX_ROT_VEL = config_.MAX_ROT_VEL;
- ROS_INFO("SmQrSearchAlgNode:: Sending New Request!");
- if(set_walk_params_client_.call(set_walk_params_srv_))
- {
- if(set_walk_params_srv_.response.ret)
- ROS_INFO("SmQrSearchAlgNode:: Parameters changed successfully");
- else
- ROS_INFO("SmQrSearchAlgNode:: Impossible to change parameters");
- }
- else
- ROS_INFO("SmQrSearchAlgNode:: Failed to Call Server on topic set_walk_params ");
-
-}
-
-
-void SmQrSearchAlgNode::send_cmd_vel(double x_amplitude, double y_amplitude, double a_amplitude)
-{
- /* this->cmd_vel_Twist_msg_.linear.x=x_amplitude/period_time;
- this->cmd_vel_Twist_msg_.linear.y=y_amplitude/period_time;
- this->cmd_vel_Twist_msg_.linear.z=0;
- this->cmd_vel_Twist_msg_.angular.x=0;
- this->cmd_vel_Twist_msg_.angular.y=0;
- this->cmd_vel_Twist_msg_.angular.z=a_amplitude/period_time;
-*/
- this->cmd_vel_Twist_msg_.linear.x=x_amplitude;
- this->cmd_vel_Twist_msg_.linear.y=y_amplitude;
- this->cmd_vel_Twist_msg_.linear.z=0;
- this->cmd_vel_Twist_msg_.angular.x=0;
- this->cmd_vel_Twist_msg_.angular.y=0;
- this->cmd_vel_Twist_msg_.angular.z=a_amplitude;
-
- this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
-}
-
//Head Tracking
void SmQrSearchAlgNode::set_head_tracking_goal(double pan_target, double tilt_target)
{
this->head_follow_target_goal_.target_pan=pan_target;
this->head_follow_target_goal_.target_tilt=tilt_target;
// this->head_follow_target_goal_.pan_range.resize(2);
- /* this->head_follow_target_goal_.pan_range[0]=this->config_.max_pan;
- this->head_follow_target_goal_.pan_range[1]=this->config_.min_pan;
- this->head_follow_target_goal_.tilt_range.resize(2);
- this->head_follow_target_goal_.tilt_range[0]=this->config_.max_tilt;
- this->head_follow_target_goal_.tilt_range[1]=this->config_.min_tilt;
- */
this->head_follow_target_goal_.pan_range[0]=1.5;
this->head_follow_target_goal_.pan_range[1]=-1.5;
//this->head_follow_target_goal_.tilt_range.resize(2);
@@ -877,61 +711,7 @@ void SmQrSearchAlgNode::send_head_target(double pan, double tilt)
this->head_target_JointTrajectoryPoint_msg_.positions[1]=tilt;
this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
}
-
-//Joint trajectory
-//void SmQrSearchAlgNode::joint_trajectory_set_goal(double pan_position, double tilt_position)
-//{
- /* this->move_joints_goal_.trajectory.header.seq=0;
- this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
- this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
-
- this->move_joints_goal_.trajectory.joint_names.resize(2);
- this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
- this->move_joints_goal_.trajectory.joint_names[1]="j_tilt";
-
- this->move_joints_goal_.trajectory.points.resize(1);
- this->move_joints_goal_.trajectory.points[0].positions.resize(2);
- this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
- this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
-
- this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_state.position[18]*180.0/3.14159;
- this->move_joints_goal_.trajectory.points[0].positions[1]=60.0-15.0*this->n;
- */
-/*
- joint_trajectory_goal_.trajectory.joint_names[0]="j_pan";
- joint_trajectory_goal_.trajectory.joint_names[1]="j_tilt";
- joint_trajectory_goal_.trajectory.points[0].positions[0]=pan_position;
- joint_trajectory_goal_.trajectory.points[0].positions[1]=tilt_position;
- joint_trajectory_goal_.trajectory.points[0].velocities[0]=config_.speed;
- joint_trajectory_goal_.trajectory.points[0].velocities[1]=config_.speed;
- joint_trajectory_goal_.trajectory.points[0].accelerations[0]=config_.acceleration;
- joint_trajectory_goal_.trajectory.points[0].accelerations[1]=config_.acceleration;
- //ROS_INFO("Set Joint Trajectory GOAL");
-
-}
*/
-
-// bool SmQrSearchAlgNode::joint_trajectory_start_action(void)
-// {
-// if(this->joint_trajectoryMakeActionRequest())
-// {
-// /* joint_trajectory_goal_.trajectory.joint_names.clear();
-// joint_trajectory_goal_.trajectory.points[0].positions.clear();
-// joint_trajectory_goal_.trajectory.points[0].velocities.clear();
-// joint_trajectory_goal_.trajectory.points[0].accelerations.clear();
-// */
-// ROS_INFO("Sent joint trajectory action request");
-// return true;
-// }
-// else
-// {
-// ROS_INFO("Impossible to make Joint Trajectory action request!");
-// return false;
-// }
-//
-// }
-
-
/* main function */
int main(int argc,char *argv[])
{
diff --git a/track_charge_station/CMakeLists.txt b/track_charge_station/CMakeLists.txt
index 00b6ffe839aae4e108a6d4fa9d35fe1da8a4ba49..7bad06901f82d5c53383cd43f5b869bb9d29b930 100644
--- a/track_charge_station/CMakeLists.txt
+++ b/track_charge_station/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs iri_ros_tools)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs iri_ros_tools tf humanoid_modules)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -60,7 +60,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs iri_ros_tools
+ CATKIN_DEPENDS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs iri_ros_tools tf humanoid_modules
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -76,7 +76,7 @@ catkin_package(
# ********************************************************************
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
-include_directories(${iriutils_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
# include_directories(${<dependency>_INCLUDE_DIR})
## Declare a cpp library
diff --git a/track_charge_station/cfg/TrackChargeStation.cfg b/track_charge_station/cfg/TrackChargeStation.cfg
index 3f7f1e0a84c321f2bd2b20d03687512c66c737b2..cbd6bf253b24618a7f93d1792ef2c8d155fc750d 100755
--- a/track_charge_station/cfg/TrackChargeStation.cfg
+++ b/track_charge_station/cfg/TrackChargeStation.cfg
@@ -39,17 +39,22 @@ gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("start_head", bool_t, 0, "Start state machine", False)
gen.add("start", bool_t, 0, "Start state machine", False)
gen.add("stop", bool_t, 0, "Stop state machine", False)
gen.add("load", bool_t, 0, "Load pid parameters", False)
-gen.add("z_target", double_t, 0, "Z target distance to charge station", 20, 10, 30)
-gen.add("x_target", double_t, 0, "X target distance to charge station", 20, 10, 30)
-gen.add("yaw_target", double_t, 0, "Orientation target to charge station", 20, 10, 30)
-gen.add("z_kp", double_t, 0, "Desired kp for z distance", 0.1, 0.0, 0.28)
+gen.add("pan_angle", double_t, 0, "Z target distance to charge station", 1, -3, 3)
+gen.add("tilt_angle", double_t, 0, "Z target distance to charge station",0.8, -3, 3)
+gen.add("z_target", double_t, 0, "Z target distance to charge station", 0.5, 0.0, 2.0)
+gen.add("x_target", double_t, 0, "X target distance to charge station", 0.0, 0.0, 2.0)
+gen.add("yaw_target", double_t, 0, "Orientation target to charge station", 0.0, 0, 100)
+gen.add("z_tolerance", double_t, 0, "Tolerance in distance z", 0.2, 0.0, 5)
+gen.add("x_tolerance", double_t, 0, "Tolerance in distance z", 0.2, 0.0, 5)
+gen.add("yaw_tolerance", double_t, 0, "Tolerance in yaw", 0.1, -10, 10)
+gen.add("z_kp", double_t, 0, "Desired kp for z distance", 0.05, 0.0, 0.28)
gen.add("z_ki", double_t, 0, "Desired ki", 0.0, 0.0, 0.28)
gen.add("z_kd", double_t, 0, "Desired kd", 0.0, 0.0, 0.28)
-gen.add("period", double_t, 0, "Period PID", 1.0, 0.0, 100)
-gen.add("z_tolerance", double_t, 0, "Tolerance in distance z", 1.0, 0.0, 10)
+gen.add("yaw_kp", double_t, 0, "Desired kp for z distance", 0.1, -10, 10)
#Walk
gen.add("Y_SWAP_AMPLITUDE", double_t, 0, "Y swap amplitude (m)", 0.025, 0, 0.050)
gen.add("Z_SWAP_AMPLITUDE", double_t, 0, "X swap amplitude (m)", 0.005, 0, 0.050)
diff --git a/track_charge_station/include/track_charge_station_alg_node.h b/track_charge_station/include/track_charge_station_alg_node.h
index 7db52a1109280b3f74212c50b74f0aee9043cb25..36ed109869dbb477a608573f31f49c829ddde423 100644
--- a/track_charge_station/include/track_charge_station_alg_node.h
+++ b/track_charge_station/include/track_charge_station_alg_node.h
@@ -25,6 +25,7 @@
#ifndef _track_charge_station_alg_node_h_
#define _track_charge_station_alg_node_h_
+#include <tf/transform_listener.h>
#include <iri_ros_tools/watchdog.h>
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "track_charge_station_alg.h"
@@ -32,22 +33,30 @@
// [publisher subscriber headers]
#include <sensor_msgs/JointState.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
-#include <geometry_msgs/Twist.h>
+//#include <geometry_msgs/Twist.h>
#include <humanoid_common_msgs/tag_pose_array.h>
// [service client headers]
-#include <humanoid_common_msgs/set_servo_modules.h>
-#include <humanoid_common_msgs/set_pid.h>
-#include <humanoid_common_msgs/set_walk_params.h>
+//#include <humanoid_common_msgs/set_servo_modules.h>
+//#include <humanoid_common_msgs/set_pid.h>
+//#include <humanoid_common_msgs/set_walk_params.h>
// [action server client headers]
-#include <actionlib/client/simple_action_client.h>
-#include <actionlib/client/terminal_state.h>
-#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
+//#include <actionlib/client/simple_action_client.h>
+//#include <actionlib/client/terminal_state.h>
+//#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
+
+#include <humanoid_modules/walk_module.h>
+#include <humanoid_modules/head_tracking_module.h>
//states
-typedef enum {IDLE, QR, WALK, SIT} States;
+typedef enum {TRACK_MODULE_IDLE,
+ TRACK_MODULE_QR,
+ TRACK_MODULE_WALK,
+ TRACK_MODULE_SIT} track_module_states_t;
+
+typedef enum {TRACK_MODULE_SUCCESS, TRACK_MODULE_ERROR} track_module_status_t;
//PID
typedef struct {double kp;
@@ -57,8 +66,10 @@ typedef struct {double kp;
double prev_error;
double integral;
double derivative;
- double target;
- double period;
+ double qr_position;
+ double distance_target;
+ double tolerance;
+ double pid_out;
}TPID;
/**
@@ -69,11 +80,11 @@ class TrackChargeStationAlgNode : public algorithm_base::IriBaseAlgorithm<TrackC
{
private:
// [publisher attributes]
- ros::Publisher head_target_publisher_;
- trajectory_msgs::JointTrajectoryPoint head_target_JointTrajectoryPoint_msg_;
+ // ros::Publisher head_target_publisher_;
+ // trajectory_msgs::JointTrajectoryPoint head_target_JointTrajectoryPoint_msg_;
- ros::Publisher cmd_vel_publisher_;
- geometry_msgs::Twist cmd_vel_Twist_msg_;
+ // ros::Publisher cmd_vel_publisher_;
+ // geometry_msgs::Twist cmd_vel_Twist_msg_;
// [subscriber attributes]
@@ -93,30 +104,31 @@ class TrackChargeStationAlgNode : public algorithm_base::IriBaseAlgorithm<TrackC
// [service attributes]
// [client attributes]
- ros::ServiceClient set_servo_modules_client_;
- humanoid_common_msgs::set_servo_modules set_servo_modules_srv_;
+ // ros::ServiceClient set_servo_modules_client_;
+ // humanoid_common_msgs::set_servo_modules set_servo_modules_srv_;
- ros::ServiceClient set_pan_pid_client_;
- humanoid_common_msgs::set_pid set_pan_pid_srv_;
+ // ros::ServiceClient set_pan_pid_client_;
+ // humanoid_common_msgs::set_pid set_pan_pid_srv_;
- ros::ServiceClient set_walk_params_client_;
- humanoid_common_msgs::set_walk_params set_walk_params_srv_;
+ // ros::ServiceClient set_walk_params_client_;
+ // humanoid_common_msgs::set_walk_params set_walk_params_srv_;
// [action server attributes]
// [action client attributes]
- actionlib::SimpleActionClient<humanoid_common_msgs::humanoid_follow_targetAction> head_follow_target_client_;
+ /* actionlib::SimpleActionClient<humanoid_common_msgs::humanoid_follow_targetAction> head_follow_target_client_;
humanoid_common_msgs::humanoid_follow_targetGoal head_follow_target_goal_;
bool head_follow_targetMakeActionRequest();
void head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result);
void head_follow_targetActive();
void head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback);
+*/
-
- States state;
- bool new_goal;
- bool end;
+ track_module_states_t state;
+ track_module_status_t status;
+ bool new_track;
+ bool cancel_track;
bool executing_trajectory;
//Head tracking
double current_pan_angle;
@@ -131,20 +143,14 @@ class TrackChargeStationAlgNode : public algorithm_base::IriBaseAlgorithm<TrackC
double tilt_angle;
bool tracking;
bool qr_detected;
- unsigned int counter;
- double qr_position_z;
- double qr_position_x;
- double qr_position_y;
CROSWatchdog watchdog_qr_lost;
//PID
TPID z_distance;
TPID x_distance;
- TPID orientation;
- double pid;
- double pid_out;
- double z_distance_target;
+ TPID orientation;
ros::Time start_time;
ros::Time new_time;
+ // double pid_out;
//ros::Duration time_period;
double time_period;
//walk
@@ -157,7 +163,12 @@ class TrackChargeStationAlgNode : public algorithm_base::IriBaseAlgorithm<TrackC
double rot_error;
double z_min_tolerance;
+ //WALK MODULE
+ CWalkModule walk_module;
+ //HEAD TRACKING MODULE
+ CHeadTrackingModule head_tracking_module;
+ tf::TransformListener tf_listener;
/**
@@ -236,9 +247,10 @@ class TrackChargeStationAlgNode : public algorithm_base::IriBaseAlgorithm<TrackC
void send_cmd_vel(double x_amplitude, double y_amplitude, double a_amplitude);
void stop_walking(void);
//PID
- // void pid(void);
- // double pid(void);
- void pid2(TPID caca, double &culo);
+ void pid(TPID &axis, double max);
+ void pid_yaw(TPID &axis);
+
+ void tf_transform(void);
};
#endif
diff --git a/track_charge_station/launch/sim_track_charge_station.launch b/track_charge_station/launch/sim_track_charge_station.launch
deleted file mode 100644
index 34c2c0c6ce5123f801976cfb666e6e6cc80c1bbc..0000000000000000000000000000000000000000
--- a/track_charge_station/launch/sim_track_charge_station.launch
+++ /dev/null
@@ -1,70 +0,0 @@
-
-<launch>
-
- <arg name="robot" default="darwin" />
- <arg name="environment" default="vision_env" />
-
- <include file="$(find darwin_description)/launch/darwin_sim.launch">
- <arg name="robot" value="$(arg robot)" />
- </include>
-
- <include file="$(find bioloid_description)/launch/vision_env.launch">
- <arg name="environment" value="$(arg environment)" />
- </include>
-
-
- <!-- launch the QR head tracking node -->
- <node name="track_charge_station"
- pkg="track_charge_station"
- type="track_charge_station"
- output="screen"
- ns="/darwin">
- <remap from="/darwin/track_charge_station/cmd_vel"
- to="/darwin/robot/cmd_vel"/>
- <remap from="/darwin/track_charge_station/set_pan_pid"
- to="/darwin/robot/set_pan_pid"/>
- <remap from="/darwin/track_charge_station/set_walk_params"
- to="/darwin/robot/set_walk_params"/>
- <remap from="/darwin/track_charge_station/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/>
- <remap from="/darwin/head_follow_target"
- to="/darwin/robot/head_follow_target"/>
- <remap from="/darwin/track_charge_station/head_target"
- to="/darwin/robot/head_target"/>
- <remap from="/darwin/track_charge_station/joint_states"
- to="/darwin/joint_states"/>
- <!-- <remap from="/darwin/track_charge_station/qr_pose"
- to="/qr_detector/qr_pose"/> -->
- </node>
-
-<!-- <include file="$(find qr_detector)/launch/tracker_darwin.launch"/> -->
-<<<
- <node pkg="qr_detector"
- name="qr_detector"
- type="qr_detector"
- output="screen"
- ns="/darwin">
- <param name="qr_x" value="0.12"/>
- <param name="qr_y" value="0.12"/>
- <param name="camera_frame" value="/darwin/camera_link"/>
- <remap from="/darwin/qr_detector/camera/image_raw"
- to="/darwin/camera/image_raw"/>
- <remap from="/darwin/qr_detector/camera/camera_info"
- to="/darwin/camera/camera_info"/>
- <remap from="/darwin/qr_detector/qr_pose"
- to="/darwin/track_charge_station/qr_pose"/>
- </node>
-
-
- <node pkg="image_view"
- name="image_view"
- type="image_view"
- args="image:=/darwin/usb_cam/image_raw">
- </node>
-
- <!-- launch dynamic reconfigure -->
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
- output="screen"/>
-
-
-</launch>
\ No newline at end of file
diff --git a/track_charge_station/launch/track_charge_station_sim.launch b/track_charge_station/launch/track_charge_station_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..bca3caf460bc194fa7e13d6b907e82ff29dd7e18
--- /dev/null
+++ b/track_charge_station/launch/track_charge_station_sim.launch
@@ -0,0 +1,81 @@
+
+<launch>
+
+ <arg name="robot" default="darwin" />
+ <arg name="environment" default="charge_env" />
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <include file="$(find darwin_description)/launch/charge_env.launch">
+ <arg name="environment" value="$(arg environment)" />
+ </include>
+
+
+ <!-- launch the QR head tracking node -->
+ <node name="track_charge_station"
+ pkg="track_charge_station"
+ type="track_charge_station"
+ output="screen"
+ ns="/darwin">
+ <remap from="/darwin/track_charge_station/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/track_charge_station/qr_pose"
+ to="/darwin/qr_detector/qr_pose"/>
+ <!--Walk module -->
+ <remap from="/darwin/track_charge_station/walk/cmd_vel"
+ to="/darwin/robot/cmd_vel"/>
+ <remap from="/darwin/track_charge_station/walk/set_walk_params"
+ to="/darwin/robot/set_walk_params"/>
+ <remap from="/darwin/track_charge_station/walk/get_walk_params"
+ to="/darwin/robot/get_walk_params"/>
+ <remap from="/darwin/track_charge_station/walk/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/track_charge_station/walk/joint_states"
+ to="/darwin/joint_states"/>
+ <!--Head tracking module -->
+ <remap from="/darwin/track_charge_station/head_tracking/head_tracking_action"
+ to="/darwin/robot/head_follow_target"/>
+ <remap from="/darwin/track_charge_station/head_tracking/head_target"
+ to="/darwin/robot/head_target"/>
+ <remap from="/darwin/track_charge_station/head_tracking/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/track_charge_station/head_tracking/set_pan_pid"
+ to="/darwin/robot/set_pan_pid"/>
+ <remap from="/darwin/track_charge_station/head_tracking/get_pan_pid"
+ to="/darwin/robot/get_pan_pid"/>
+ <remap from="/darwin/track_charge_station/head_tracking/set_tilt_pid"
+ to="/darwin/robot/set_tilt_pid"/>
+ <remap from="/darwin/track_charge_station/head_tracking/get_tilt_pid"
+ to="/darwin/robot/get_tilt_pid"/>
+
+ </node>
+
+<!-- <include file="$(find qr_detector)/launch/tracker_darwin.launch"/> -->
+
+ <node pkg="qr_detector"
+ name="qr_detector"
+ type="qr_detector"
+ output="screen"
+ ns="/darwin">
+ <param name="qr_x" value="0.14"/>
+ <param name="qr_y" value="0.14"/>
+ <param name="camera_frame" value="/darwin/camera_link"/>d
+ <remap from="~/camera/image_raw" to="/darwin/camera/image_raw"/>
+ <remap from="~/camera/camera_info" to="/darwin/camera/camera_info"/>
+ </node>
+
+
+ <node pkg="image_view"
+ name="image_view"
+ type="image_view"
+ args="image:=/darwin/camera/image_raw">
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+
+</launch>
\ No newline at end of file
diff --git a/track_charge_station/package.xml b/track_charge_station/package.xml
index 170d809c8e74bad4b03e1dcfa260b3ca4dfa20ee..9079abf2227d169a523f4bcdccde8a0581335b05 100644
--- a/track_charge_station/package.xml
+++ b/track_charge_station/package.xml
@@ -47,6 +47,8 @@
<build_depend>geometry_msgs</build_depend>
<build_depend>humanoid_common_msgs</build_depend>
<build_depend>iri_ros_tools</build_depend>
+ <build_depend>humanoid_modules</build_depend>
+ <build_depend>tf</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
@@ -54,6 +56,8 @@
<run_depend>geometry_msgs</run_depend>
<run_depend>humanoid_common_msgs</run_depend>
<run_depend>iri_ros_tools</run_depend>
+ <run_depend>tf</run_depend>
+ <run_depend>humanoid_modules</run_depend>
<!-- The export tag contains other, unspecified, tags -->
diff --git a/track_charge_station/src/track_charge_station_alg_node.cpp b/track_charge_station/src/track_charge_station_alg_node.cpp
index ac68f8ee3ad8fd101ff1c6361090f2b924e50cc2..d9c6eb6b1ea92a9699743bff5b027b6b259ba6c5 100644
--- a/track_charge_station/src/track_charge_station_alg_node.cpp
+++ b/track_charge_station/src/track_charge_station_alg_node.cpp
@@ -1,50 +1,28 @@
+
+/**
+ * Ideal Z distance 0.3 (30cm) with 0.05 tolerance (5cm) - max step 0.02
+ * Ideal X distance 0cm with 0.2 or 0.1 tolerance (10cm) - max step 0.01
+ * Ideal yaw orientation 0 with 0.1 tolerance - max step 0.3 rad
+ *
+ * Pending to adjust wathdog qr lost
+ *
+ */
+
+
#include "track_charge_station_alg_node.h"
-#define NUM_SERVOS 32
-// joint names
-const std::string servo_names[NUM_SERVOS]={std::string("Servo0"),
- std::string("j_shoulder_pitch_r"),
- std::string("j_shoulder_pitch_l"),
- std::string("j_shoulder_roll_r"),
- std::string("j_shoulder_roll_l"),
- std::string("j_elbow_r"),
- std::string("j_elbow_l"),
- std::string("j_hip_yaw_r"),
- std::string("j_hip_yaw_l"),
- std::string("j_hip_roll_r"),
- std::string("j_hip_roll_l"),
- std::string("j_hip_pitch_r"),
- std::string("j_hip_pitch_l"),
- std::string("j_knee_r"),
- std::string("j_knee_l"),
- std::string("j_ankle_pitch_r"),
- std::string("j_ankle_pitch_l"),
- std::string("j_ankle_roll_r"),
- std::string("j_ankle_roll_l"),
- std::string("j_pan"),
- std::string("j_tilt"),
- std::string("Servo21"),
- std::string("Servo22"),
- std::string("Servo23"),
- std::string("Servo24"),
- std::string("Servo25"),
- std::string("Servo26"),
- std::string("Servo27"),
- std::string("Servo28"),
- std::string("Servo29"),
- std::string("Servo30"),
- std::string("Servo31")};
-
TrackChargeStationAlgNode::TrackChargeStationAlgNode(void) :
algorithm_base::IriBaseAlgorithm<TrackChargeStationAlgorithm>(),
- head_follow_target_client_("head_follow_target", true)
+ //head_follow_target_client_("head_follow_target", true),
+ head_tracking_module("head_tracking"),
+ walk_module("walk")
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
- this->head_target_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("head_target", 1);
- this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+// this->head_target_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("head_target", 1);
+// this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
// [init subscribers]
this->joint_states_subscriber_ = this->public_node_handle_.subscribe("joint_states", 1, &TrackChargeStationAlgNode::joint_states_callback, this);
@@ -57,50 +35,49 @@ TrackChargeStationAlgNode::TrackChargeStationAlgNode(void) :
// [init services]
// [init clients]
- set_servo_modules_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_servo_modules>("set_servo_modules");
+// set_servo_modules_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_servo_modules>("set_servo_modules");
- set_pan_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_pid>("set_pan_pid");
+// set_pan_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_pid>("set_pan_pid");
- set_walk_params_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_walk_params>("set_walk_params");
+ // set_walk_params_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_walk_params>("set_walk_params");
// [init action servers]
// [init action clients]
- state=IDLE;
- new_goal=false;
+ state=TRACK_MODULE_IDLE;
+ new_track=false;
//Head tracking
current_pan_angle=0.0;
current_tilt_angle=0.0;
//QR
- counter=0;
+ qr_detected=false;
+ this->tracking=false;
//PID
- pid=0.0;
- pid_out=0.0;
- z_distance.error=0.0;
- z_distance.prev_error=0.0;
- z_distance.kp=0.0;
+ z_distance.kp=0.1;
z_distance.ki=0.0;
z_distance.kd=0.0;
- z_distance.period=0.0;
- //walk
- cmd_vel_x=0.0;
- cmd_vel_y=0.0;
- cmd_vel_a=0.0;
- z_error=0.0;
- x_error=0.0;
- rot_error=0.0;
-
-
+ z_distance.qr_position=0.0;
+ z_distance.distance_target=0.0;
+ z_distance.prev_error=0.0;
+ z_distance.tolerance=0.0;
+ x_distance.kp=0.1;
+ x_distance.ki=0.0;
+ x_distance.kd=0.0;
+ x_distance.qr_position=0.0;
+ x_distance.distance_target=0.0;
+ x_distance.prev_error=0.0;
+ x_distance.tolerance=0.0;
+ orientation.kp=0.1;
+ orientation.ki=0.0;
+ orientation.kd=0.0;
+ orientation.qr_position=0.0;
+ orientation.distance_target=0.0;
+ orientation.prev_error=0.0;
+ orientation.tolerance=0.0010;
- //head_follow
- this->head_follow_target_goal_.pan_range.resize(2);
- this->head_follow_target_goal_.tilt_range.resize(2);
-
- //head_target
- this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
- //this->head_target_JointTrajectoryPoint_msg_.velocities.resize(2);
+
}
TrackChargeStationAlgNode::~TrackChargeStationAlgNode(void)
@@ -186,49 +163,58 @@ void TrackChargeStationAlgNode::mainNodeThread(void)
//this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
// Uncomment the following line to publish the topic message
- //this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
*/
- switch(state)
+ if(this->cancel_track)
+ {
+ this->cancel_track=false;
+ this->state=TRACK_MODULE_IDLE;
+ head_tracking_module.stop_tracking();
+ walk_module.stop();
+ }
+ else
+ {
+ switch(this->state)
{
- case IDLE:
+ case TRACK_MODULE_IDLE:
ROS_INFO("IDLE");
- if(new_goal)
+ // this->tf_transform();
+ if(this->new_track)
{
//Start tracking QR
this->tracking=true;
- set_servo_module("walk"); //Servos to walk module
- //set_walk_params();
- set_servo_module_head("head"); //Head servos to head module
- set_head_tracking_goal(this->current_pan_angle, this->current_tilt_angle);
- if(head_follow_targetMakeActionRequest())
- {
- state=QR;
- }
- new_goal=false;
+ // head_tracking_module.start_tracking(pan_angle, tilt_angle);
+ this->state=TRACK_MODULE_QR;
+ // state=WAIT;
+ new_track=false;
//state=START;
+ this->start_time=ros::Time::now();
}
break;
- /* case START:
- ROS_INFO("START");
- pid();
- if(end)
- state=IDLE;
-
- break; */
+ /* case WAIT:
+ if(head_tracking_module.get_state()==HEAD_TRACKING_MODULE_WAIT)
+ state=TRACK_MODULE_QR;
+ else
+ state=WAIT;
+ break;
+ */
//Comprobar si se ha perdido QR y actualizar pid (con señal bool)
- case QR:
- // if(counter>50)
- if(this->watchdog_qr_lost.is_active())
+ case TRACK_MODULE_QR:
+
+ if(!qr_detected)
{
- ROS_INFO("QR lost");
+ if(this->watchdog_qr_lost.is_active())
+ {
+ ROS_INFO("QR LOST");
+ walk_module.stop(); //stop walking
this->tracking=false;
- this->head_follow_target_client_.cancelGoal();
+ head_tracking_module.stop_tracking();
//Volver a la maquina de estados SmQrSearchAlgNode
- state=IDLE;
+ this->state=TRACK_MODULE_IDLE;
+ }
}
/* else if(end){
ROS_INFO("Stop State machine");
@@ -240,14 +226,13 @@ void TrackChargeStationAlgNode::mainNodeThread(void)
else
{
ROS_INFO("TRACKING");
- //counter=0;
//pid();
//send_cmd_vel(pid_out);
- state=WALK;
+ this->state=TRACK_MODULE_WALK;
}
break;
- case WALK:
+ /* case TRACK_MODULE_WALK:
ROS_INFO("Walking z");
//Distancia z estacion carga
// pid(z_distance, &z_error, &z_cmd_vel); //Actualizar PID
@@ -267,28 +252,186 @@ void TrackChargeStationAlgNode::mainNodeThread(void)
// }
- pid2(z_distance, cmd_vel_y);
+ pid2(z_distance, cmd_vel_y);
if(z_distance.error<z_min_tolerance) //si la distancia z (error) es menor a la minima distancia admitida (tolerancia)
{
cmd_vel_y=0.0; //No mover eje y
- state=SIT;
+ this->state=TRACK_MODULE_SIT;
}
else
{
- state=QR;
+ this->state=TRACK_MODULE_QR;
}
send_cmd_vel(0, cmd_vel_y, 0);
break;
+ */
- case SIT:
- ROS_INFO("Sitting");
- state=IDLE;
+
+ //Z
+ /*
+ case TRACK_MODULE_WALK:
+ ROS_INFO("Walking z");
+ // pid2(z_distance, cmd_vel_y); //calcula step
+ pid(z_distance);
+ ROS_INFO("Error: %f, tolerance: %f", z_distance.error, this->z_distance.tolerance);
+ ROS_INFO("PID: %f", z_distance.pid_out);
+ if(this->z_distance.error < this->z_distance.tolerance) //si la distancia z (error) es menor a la minima distancia admitida (tolerancia)
+ {
+ ROS_INFO("Stop walking");
+ // walk_module.set_steps_size(0.0,0.0,0.0);
+ z_distance.pid_out=0.0;
+ walk_module.stop(); //parar movimiento de ese eje
+ this->state=TRACK_MODULE_SIT;
+ }
+ else
+ {
+ this->state=TRACK_MODULE_QR;
+ }
+ walk_module.add_whole_body();
+ walk_module.set_steps_size(z_distance.pid_out,0.0,0.0);
break;
+*/
+
+ //X
+ /*
+ case TRACK_MODULE_WALK:
+ ROS_INFO("Walking x");
+ pid(x_distance);
+ ROS_INFO("pos, target: %f, %f", x_distance.qr_position, x_distance.distance_target);
+ ROS_INFO("Error: %f", x_distance.error);
+ ROS_INFO("PID: %f", x_distance.pid_out);
+ if(fabs(this->x_distance.error) < this->x_distance.tolerance) //si la distancia z (error) es menor a la minima distancia admitida (tolerancia)
+ {
+ ROS_INFO("Stop walking");
+ // walk_module.set_steps_size(0.0,0.0,0.0);
+ x_distance.pid_out=0.0;
+ walk_module.stop(); //parar movimiento de ese eje
+ this->state=TRACK_MODULE_SIT;
+ }
+ else
+ {
+ this->state=TRACK_MODULE_QR;
+ }
+ walk_module.add_whole_body();
+ walk_module.set_steps_size(0.0,x_distance.pid_out,0.0);
+ break;
+ */
+ //YAW
+ /*
+ case TRACK_MODULE_WALK:
+ ROS_INFO("walking way");
+ //pid_yaw(orientation);
+ pid(orientation);
+ ROS_INFO("Pos: %f, Target: %f", orientation.qr_position, orientation.distance_target);
+ ROS_INFO("Error: %f", orientation.error);
+ ROS_INFO("PID: %f", orientation.pid_out);
+ if(fabs(this->orientation.error) < this->orientation.tolerance) //si la distancia z (error) es menor a la minima distancia admitida (tolerancia)
+ {
+ ROS_INFO("Stop walking");
+ // walk_module.set_steps_size(0.0,0.0,0.0);
+ orientation.pid_out=0.0;
+ walk_module.stop(); //parar movimiento de ese eje
+ this->state=TRACK_MODULE_SIT;
+ }
+ else
+ {
+ this->state=TRACK_MODULE_QR;
+ }
+ // walk_module.add_whole_body();
+ walk_module.set_steps_size(0.0,0.0,orientation.pid_out);
+ break;
+ */
+
+
+ //TODOS
+ case TRACK_MODULE_WALK:
+ // this->tf_transform();
+ ROS_INFO("WALKING");
+ pid(z_distance, 0.02);
+ pid(orientation,0.3);
+ pid(x_distance, 0.005);
+ ROS_INFO("Error YAW: %f, PID: %f", orientation.error, orientation.pid_out);
+ ROS_INFO("Error Z: %f, PID: %f", z_distance.error, z_distance.pid_out);
+ ROS_INFO("Error X: %f, PID: %f", x_distance.error, x_distance.pid_out);
+ if(fabs(this->orientation.error) < this->orientation.tolerance) //si la distancia z (error) es menor a la minima distancia admitida (tolerancia)
+ {
+ ROS_INFO("Stop rotating");
+ // walk_module.set_steps_size(0.0,0.0,0.0);
+ orientation.pid_out=0.0;
+ // walk_module.stop(); //parar movimiento de ese eje
+ // this->state=TRACK_MODULE_SIT;
+ }
+ if(this->z_distance.error<this->z_distance.tolerance)
+ {
+ ROS_INFO("Stop walking Z");
+ z_distance.pid_out=0.0;
+ }
+ if(this->x_distance.error<this->x_distance.tolerance)
+ {
+ ROS_INFO("Stop walking X");
+ x_distance.pid_out=0.0;
+ }
+ if(orientation.pid_out==0.0 && z_distance.pid_out==0.0 && x_distance.pid_out==0.0)
+ {
+ state=TRACK_MODULE_SIT;
+ walk_module.stop();
+
+ }
+ else
+ {
+ this->state=TRACK_MODULE_QR;
+ }
+ // walk_module.add_whole_body();
+ walk_module.set_steps_size(z_distance.pid_out,x_distance.pid_out,orientation.pid_out);
+ break;
+
+
+ case TRACK_MODULE_SIT:
+ ROS_INFO("Sitting");
+ head_tracking_module.stop_tracking();
+ this->state=TRACK_MODULE_IDLE;
+ break;
+
+ }
+ }
}
+
+
+void TrackChargeStationAlgNode::tf_transform(void)
+{
+ std::string target_frame="darwin/base_link";
+ std::string source_frame="darwin/head";
+ ros::Time target_time = ros::Time::now();
+ geometry_msgs::PoseStamped p;
+ tf::StampedTransform t;
+
+ bool tf_exists = tf_listener.waitForTransform(target_frame, source_frame, target_time, ros::Duration(0.5), ros::Duration(0.01));
+ if(tf_exists)
+ {
+ p.header.frame_id = source_frame;
+ p.header.stamp = target_time;
+ p.pose.position.x = 0.0;
+ p.pose.position.y = 0.0;
+ p.pose.position.z = 0.0;
+ p.pose.orientation = tf::createQuaternionMsgFromYaw(1.0);
+ tf_listener.transformPose(target_frame, p, p);
+ // tf_listener.lookupTransform(target_frame, source_frame, target_time, t);
+
+ double x=p.pose.position.x;
+ double y=p.pose.position.y;
+ double z=p.pose.position.z;
+ double yaw=tf::getYaw(p.pose.orientation);
+
+ ROS_INFO("X: %f", x);
+ ROS_INFO("Y: %f", y);
+ ROS_INFO("Y: %f", z);
+ ROS_INFO("YAW: %f", yaw);
+ }
}
+
/* [subscriber callbacks] */
void TrackChargeStationAlgNode::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
{
@@ -326,7 +469,7 @@ void TrackChargeStationAlgNode::joint_states_mutex_exit(void)
{
pthread_mutex_unlock(&this->joint_states_mutex_);
}
-
+/*
void TrackChargeStationAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
{
//ROS_INFO("TrackChargeStationAlgNode::qr_pose_callback: New Message Received");
@@ -335,27 +478,32 @@ void TrackChargeStationAlgNode::qr_pose_callback(const humanoid_common_msgs::tag
//this->alg_.lock();
this->qr_pose_mutex_enter();
-
if(msg->tags.size()>0)
{
//qr_detected=msg->tags.size();
qr_detected=true;
- qr_position_z=(msg->tags[0].position.z)*100;
- qr_position_x=(msg->tags[0].position.x)*100;
- qr_position_y=(msg->tags[0].position.y)*100;
+ z_distance.qr_position=(msg->tags[0].position.z)*100;
+ x_distance.qr_position=(msg->tags[0].position.x)*100;
+ //qr_position_y=(msg->tags[0].position.y)*100;
+ //ROS_INFO("qr posicion Z: %f", z_distance.qr_position);
+ // ROS_INFO("qr posicion X: %f", x_distance.qr_position);
- this->pan_angle=this->current_pan_angle+atan2(-msg->tags[0].position.x,msg->tags[0].position.z);
- this->tilt_angle=this->current_tilt_angle+atan2(-msg->tags[0].position.y,msg->tags[0].position.z);
- //ROS_INFO("Next target pan angle: %f (%f,%f)",this->pan_angle,this->current_pan_angle,atan2(msg->tags[0].position.x,msg->tags[0].position.z));
- //ROS_INFO("Next target tilt angle: %f (%f,%f)",this->tilt_angle,this->current_tilt_angle,atan2(msg->tags[0].position.y,msg->tags[0].position.z));
-
- if(this->tracking)
- {
- send_head_target(pan_angle, tilt_angle);
- }
- watchdog_qr_lost.reset(ros::Duration(5));
+ this->pan_angle=this->current_pan_angle+atan2(msg->tags[0].position.x,msg->tags[0].position.z);
+ this->tilt_angle=this->current_tilt_angle+atan2(msg->tags[0].position.y,msg->tags[0].position.z);
+ // ROS_INFO("Next target pan angle: %f (%f,%f)",this->pan_angle,this->current_pan_angle,atan2(msg->tags[0].position.x,msg->tags[0].position.z));
+ // ROS_INFO("Next target tilt angle: %f (%f,%f)",this->tilt_angle,this->current_tilt_angle,atan2(msg->tags[0].position.y,msg->tags[0].position.z));
+
+ //if(this->tracking)
+ //{
+ ROS_INFO("Send new head target!");
+ //send_head_target(pan_angle, tilt_angle);
+
+ head_tracking_module.update_target(pan_angle, tilt_angle);
+ //}
+ watchdog_qr_lost.reset(ros::Duration(1));
//counter=0;
}else{
+ ROS_INFO("Qr NOT detected");
qr_detected=false;
//counter++;
}
@@ -365,6 +513,91 @@ void TrackChargeStationAlgNode::qr_pose_callback(const humanoid_common_msgs::tag
//this->alg_.unlock();
this->qr_pose_mutex_exit();
}
+*/
+void TrackChargeStationAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
+{
+ this->qr_pose_mutex_enter();
+
+ if(msg->tags.size()>0)
+ {
+ qr_detected=true;
+
+ this->pan_angle=this->current_pan_angle+atan2(msg->tags[0].position.x,msg->tags[0].position.z);
+ this->tilt_angle=this->current_tilt_angle+atan2(msg->tags[0].position.y,msg->tags[0].position.z);
+ ROS_INFO("Send new head target!");
+ head_tracking_module.update_target(pan_angle, tilt_angle);
+
+
+
+ // z_distance.qr_position=(msg->tags[0].position.z)*100;
+ // x_distance.qr_position=(msg->tags[0].position.x)*100;
+ //qr_position_y=(msg->tags[0].position.y)*100;
+ // ROS_INFO("qr posicion Z: %f", z_distance.qr_position);
+ // ROS_INFO("qr posicion X: %f", x_distance.qr_position);
+
+ ROS_INFO("QR X: %f cm", (msg->tags[0].position.x)*100);
+ ROS_INFO("QR Y: %f cm", (msg->tags[0].position.y)*100);
+ ROS_INFO("QR Z: %f cm", (msg->tags[0].position.z)*100);
+ ROS_INFO("QR YAW: %f", tf::getYaw(msg->tags[0].orientation));
+
+ std::string target_frame="darwin/base_link";
+ std::string source_frame="darwin/head";
+ ros::Time target_time = ros::Time::now();
+ geometry_msgs::PoseStamped in;
+ geometry_msgs::PoseStamped out;
+ tf::Quaternion tf_quat;
+ tf::Matrix3x3 tf_rotation;
+ double roll, pitch, yaw;
+
+ bool tf_exists = tf_listener.waitForTransform(target_frame, source_frame, target_time, ros::Duration(0.5), ros::Duration(0.01));
+ if(tf_exists)
+ {
+ in.header.frame_id = source_frame;
+ in.header.stamp = target_time;
+ in.pose.position.x = msg->tags[0].position.x;
+ in.pose.position.y = msg->tags[0].position.y;
+ in.pose.position.z = msg->tags[0].position.z;
+ in.pose.orientation = msg->tags[0].orientation;
+// in.pose.orientation.y = msg->tags[0].orientation.y;
+// in.pose.orientation.z = msg->tags[0].orientation.z;
+// in.pose.orientation.w = msg->tags[0].orientation.w;
+ tf_listener.transformPose(target_frame, in, out);
+
+ x_distance.qr_position=out.pose.position.x;
+ // double y=out.pose.position.y;
+ z_distance.qr_position=out.pose.position.y;
+ double z=out.pose.position.z;
+ // orientation.qr_position=tf::getYaw(out.pose.orientation);
+
+ tf::quaternionMsgToTF(out.pose.orientation,tf_quat);
+ tf_rotation=tf::Matrix3x3(tf_quat);
+ tf_rotation.getRPY(roll,pitch,yaw);
+ orientation.qr_position=pitch;
+
+
+ ROS_INFO("X: %f cm", (x_distance.qr_position)*100); //Distancia X QR
+ ROS_INFO("Y: %f cm", (z_distance.qr_position)*100); //Distancia Z QR
+ // ROS_INFO("Z: %f", z*100);
+ ROS_INFO("YAW: %f cm", (orientation.qr_position)*100);
+// ROS_INFO("YAW_y: %f", (orientation.qr_position.y)*100);
+// ROS_INFO("YAW_z: %f", (orientation.qr_position.z)*100);
+// ROS_INFO("YAW_w: %f", (orientation.qr_position.w)*100);
+ // ROS_INFO("ROLL: %f", roll);
+ // ROS_INFO("PIRCH: %f", pitch);
+ // ROS_INFO("YAW: %f", roll);
+ }
+
+
+
+ watchdog_qr_lost.reset(ros::Duration(2));
+ }else{
+ ROS_INFO("Qr NOT detected");
+ qr_detected=false;
+ }
+
+
+ this->qr_pose_mutex_exit();
+}
void TrackChargeStationAlgNode::qr_pose_mutex_enter(void)
{
@@ -380,6 +613,7 @@ void TrackChargeStationAlgNode::qr_pose_mutex_exit(void)
/* [service callbacks] */
/* [action callbacks] */
+/*
void TrackChargeStationAlgNode::head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result)
{
alg_.lock();
@@ -417,9 +651,10 @@ void TrackChargeStationAlgNode::head_follow_targetFeedback(const humanoid_common
}
alg_.unlock();
}
-
+*/
/* [action requests] */
+/*
bool TrackChargeStationAlgNode::head_follow_targetMakeActionRequest()
{
// IMPORTANT: Please note that all mutex used in the client callback functions
@@ -446,7 +681,7 @@ bool TrackChargeStationAlgNode::head_follow_targetMakeActionRequest()
return false;
}
}
-
+*/
void TrackChargeStationAlgNode::node_config_update(Config &config, uint32_t level)
{
@@ -454,27 +689,42 @@ void TrackChargeStationAlgNode::node_config_update(Config &config, uint32_t leve
if(config.start) //Equivale a la funcion start_tracking
{
- z_distance_target=config.z_target;
- // x_distance_target=config.x_target;
- //orientation_target=config.yaw_target;
- z_min_tolerance=config.z_tolerance;
- //x_min_error=config.x_error;
- //rot_min_error=config.yaw_error;
- new_goal=true;
+ z_distance.distance_target=config.z_target;
+ x_distance.distance_target=config.x_target;
+ orientation.distance_target=config.yaw_target;
+ z_distance.tolerance=config.z_tolerance;
+ x_distance.tolerance=config.x_tolerance;
+ orientation.tolerance=config.yaw_tolerance;
+ new_track=true;
config.start=false;
}
+ if(config.start_head)
+ {
+ head_tracking_module.start_tracking(config.pan_angle, config.tilt_angle);
+ config.start_head=false;
+ }
if(config.load)
{
+ // head_tracking_module.start_tracking(config.pan_angle, config.tilt_angle);
+ head_tracking_module.update_target(config.pan_angle, config.tilt_angle);
+
+ // pan_angle=config.pan_angle;
+ // tilt_angle=config.tilt_angle;
z_distance.kp=config.z_kp;
z_distance.ki=config.z_ki;
z_distance.kd=config.z_kd;
- z_distance.period=config.period;
+ x_distance.kp=config.z_kp;
+ x_distance.ki=config.z_ki;
+ x_distance.kd=config.z_kd;
+ orientation.kp=config.yaw_kp;
+ orientation.ki=config.z_ki;
+ orientation.kd=config.z_kd;
config.load=false;
}
if(config.stop)
{
- end=true;
+ this->cancel_track=true;
config.stop=false;
}
@@ -494,80 +744,119 @@ void TrackChargeStationAlgNode::addNodeDiagnostics(void)
//Start tracking state machine
void TrackChargeStationAlgNode::start_tracking(double z_target, double x_target, double yaw_target)
{
- z_distance_target=z_target;
+ z_distance.distance_target=z_target;
// x_distance_target=x_target;
// orientation_target=yaw_target;
- new_goal=true; //Activar maquina de estados
+ new_track=true; //Activar maquina de estados
}
-bool TrackChargeStationAlgNode::set_servo_module_head(std::string module)
+void TrackChargeStationAlgNode::pid(TPID &axis, double max) //distancia z
{
- this->set_servo_modules_srv_.request.names.resize(2);
- this->set_servo_modules_srv_.request.modules.resize(2);
- this->set_servo_modules_srv_.request.names[0]="j_pan";
- this->set_servo_modules_srv_.request.modules[0]=module;
- this->set_servo_modules_srv_.request.names[1]="j_tilt";
- this->set_servo_modules_srv_.request.modules[1]=module;
- if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
- return true;
- else
- {
- ROS_DEBUG("Impossible to call set_servo_modules service");
- return false;
- }
+ double res;
+
+ //Period time
+ this->new_time=ros::Time::now();
+ time_period = (new_time-this->start_time).toSec();
+ // ROS_INFO("time period: %f", time_period);
+
+ //Calculate PID
+ axis.error = axis.qr_position-axis.distance_target;//axis.error=z_distance_target-qr_position_z;
+ // axis.derivative = (axis.error-axis.prev_error)/time_period;
+ //axis.integral = (axis.error*time_period); //+=
+ //res += axis.kp*axis.error + axis.ki*axis.integral + axis.kd*axis.derivative;
+ res = axis.kp*axis.error;
+ axis.prev_error=axis.error;
+// ROS_INFO("ERROR: %f", axis.error);
+ //ROS_INFO("Integral: %f, Derivada: %f", axis.integral, axis.derivative);
+ //ROS_INFO("qr X: %f, qr Y: %f, qr Z: %f", qr_position_x, qr_position_y, qr_position_z);
+
+ //dependiendo de pid --> modificar amplitude de cmd_vel
+ axis.pid_out = res;
+ // ROS_INFO("PID: %f", axis.pid_out);
+
+ if(axis.pid_out > max)//maximo cmd_vel
+ axis.pid_out = max;
+ else if(axis.pid_out < -max)
+ axis.pid_out = -max;
+
+
+
+ this->start_time=ros::Time::now();
}
-
-bool TrackChargeStationAlgNode::set_servo_module(std::string module)
+void TrackChargeStationAlgNode::pid_yaw(TPID &axis) //distancia z
{
- unsigned int i;
- this->set_servo_modules_srv_.request.names.resize(NUM_SERVOS);
- this->set_servo_modules_srv_.request.modules.resize(NUM_SERVOS);
- for(i=0;i<NUM_SERVOS;i++)
- {
- this->set_servo_modules_srv_.request.names[i]=servo_names[i];
- this->set_servo_modules_srv_.request.modules[i]=module;
- }
- if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
- return true;
- else
- {
- ROS_DEBUG("Impossible to call set_servo_modules service");
- return false;
- }
+ double res;
+
+ //Period time
+ this->new_time=ros::Time::now();
+ time_period = (new_time-this->start_time).toSec();
+ // ROS_INFO("time period: %f", time_period);
+
+ //Calculate PID
+ axis.error = axis.qr_position-axis.distance_target;//axis.error=z_distance_target-qr_position_z;
+ // axis.derivative = (axis.error-axis.prev_error)/time_period;
+ //axis.integral = (axis.error*time_period); //+=
+ //res += axis.kp*axis.error + axis.ki*axis.integral + axis.kd*axis.derivative;
+ res = axis.kp*axis.error;
+ axis.prev_error=axis.error;
+// ROS_INFO("ERROR: %f", axis.error);
+ //ROS_INFO("Integral: %f, Derivada: %f", axis.integral, axis.derivative);
+ //ROS_INFO("qr X: %f, qr Y: %f, qr Z: %f", qr_position_x, qr_position_y, qr_position_z);
+
+ //dependiendo de pid --> modificar amplitude de cmd_vel
+ axis.pid_out = res;
+ // ROS_INFO("PID: %f", axis.pid_out);
+
+ if(axis.pid_out>0.4)//maximo cmd_vel
+ axis.pid_out=0.4;
+ else if(axis.pid_out<-0.4)
+ axis.pid_out=-0.4;
+
+
+
+ this->start_time=ros::Time::now();
}
/*
-void TrackChargeStationAlgNode::pid(void) //distancia z
+void TrackChargeStationAlgNode::pid_yaw(TPID &axis) //distancia yaw
{
- z_distance.error = qr_position_z-z_distance_target;
- //z_distance.error=z_distance_target-qr_position_z;
- z_distance.derivative = (z_distance.error-z_distance.prev_error)/z_distance.period;
- z_distance.integral = (z_distance.error*z_distance.period); //+=
- //pid += z_distance.kp*z_distance.error + z_distance.ki*z_distance.integral + z_distance.kd*z_distance.derivative;
- pid = z_distance.kp*z_distance.error;
- z_distance.prev_error=z_distance.error;
- ROS_INFO("ERROR: %f, PID: %f", z_distance.error, pid);
- //ROS_INFO("Integral: %f, Derivada: %f", z_distance.integral, z_distance.derivative);
+ double res;
+
+ //Period time
+ this->new_time=ros::Time::now();
+ time_period = (new_time-this->start_time).toSec();
+ // ROS_INFO("time period: %f", time_period);
+
+ //Calculate PID
+ axis.error = this->current_pan_angle-axis.distance_target;
+
+ res = axis.kp*axis.error;
+ axis.prev_error=axis.error;
+// ROS_INFO("ERROR: %f", axis.error);
+ //ROS_INFO("Integral: %f, Derivada: %f", axis.integral, axis.derivative);
//ROS_INFO("qr X: %f, qr Y: %f, qr Z: %f", qr_position_x, qr_position_y, qr_position_z);
- //dependiendo de pid --> modificar y_amplitude de cmd_vel
- pid_out = pid/100;
- ROS_INFO("PID 2: %f", pid_out);
+ axis.pid_out = res;
+ // ROS_INFO("PID: %f", axis.pid_out);
- if(pid_out>0.04)//maximo cmd_vel
- return 0.04;
- else if(pid_out<-0.04)
- return -0.04;
- else
- return pid_out;
+ if(axis.pid_out>0.2)//maximo cmd_vel
+ axis.pid_out=0.2;
+ else if(axis.pid_out<-0.2)
+ axis.pid_out=-0.2;
+
+ ROS_INFO("PID OUT: %f", axis.pid_out);
+
+ this->start_time=ros::Time::now();
}
*/
-void TrackChargeStationAlgNode::pid2(TPID caca, double &culo) //distancia z
+/*
+void TrackChargeStationAlgNode::pid2(TPID , double &) //distancia z
{
this->new_time=ros::Time::now();
//this->time_period = (new_time-this->start_time);
time_period = (new_time-this->start_time).toSec();
+ ROS_INFO("time period: %f", time_period);
z_distance.error = qr_position_z-z_distance_target;
// if(z_distance.error<1)
@@ -594,19 +883,15 @@ void TrackChargeStationAlgNode::pid2(TPID caca, double &culo) //distancia z
//}
this->start_time=ros::Time::now();
}
-
+*/
//Head tracking
+/*
void TrackChargeStationAlgNode::set_head_tracking_goal(double pan_target, double tilt_target)
{
this->head_follow_target_goal_.target_pan=pan_target;
this->head_follow_target_goal_.target_tilt=tilt_target;
// this->head_follow_target_goal_.pan_range.resize(2);
- /* this->head_follow_target_goal_.pan_range[0]=this->config_.max_pan;
- this->head_follow_target_goal_.pan_range[1]=this->config_.min_pan;
- this->head_follow_target_goal_.tilt_range.resize(2);
- this->head_follow_target_goal_.tilt_range[0]=this->config_.max_tilt;
- this->head_follow_target_goal_.tilt_range[1]=this->config_.min_tilt;
- */
+
this->head_follow_target_goal_.pan_range[0]=1.5;
this->head_follow_target_goal_.pan_range[1]=-1.5;
//this->head_follow_target_goal_.tilt_range.resize(2);
@@ -614,6 +899,7 @@ void TrackChargeStationAlgNode::set_head_tracking_goal(double pan_target, double
this->head_follow_target_goal_.tilt_range[1]=-1;
}
+
void TrackChargeStationAlgNode::send_head_target(double pan, double tilt)
{
//this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
@@ -621,8 +907,9 @@ void TrackChargeStationAlgNode::send_head_target(double pan, double tilt)
this->head_target_JointTrajectoryPoint_msg_.positions[1]=tilt;
this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
}
-
+*/
//Walk
+/*
bool TrackChargeStationAlgNode::set_walk_params(void)
{
set_walk_params_srv_.request.params.Y_SWAP_AMPLITUDE = config_.Y_SWAP_AMPLITUDE;
@@ -654,16 +941,10 @@ bool TrackChargeStationAlgNode::set_walk_params(void)
ROS_INFO("SmQrSearchAlgNode:: Failed to Call Server on topic set_walk_params ");
}
-
+*/
+ /*
void TrackChargeStationAlgNode::send_cmd_vel(double x_amplitude, double y_amplitude, double a_amplitude)
{
- /* this->cmd_vel_Twist_msg_.linear.x=x_amplitude/period_time;
- this->cmd_vel_Twist_msg_.linear.y=y_amplitude/period_time;
- this->cmd_vel_Twist_msg_.linear.z=0;
- this->cmd_vel_Twist_msg_.angular.x=0;
- this->cmd_vel_Twist_msg_.angular.y=0;
- this->cmd_vel_Twist_msg_.angular.z=a_amplitude/period_time;
-*/
this->cmd_vel_Twist_msg_.linear.x=x_amplitude; //longitudinal. de -0.04 a 0.04 en metros
this->cmd_vel_Twist_msg_.linear.y=y_amplitude; //Transversal
this->cmd_vel_Twist_msg_.linear.z=0;
@@ -673,7 +954,8 @@ void TrackChargeStationAlgNode::send_cmd_vel(double x_amplitude, double y_amplit
this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
}
-
+*/
+/*
void TrackChargeStationAlgNode::stop_walking(void)
{
this->cmd_vel_Twist_msg_.linear.x=0;
@@ -685,7 +967,7 @@ void TrackChargeStationAlgNode::stop_walking(void)
this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
}
-
+*/
/*
void pid(TPID algo)