diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml
index 7da006515acef2554deb51c55751b820bffc090c..10128ba8b399209892e558af72d6a3ef193f9c77 100644
--- a/config/omniwheels_config.yaml
+++ b/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
diff --git a/scripts/Buffer.py b/scripts/Buffer.py
index 6313d5b24281c2e6ad5d816d8a4997cb37e70f89..3d7babaa77b4bb09f6a2f3faace848f8bf166393 100644
--- a/scripts/Buffer.py
+++ b/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)
diff --git a/scripts/__pycache__/Buffer.cpython-37.pyc b/scripts/__pycache__/Buffer.cpython-37.pyc
index b063a054839cc29247a2074c1200ab964583dbd5..a90aba17ef30ca98717b2f88bd06d8d903612b8f 100644
Binary files a/scripts/__pycache__/Buffer.cpython-37.pyc and b/scripts/__pycache__/Buffer.cpython-37.pyc differ
diff --git a/scripts/crazyflie_logger_node.py b/scripts/crazyflie_logger_node.py
index 6798272f8327628b7645a77cc9e5b0593d38aa8c..5ecd22901e9e2f9fe7f91131939d0f75cfcbae0c 100755
--- a/scripts/crazyflie_logger_node.py
+++ b/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;
diff --git a/scripts/omni_wheel_controller_node.py b/scripts/omni_wheel_controller_node.py
index 0ee957cb2c5326029d18312b10121d29086a4764..ba5a36ed3d4046c161fa4206a0c0beeb85adb5f7 100755
--- a/scripts/omni_wheel_controller_node.py
+++ b/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;
@@ -53,8 +56,8 @@ class omniWheelvelocityController:
self._current_pose = Pose()
self.vel_callback_initiated = False;
self.pose_callback_initiated = False;
- 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.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 = rospy.get_param("/r") # Wheel radius. Has been fudge-factored because the actual velocity of the wheels did not align with the set-points.
kp_x = rospy.get_param("/kp_x")
@@ -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]
@@ -123,10 +127,16 @@ class omniWheelvelocityController:
self.motor_command[0] = 0
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,10 +181,16 @@ 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:
omniwheel1 = omniWheelvelocityController()
except (rospy.ServiceException, rospy.ROSException) as e:
- print("ERROR : "+str(e))
+ print("ERROR : "+str(e))