Controller finished

config/omniwheels_config.yaml

@@ -6,8 +6,12 @@ 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: 0.1
-ki: 0.0
+kp_x: 0.095 
+ki_x: 0.01
+kp_y: 0.095 
+ki_y: 0.0
+kp_z: 1.75 
+ki_z: 0.0
 rate_hz: 50 # rate of the control loop
 
 

scripts/omni_wheel_controller_node.py

@@ -57,10 +57,12 @@ class omniWheelvelocityController:
                 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 = rospy.get_param("/kp")
-                ki = rospy.get_param("/ki")
-                
-
+                kp_x = rospy.get_param("/kp_x")
+                ki_x = rospy.get_param("/ki_x")
+                kp_y = rospy.get_param("/kp_y")
+                ki_y = rospy.get_param("/ki_y")                
+                kp_z = rospy.get_param("/kp_z")
+                ki_z = rospy.get_param("/ki_z")   
 
                 # get ros param value
                 robot_name = rospy.get_param("/name");
@@ -92,9 +94,9 @@ class omniWheelvelocityController:
                 rate_hz = rospy.get_param("/rate_hz")
                 rate = rospy.Rate(rate_hz) # 10hz
                 dt = 1.0/float(rate_hz);
-                self.pi = SimplePI(kp,ki,dt)         # Create a PI-controller 
-
-                
+                self.pi_x = SimplePI(kp_x,ki_x,dt)         # Create a PI-controller 
+                self.pi_y = SimplePI(kp_y,ki_y,dt)         # Create a PI-controller 
+                self.pi_z = SimplePI(kp_y,ki_y,dt)         # Create a PI-controller                 
 
                 # main loop
                 while not rospy.is_shutdown():
@@ -105,14 +107,18 @@ 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)
                                 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(vel_ref, np.deg2rad(-180));
+                                vel_x = vel_ref[0] + self.pi_x.update_control(error[0])
+                                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);
                                 #print(np.rad2deg(yaw))
+                                #print(ph)
                                 
                                 self.motor_command[0] = ph[0]
                                 self.motor_command[1] = ph[1]
                                 self.motor_command[2] = ph[2]
-                                print(error)
+                                #print(error_x)
                                 self.vel_cmd_arrived = False
 
                         else:
@@ -172,7 +178,5 @@ if __name__ == '__main__':
         rospy.init_node("omni_wheel_controller_node")
         try:
                 omniwheel1 = omniWheelvelocityController()
-        except rospy.ROSInterruptException:
-                pass
-
-
+        except (rospy.ServiceException, rospy.ROSException) as e:
+        	print("ERROR : "+str(e))