diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml index ac92cd5bbd88bf21b70eff51467beb5bfdfc764d..ef579f15c48dfc87d1eac45e223e6e5278c48f20 100644 --- a/config/omniwheels_config.yaml +++ b/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 diff --git a/scripts/omni_wheel_controller_node.py b/scripts/omni_wheel_controller_node.py index 4d726ec7ca5d998c92c90597eb8c13fcbfa74653..6fd014d8a12136e96c8fdfa8b4d7d71198616a83 100755 --- a/scripts/omni_wheel_controller_node.py +++ b/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)