velocity controller working, pid is needed

config/omniwheels_config.yaml

@@ -5,5 +5,6 @@ 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
 
 

scripts/omni_wheel_controller_node.py

@@ -52,10 +52,10 @@ class omniWheelvelocityController:
                 self._current_pose = Pose()
                 self.vel_callback_initiated  = False;
                 self.pose_callback_initiated = False;
-                self.R = 0.165 		# Distance between center of robot and wheels
+                self.R = rospy.get_param("/R")		# Distance between center of robot and wheels
                 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 = 0.027 # Wheel radius. Has been fudge-factored because the actual velocity of the wheels did not align with the set-points.
+                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.
 
 
                 # get ros param value
@@ -93,10 +93,9 @@ class omniWheelvelocityController:
                         if self.vel_cmd_arrived == True and self.vel_callback_initiated == True and self.pose_callback_initiated==True:
                                 # compute motor values from velocity command
                                 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([1,0,0]);#self._current_vel.linear.x, self._current_vel.linear.y, self._current_vel.angular.z])
-                                ph = self.vel2motor(xdot, 0);
-                                #print(ph)
-                                print(yaw)
+                                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]
@@ -150,7 +149,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)