radius adjusted to compensate velocity control

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
 
 

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)
 

test.py

+here
+