def __init__(self):
self.euler_angles = [0, 0, 0]
self.external_demands = [0, 0, 0, 0, 0, 0]
self.motor_demands = [0, 0, 0, 0, 0, 0]
self.depth_demand = 0
self.depth_real_value = 0
self.yaw_demand = 0
self.pitch_demand = 0
self.roll_demand = 0
rospy.init_node('cauv_controller')
self.pub = rospy.Publisher('motor_demand',
std_msgs.msg.String,
queue_size=5)
rospy.Subscriber("imu_euler_angles", std_msgs.msg.String,
self.euler_angle_callback)
rospy.Subscriber("external_demands", std_msgs.msg.String,
self.external_demand_callback)
rospy.Subscriber("depth_demand", std_msgs.msg.String,
self.set_depth_demand)
rospy.Subscriber("depth_value", std_msgs.msg.String,
self.set_depth_real_value)
rospy.Subscriber("pitch_demand", std_msgs.msg.String,
self.set_pitch_demand)
rospy.Subscriber("yaw_demand", std_msgs.msg.String,
self.set_yaw_demand)
rospy.Subscriber("roll_demand", std_msgs.msg.String,
self.set_roll_demand)
self.rate = rospy.Rate(10) # hz
self.yaw_pid = PIDControl(0, 0, 0, True)
self.pitch_pid = PIDControl(0, 0, 0, True)
self.roll_pid = PIDControl(0, 0, 0, True)
self.depth_pid = PIDControl(0, 0, 0, False)
def __init__(self, kp, ki, kd, limit, sigma, Ts, flag=True):
self.kp = kp # Proportional control gain
self.ki = ki # Integral control gain
self.kd = kd # Derivative control gain
self.limit = limit # The output saturates at this limit
self.sigma = sigma # dirty derivative bandwidth is 1/sigma
self.beta = (2.0 * sigma - Ts) / (2.0 * sigma + Ts)
self.Ts = Ts # sample rate
self.flag = flag
self.PID_object = PIDControl(self.kp, self.ki, self.kd, self.limit,
self.sigma, self.Ts, self.flag)
def test_create_processes(odisseus_config_obj):
error_msg = "No Erro in test" + test_create_processes.__name__
master = MasterProcess(odisseus_configuration=odisseus_config_obj)
pid_control = PIDControl(Kp=odisseus_config_obj.PROPULSION_PID_Kp,
Ki=odisseus_config_obj.PROPULSION_PID_Ki,
Kd=odisseus_config_obj.PROPULSION_PID_Kd)
kwargs = dict()
kwargs[odisseus_config_obj.PROPULSION_CONTROLLER_NAME] = pid_control
master.create_processes(**kwargs)
names = master.get_processes_names()
count = 0
if odisseus_config_obj.ENABLE_IR_SENSOR:
count += 1
if odisseus_config_obj.ENABLE_CAMERA:
count += 1
if odisseus_config_obj.ENABLE_MOTORS:
count += 1
if odisseus_config_obj.ENABLE_ULTRASOUND_SENSOR:
count += 1
if odisseus_config_obj.ENABLE_WEB_SERVER:
count += 1
master.terminate_all_processes()
if count != len(names):
error_msg = "ERROR in {0}: Not all processes were started. Should be {1} but started only {2} ".format(
test_create_processes.__name__, count, len(names))
return error_msg
def __init__(self):
self.ctrl = PIDControl(P.kp, P.ki, P.kd, P.umax, P.sigma, P.Ts)
class Controller:
def __init__(self):
self.ctrl = PIDControl(P.kp, P.ki, P.kd, P.umax, P.sigma, P.Ts)
def update(self, r, y):
return self.ctrl.PID(r,y)
def update(self, r, y):
PID_object = PIDControl(self.kp, self.ki, self.kd, self.limit,
self.sigma, self.Ts, self.flag)
return PID_object.PID(r, y)
def test_getErrorAngle1(self):
testPid = PIDControl(0, 0, 0, True)
errAngle = testPid.getErrorAngle(120, 180)
self.assertEqual(errAngle, -60)
def test_getErrorAngle3(self):
testPid = PIDControl(0, 0, 0, True)
errAngle = testPid.getErrorAngle(-30, 45)
self.assertEqual(errAngle, -75)
def test_getErrorAngle2(self):
testPid = PIDControl(0, 0, 0, True)
errAngle = testPid.getErrorAngle(720, 240)
self.assertEqual(errAngle, 120)
self.tau = tau # Time Constant
self.omega = omega # initial state
self.Ts = Ts
def f(t, x, u):
return np.array([x[1], -1 / P.tau * x[1] + P.K / P.tau * u])
# Initial Conditions
r0 = 1
theta0 = 0
omega0 = 0
t0 = 0
x0 = np.array([theta0, omega0])
C = PIDControl(P.kp, P.ki, P.kd, P.umax, P.sigma, P.Ts, flag=True)
plant = System(P.K, P.tau, omega0, P.Ts)
integrator_RK4 = intg.get_integrator(P.Ts, System.f)
# Simulate step response
t_history = [t0]
x_history = [x0]
u_history = [0]
r = r0
x = np.array([theta0, omega0])
t = t0
for i in range(P.nsteps):
u = C.PID(r, x[0])