latest version

config/omniwheels_config.yaml

@@ -4,14 +4,14 @@ id: 1
 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.00065 #this does not match the real wheelradius
-kp_x: 0.095 
+R: 0.16
+r: 0.0007 #this does not match the real wheelradius
+kp_x: 0.1
+kp_y: 0.1
+kp_z: 0.1
 ki_x: 0.01
-kp_y: 0.095 
-ki_y: 0.0
-kp_z: 1.75 
-ki_z: 0.0
+ki_y: 0.01
+ki_z: 0.01
 rate_hz: 50 # rate of the control loop
 
 

scripts/Buffer.py

@@ -4,11 +4,12 @@ import numpy as np
 class Buffer:
     def __init__(self, buffer_size):
         self.buf = np.zeros(buffer_size)
+        self.bfs = buffer_size
         
     def get_mean(self):
         return self.buf.mean()
     def fill(self, data):
-        self.buf[0:-2] = self.buf[1:-1]
-        self.buf[-1] = data
+        self.buf[0:self.bfs-1] = self.buf[1:self.bfs]
+        self.buf[self.bfs-1] = data
     def print_buffer(self):
         print(self.buf)

scripts/crazyflie_logger_node.py

@@ -50,7 +50,7 @@ class ParameterLogging:
     Simple logging example class that logs the Stabilizer from a supplied
     link uri and disconnects after 5s.
     """
-    period_in_ms = 20
+    period_in_ms = 30
 
     def __init__(self, link_uri):
         
@@ -216,11 +216,11 @@ class ParameterLogging:
         self.pos_x_buf.fill(data['stateEstimateZ.x']/1000.0)
         self.pos_y_buf.fill(data['stateEstimateZ.y']/1000.0)
         z = data['stateEstimateZ.z']/1000.0
-        yaw = (data['stateEstimate.yaw'])
-        self.pos_yaw_buf.fill(np.mod(yaw-30, 360));
+        self.pos_yaw_buf.fill(data['stateEstimate.yaw'])
+        yaw_omni = np.mod(self.pos_yaw_buf.get_mean()-30, 360);
 
        
-        quat_val = get_quaternion_from_euler(0,0,np.deg2rad(self.pos_yaw_buf.get_mean()))
+        quat_val = get_quaternion_from_euler(0,0,np.deg2rad(yaw_omni))
         
 
         
@@ -228,8 +228,8 @@ class ParameterLogging:
         self._pose_data_.orientation.y =quat_val[1]
         self._pose_data_.orientation.z = quat_val[2]
         self._pose_data_.orientation.w = quat_val[3] 
-        self._pose_data_.position.x =self.pos_x_buf.get_mean()+ np.sin(np.deg2rad(yaw))*self._position_offset;
-        self._pose_data_.position.y = self.pos_x_buf.get_mean()- np.cos(np.deg2rad(yaw))*self._position_offset
+        self._pose_data_.position.x = self.pos_x_buf.get_mean() + np.sin(np.deg2rad(self.pos_yaw_buf.get_mean()))*self._position_offset;
+        self._pose_data_.position.y = self.pos_y_buf.get_mean()- np.cos(np.deg2rad(self.pos_yaw_buf.get_mean()))*self._position_offset
         self._pose_data_.position.z = z;
         
              

scripts/omni_wheel_controller_node.py

@@ -14,6 +14,7 @@ from dynamixel.model.xm430_w210_t_r import XM430_W210_T_R
 import dynamixel.channel
 import time
 
+
 def euler_from_quaternion(x, y, z, w):
         """
         Convert a quaternion into euler angles (roll, pitch, yaw)
@@ -39,6 +40,8 @@ class omniWheelvelocityController:
         def __init__(self):
                 # Load the servos
                 self.servos = self.load_servos()
+                self.MAX_WHEEL_SPEED =310
+
                 self.motor_command = [0,0,0];
                 self.send_command = False;
                 self.vel_cmd_arrived = False;
@@ -109,9 +112,10 @@ class omniWheelvelocityController:
                                 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);
+                                ph = self.vel2motor(xdot, (yaw));
                                 #print(np.rad2deg(yaw))
-                                #print(ph)
+                                #print(error)
+                                #print(xdot)
                                 
                                 self.motor_command[0] = ph[0]
                                 self.motor_command[1] = ph[1]
@@ -124,9 +128,15 @@ class omniWheelvelocityController:
                                 self.motor_command[1] = 0
                                 self.motor_command[2] = 0
                                 
+
                         # Send command to the motors
                         for (j,s) in enumerate(self.servos):
-                                s.goal_velocity.write(round(self.motor_command[j]))
+                                w_c_ = self.motor_command[j]
+                                w_c = self.saturate(w_c_, self.MAX_WHEEL_SPEED);
+                                
+                                #print(str(w_c_) + ": " + str(w_c))
+                                s.goal_velocity.write(round(w_c))#self.motor_command[j]))
+                                #print(round(w_c))
                         rate.sleep()
 
 
@@ -171,7 +181,13 @@ class omniWheelvelocityController:
                 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)
-
+        def saturate(self, command, limit):
+            if command > limit:
+                return limit
+            elif command < -limit:
+                return -limit
+            else:
+                return command
 if __name__ == '__main__':
         rospy.init_node("omni_wheel_controller_node")
         try: