From 92cbca1f73b3f3b2ad2d02e761c620a8a896eba9 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Tue, 10 May 2022 17:38:25 +0200
Subject: [PATCH] remaining pid
---
scripts/omni_wheel_controller_node.py | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/scripts/omni_wheel_controller_node.py b/scripts/omni_wheel_controller_node.py
index 6fd014d..ef92a8a 100755
--- a/scripts/omni_wheel_controller_node.py
+++ b/scripts/omni_wheel_controller_node.py
@@ -95,10 +95,11 @@ class omniWheelvelocityController:
roll, pitch,yaw = euler_from_quaternion(self._current_pose.orientation.x,self._current_pose.orientation.y,self._current_pose.orientation.z,self._current_pose.orientation.w)
xdot = np.array([self.vel_cmd.linear.x, self.vel_cmd.linear.y, self.vel_cmd.angular.z])
ph = self.vel2motor(xdot, np.deg2rad(yaw));
- print(np.rad2deg(yaw))
self.motor_command[0] = ph[0]
self.motor_command[1] = ph[1]
self.motor_command[2] = ph[2]
+ error = xdot - np.array([self._current_vel.linear.x, self._current_vel.linear.y, self._current_vel.angular.z]);
+ print(error)
self.vel_cmd_arrived = False
else:
--
GitLab