added effort position controllers

tt-saito · Jul 6, 2022 · dd755fa · dd755fa
1 parent 206612d
commit dd755fa
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 5 deletions.
diff --git a/mars_rover/config/mars_rover_control.yaml b/mars_rover/config/mars_rover_control.yaml
@@ -15,7 +15,7 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
 
     wheel_tree_position_controller:
-      type: effort_controllers/JointGroupEffortController
+      type: effort_controllers/JointGroupPositionController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
@@ -90,3 +90,28 @@ wheel_tree_position_controller:
       - position
       - velocity
       - effort
+    gains:
+      suspension_arm_F_L_joint:
+        p: 2200.0
+        i: 10.0
+        d: 10.0
+      suspension_arm_B_L_joint:
+        p: 4200.0
+        i: 10.0
+        d: 10.0
+      suspension_arm_B2_L_joint:
+        p: 2200.0
+        i: 10.0
+        d: 10.0
+      suspension_arm_F_R_joint:
+        p: 2200.0
+        i: 10.0
+        d: 10.0
+      suspension_arm_B_R_joint:
+        p: 4200.0
+        i: 10.0
+        d: 10.0
+      suspension_arm_B2_R_joint:
+        p: 2200.0
+        i: 10.0
+        d: 10.0
diff --git a/mars_rover/nodes/move_wheel b/mars_rover/nodes/move_wheel
@@ -98,7 +98,7 @@ class MoveWheel(Node):
 
         target_val = Float64MultiArray()
         if sus_val is None:
-            target_val.data = [600.0,-700.0,-100.0,600.0,-700.0,-100.0]
+            target_val.data = [0.2,-0.2,-0.2,0.2,-0.2,-0.2]
         self.suspension_publisher_.publish(target_val)
 
 

diff --git a/mars_rover/nodes/run_demo b/mars_rover/nodes/run_demo
@@ -22,14 +22,14 @@ class RunDemo(Node):
         self.timer = self.create_timer(timer_period, self.timer_callback)
 
         action1 = Twist()
-        action1.linear.x = 3.0
+        action1.linear.x = 2.0
 
         action2 = Twist()
-        action2.linear.x = 4.0
+        action2.linear.x = 3.0
         action2.angular.z = 0.4
 
         action3 = Twist()
-        action3.linear.x = 4.0
+        action3.linear.x = 3.0
         action3.angular.z = -0.4
 
         self.actions = [action1, action2, action3]