diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml
index e33f28a0ee1f691e652529164a74e0e65113073d..7da006515acef2554deb51c55751b820bffc090c 100644
--- a/config/omniwheels_config.yaml
+++ b/config/omniwheels_config.yaml
@@ -6,8 +6,12 @@ type: default
 position_offset: 0.11 # ofsset from crazyflie module to the cenyter of the omniwheels
 R: 0.0165
 r: 0.00065 #this does not match the real wheelradius
-kp: 0.1
-ki: 0.0
+kp_x: 0.095 
+ki_x: 0.01
+kp_y: 0.095 
+ki_y: 0.0
+kp_z: 1.75 
+ki_z: 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 32a73a5b7baec1334483340090842eff22b118c2..8127a284f1048c518bba82a79ac1ba38198cf299 100755
--- a/scripts/omni_wheel_controller_node.py
+++ b/scripts/omni_wheel_controller_node.py
@@ -57,10 +57,12 @@ class omniWheelvelocityController:
                 self.a1 = 2*np.pi/3 	# Angle between x axis and first wheel
                 self.a2 = 4*np.pi/3  # Angle between x axis and second wheel
                 self.r = rospy.get_param("/r") # Wheel radius. Has been fudge-factored because the actual velocity of the wheels did not align with the set-points.
-                kp = rospy.get_param("/kp")
-                ki = rospy.get_param("/ki")
-                
-
+                kp_x = rospy.get_param("/kp_x")
+                ki_x = rospy.get_param("/ki_x")
+                kp_y = rospy.get_param("/kp_y")
+                ki_y = rospy.get_param("/ki_y")                
+                kp_z = rospy.get_param("/kp_z")
+                ki_z = rospy.get_param("/ki_z")   
 
                 # get ros param value
                 robot_name = rospy.get_param("/name");
@@ -92,9 +94,9 @@ class omniWheelvelocityController:
                 rate_hz = rospy.get_param("/rate_hz")
                 rate = rospy.Rate(rate_hz) # 10hz
                 dt = 1.0/float(rate_hz);
-                self.pi = SimplePI(kp,ki,dt)         # Create a PI-controller 
-
-                
+                self.pi_x = SimplePI(kp_x,ki_x,dt)         # Create a PI-controller 
+                self.pi_y = SimplePI(kp_y,ki_y,dt)         # Create a PI-controller 
+                self.pi_z = SimplePI(kp_y,ki_y,dt)         # Create a PI-controller                 
 
                 # main loop
                 while not rospy.is_shutdown():
@@ -105,14 +107,18 @@ 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)
                                 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(vel_ref, np.deg2rad(-180));
+                                vel_x = vel_ref[0] + self.pi_x.update_control(error[0])
+                                vel_y = vel_ref[1] + self.pi_y.update_control(error[1])
+                                vel_z = vel_ref[2] + self.pi_z.update_control(error[2])
+                                xdot = np.array([vel_x[0],vel_y[0],vel_z[0]])
+                                ph = self.vel2motor(xdot, yaw);
                                 #print(np.rad2deg(yaw))
+                                #print(ph)
                                 
                                 self.motor_command[0] = ph[0]
                                 self.motor_command[1] = ph[1]
                                 self.motor_command[2] = ph[2]
-                                print(error)
+                                #print(error_x)
                                 self.vel_cmd_arrived = False
 
                         else:
@@ -164,7 +170,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)
 
@@ -172,7 +178,5 @@ if __name__ == '__main__':
         rospy.init_node("omni_wheel_controller_node")
         try:
                 omniwheel1 = omniWheelvelocityController()
-        except rospy.ROSInterruptException:
-                pass
-
-
+        except (rospy.ServiceException, rospy.ROSException) as e:
+        	print("ERROR : "+str(e))