remaining pid

scripts/omni_wheel_controller_node.py

@@ -95,10 +95,11 @@ 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)
                                 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]
+                                error = xdot - np.array([self._current_vel.linear.x, self._current_vel.linear.y, self._current_vel.angular.z]);
+                                print(error)
                                 self.vel_cmd_arrived = False
 
                         else: