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