From e0a53985ae669cf1038cbe423a5c440e873db6b2 Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Tue, 30 Jun 2020 14:59:33 -0300
Subject: [PATCH] Add group_pid controller.

---
 q2d_bringup/config/group_pid.yaml      | 11 +++++++++++
 q2d_bringup/config/pid.yaml            |  2 +-
 q2d_bringup/launch/group_bypass.launch |  6 +-----
 q2d_bringup/launch/group_ijc.launch    |  5 +++++
 q2d_bringup/launch/group_pid.launch    |  1 +
 5 files changed, 19 insertions(+), 6 deletions(-)
 create mode 100644 q2d_bringup/config/group_pid.yaml
 mode change 100644 => 120000 q2d_bringup/launch/group_bypass.launch
 create mode 100644 q2d_bringup/launch/group_ijc.launch
 create mode 120000 q2d_bringup/launch/group_pid.launch

diff --git a/q2d_bringup/config/group_pid.yaml b/q2d_bringup/config/group_pid.yaml
new file mode 100644
index 0000000..c6317f2
--- /dev/null
+++ b/q2d_bringup/config/group_pid.yaml
@@ -0,0 +1,11 @@
+group_controller:
+        type: effort_controllers/JointGroupEffortController
+        joints:
+                - shoulder_active_joint
+                - elbow_active_joint
+        shoulder_active_joint/pid: {p: 2310, i: 4640, d: 0.299}
+        elbow_active_joint/pid: {p: 339, i: 851, d: 0.351}
+
+joint_states_publisher:
+        type: joint_state_controller/JointStateController
+        publish_rate: 100
diff --git a/q2d_bringup/config/pid.yaml b/q2d_bringup/config/pid.yaml
index 5d0200c..6ef84b6 100644
--- a/q2d_bringup/config/pid.yaml
+++ b/q2d_bringup/config/pid.yaml
@@ -1,7 +1,7 @@
 shoulder_controller:
         type: effort_controllers/JointPositionController
         joint: shoulder_active_joint
-        pid: {p: 2310, i: 4640, d: 0.299}   
+        pid: {p: 2310, i: 4640, d: 0.299}
 
 elbow_controller:
         type: effort_controllers/JointPositionController
diff --git a/q2d_bringup/launch/group_bypass.launch b/q2d_bringup/launch/group_bypass.launch
deleted file mode 100644
index 89dd4cd..0000000
--- a/q2d_bringup/launch/group_bypass.launch
+++ /dev/null
@@ -1,5 +0,0 @@
-<launch>
-	<node name="controller_spawner" pkg="controller_manager"
-		type="spawner" respawn="false" output="screen"
-		args="group_controller joint_states_publisher" />
-</launch>
diff --git a/q2d_bringup/launch/group_bypass.launch b/q2d_bringup/launch/group_bypass.launch
new file mode 120000
index 0000000..2f8f799
--- /dev/null
+++ b/q2d_bringup/launch/group_bypass.launch
@@ -0,0 +1 @@
+group_ijc.launch
\ No newline at end of file
diff --git a/q2d_bringup/launch/group_ijc.launch b/q2d_bringup/launch/group_ijc.launch
new file mode 100644
index 0000000..89dd4cd
--- /dev/null
+++ b/q2d_bringup/launch/group_ijc.launch
@@ -0,0 +1,5 @@
+<launch>
+	<node name="controller_spawner" pkg="controller_manager"
+		type="spawner" respawn="false" output="screen"
+		args="group_controller joint_states_publisher" />
+</launch>
diff --git a/q2d_bringup/launch/group_pid.launch b/q2d_bringup/launch/group_pid.launch
new file mode 120000
index 0000000..2f8f799
--- /dev/null
+++ b/q2d_bringup/launch/group_pid.launch
@@ -0,0 +1 @@
+group_ijc.launch
\ No newline at end of file
-- 
2.12.0