diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml
index 25c5056103b13c9caf822fa17d494b5e41b65601..e33f28a0ee1f691e652529164a74e0e65113073d 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 4b4ae175b5116e2c387b3a5993745a232a0085f4..32a73a5b7baec1334483340090842eff22b118c2 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 0000000000000000000000000000000000000000..05b25d5e1d5a25abb6cbe0002386afcafe96bee3
--- /dev/null
+++ b/test.py
@@ -0,0 +1,2 @@
+here
+