From 7260078b3bf9b136140d9f9b33fbaee9abf14848 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com> Date: Wed, 11 May 2022 08:10:38 +0200 Subject: [PATCH] radius adjusted to compensate velocity control --- config/omniwheels_config.yaml | 4 ++-- scripts/omni_wheel_controller_node.py | 5 +++-- test.py | 2 ++ 3 files changed, 7 insertions(+), 4 deletions(-) create mode 100644 test.py diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml index 25c5056..e33f28a 100644 --- a/config/omniwheels_config.yaml +++ b/config/omniwheels_config.yaml @@ -5,9 +5,9 @@ channel: 90 # change to the correct one type: default position_offset: 0.11 # ofsset from crazyflie module to the cenyter of the omniwheels R: 0.0165 -r: 0.003 +r: 0.00065 #this does not match the real wheelradius kp: 0.1 -ki: 0.01 +ki: 0.0 rate_hz: 50 # rate of the control loop diff --git a/scripts/omni_wheel_controller_node.py b/scripts/omni_wheel_controller_node.py index 4b4ae17..32a73a5 100755 --- a/scripts/omni_wheel_controller_node.py +++ b/scripts/omni_wheel_controller_node.py @@ -106,7 +106,8 @@ class omniWheelvelocityController: vel_ref = np.array([self.vel_cmd.linear.x, self.vel_cmd.linear.y, self.vel_cmd.angular.z]) error = vel_ref - np.array([self._current_vel.linear.x, self._current_vel.linear.y, self._current_vel.angular.z]); xdot = vel_ref + self.pi.update_control(error) - ph = self.vel2motor(xdot, np.deg2rad(yaw)); + ph = self.vel2motor(vel_ref, np.deg2rad(-180)); + #print(np.rad2deg(yaw)) self.motor_command[0] = ph[0] self.motor_command[1] = ph[1] @@ -163,7 +164,7 @@ class omniWheelvelocityController: self._current_pose = data; self.pose_callback_initiated = True; def vel2motor(self,xdot, ang): - M = 1/self.r*np.array([[-np.sin(ang), np.cos(ang), self.R ],[-np.sin(ang+self.a1), np.cos(ang+self.a1), self.R],[-np.sin(ang+self.a2), np.cos(ang+self.a2), self.R]]) + M = -1 /self.r*np.array([[-np.sin(ang), np.cos(ang), self.R ],[-np.sin(ang+self.a1), np.cos(ang+self.a1), self.R],[-np.sin(ang+self.a2), np.cos(ang+self.a2), self.R]]) return M.dot(xdot) #return M.dot(xdot) diff --git a/test.py b/test.py new file mode 100644 index 0000000..05b25d5 --- /dev/null +++ b/test.py @@ -0,0 +1,2 @@ +here + -- GitLab