def __init__(self):
rospy.Subscriber("/Indoor_Bldddimp/setpoint_position", PoseStamped,
self.process_setpoint)
rospy.Subscriber("/imu/data", Imu, self.process_current)
self.blimp = Blimp()
self.set_yaw = 90
self.set_trans_x = 0
self.set_trans_z = 0
self.curr_yaw = 0
self.curr_trans_x = 0
self.curr_trans_z = 0
yaw_kp = 0.8 #rospy.get_param("~yaw_kp")
yaw_ki = 0.8 #rospy.get_param("~yaw_ki")
yaw_kd = 0.8 #rospy.get_param("~yaw_kd")
self.yaw_control = PID_Controller(yaw_kp, yaw_ki, yaw_kd)
def _initialize_pid_controlers(self):
'''
Initialize communication with the pid controllers.
Parameters:
N/A
Returns:
N/A
'''
pid_params = rospy.get_param("/pid")
velocity_pid_params = pid_params['velocity']
steering_pid_params = pid_params['steering']
velocity_k_p = velocity_pid_params['k_p']
velocity_k_i = velocity_pid_params['k_i']
velocity_k_d = velocity_pid_params['k_d']
self.velocity_pid_controller = PID_Controller(k_p=velocity_k_p,
k_i=velocity_k_i,
k_d=velocity_k_d,
integral_min=-10,
integral_max=10,
max_control_effort=255,
min_control_effort=-255)
steering_k_p = steering_pid_params['k_p']
steering_k_i = steering_pid_params['k_i']
steering_k_d = steering_pid_params['k_d']
self.steering_pid_controller = PID_Controller(k_p=steering_k_p,
k_i=steering_k_i,
k_d=steering_k_d,
max_control_effort=255,
min_control_effort=-255,
integral_min=-10,
integral_max=10,
angle_error=True)
#Initialize the service for updating the pid controller gains
rospy.Service('update_pid_gains', pid_gains,
self.handle_pid_gains_update)
return
class Yaw_control:
def __init__(self):
rospy.Subscriber("/Indoor_Bldddimp/setpoint_position", PoseStamped,
self.process_setpoint)
rospy.Subscriber("/imu/data", Imu, self.process_current)
self.blimp = Blimp()
self.set_yaw = 90
self.set_trans_x = 0
self.set_trans_z = 0
self.curr_yaw = 0
self.curr_trans_x = 0
self.curr_trans_z = 0
yaw_kp = 0.8 #rospy.get_param("~yaw_kp")
yaw_ki = 0.8 #rospy.get_param("~yaw_ki")
yaw_kd = 0.8 #rospy.get_param("~yaw_kd")
self.yaw_control = PID_Controller(yaw_kp, yaw_ki, yaw_kd)
def control_loop(self):
print("Set Yaw Angle: " + repr(self.set_yaw))
print("Current Yaw Angle: " + repr(self.curr_yaw))
mv = self.yaw_control.corrective_action(self.set_yaw, self.curr_yaw)
print("corrected speed")
print(mv)
self.blimp.motors.turn(mv)
def pan_tilt(self):
self.pan_tilt.pitch(10)
self.pan_tilt.roll(6)
def process_setpoint(self, poseStamped):
pose = poseStamped.pose
quart = pose.orientation
trans = pose.position
exp_quart = [quart.x, quart.y, quart.z, quart.w]
r, p, y = euler_from_quaternion(exp_quart)
self.set_yaw = y
def process_current(self, imu_data):
self.curr_yaw = imu_data.orientation.x
def __init__(self):
rospy.Subscriber("key", String, self.velocity_set)
rospy.Subscriber("/ultrasonic", Float64, self.obstacle_control)
self.blimp = Blimp()
self.set_yaw = 90
self.set_trans_x = 0
self.set_trans_z = 0
self.curr_yaw = 0
self.curr_trans_x = 0
self.curr_trans_z = 0
yaw_kp = 0.8 #rospy.get_param("~yaw_kp")
yaw_ki = 0.8 #rospy.get_param("~yaw_ki")
yaw_kd = 0.8 #rospy.get_param("~yaw_kd")
self.yaw_control = PID_Controller(yaw_kp, yaw_ki, yaw_kd)
def __init__(self):
rospy.Subscriber("/Outdoor_Blimp/setpoint_position", PoseStamped,
self.process_setpoint)
rospy.Subscriber("/Outdoor_Blimp/imu/orientation", Vector3,
self.process_current)
self.blimp = Blimp()
self.set_yaw = 0
self.set_pitch = 0
self.set_roll = 0
self.set_trans_x = 0
self.set_trans_z = 0
self.curr_pitch = 0
self.curr_roll = 0
self.curr_yaw = 0
self.curr_trans_x = 0
self.curr_trans_z = 0
yaw_kp = rospy.get_param("~yaw_kp")
yaw_ki = rospy.get_param("~yaw_ki")
yaw_kd = rospy.get_param("~yaw_kd")
trans_x_kp = rospy.get_param("~trans_x_kp")
trans_x_ki = rospy.get_param("~trans_x_ki")
trans_x_kd = rospy.get_param("~trans_x_kd")
trans_z_kp = rospy.get_param("~trans_z_kp")
trans_z_ki = rospy.get_param("~trans_z_ki")
trans_z_kd = rospy.get_param("~trans_z_kd")
self.curr_roll = 0
self.yaw_control = PID_Controller(yaw_kp, yaw_ki, yaw_kd)
self.trans_x_control = PID_Controller(trans_x_kp, trans_x_ki,
trans_x_kd)
self.trans_z_control = PID_Controller(trans_z_kp, trans_z_ki,
trans_z_kd)
self.servos = pigpio.pi()
def run_sim(self):
dt = .02
self.manage_inputs()
self.mass = MassModel(self, self.initial_mass_state, dt)
if self.MODE == 'pid':
controller = PID_Controller(self, dt)
else:
controller = BB_Controller(self, dt)
ref = self.ref #meters
self.draw_reference(ref)
xt = [self.mass.X[0], self.mass.X[0]]
tt = [-1, 0]
rt = [self.initial_mass_state[0], self.initial_mass_state[0]]
ut = [0, 0]
t_end = self.TIME_RUN #seconds
count = 0
while self.mass.t < t_end:
count += 1
if (count % 10) == 0:
print "time: %.2f" % self.mass.t
t0 = datetime.datetime.now()
tt.append(self.mass.t)
xt.append(self.mass.X[0])
ut.append(sum(controller.control_output))
rt.append(ref)
gui.update_mass(self.mass.X)
controller.run(self.mass.X, ref)
control = controller.control_output
self.draw_control_effort(control)
self.mass.step_time_forward(sum(control))
self.master.update_idletasks()
self.master.update()
t1 = datetime.datetime.now()
dt_actual = (t1 - t0).total_seconds()
if dt_actual < dt:
time.sleep(dt - dt_actual)
else:
time.sleep(.005)
self.step_info(np.array(tt), np.array(xt), ref)
line1, = plt.plot(tt, xt, label='Block Location X(t)')
line2, = plt.plot(tt, rt, 'g--', label='Reference Command R(t)')
plt.ylabel('x(t) (meters)')
plt.xlabel('Time (seconds)')
#plt.axis([-1, t_end, -.5, 1.5])
# plt.legend(handles=[line1,line2])
plt.legend([line1, line2], loc='best')
plt.show()
dt = time[1] - time[0] # delta t, (sec)
##################################################################################
# Core simulation code
# Inital conditions (i.e., initial state vector)
y = [0, 0]
#y[0] = initial altitude, (m)
#y[1] = initial speed, (m/s)
# Initialize array to store values
soln = np.zeros((len(time), len(y)))
# Create instance of PID_Controller class and initalize all the variables
pid = PID_Controller(kp=kp,
ki=ki,
kd=kd,
max_windup=1e6,
u_bounds=[0, umax],
alpha=alpha)
# Set altitude target
r = 10 # meters
pid.setTarget(r)
# Simulate quadrotor motion
j = 0 # dummy counter
for t in time:
# Evaluate state at next time point
y = ydot(y, t, pid)
# Store results
soln[j, :] = y
j += 1
def set_up_PID_controllers(self):
'''
Setup the PID controllers with the initial values set in the subs
parameter xml server file.
Parameters:
N/A
Returns:
N/A
'''
d_t = float(self.param_serv.get_param("Control/PID/dt"))
roll_p = float(self.param_serv.get_param("Control/PID/roll_pid/p"))
roll_i = float(self.param_serv.get_param("Control/PID/roll_pid/i"))
roll_d = float(self.param_serv.get_param("Control/PID/roll_pid/d"))
self.roll_pid_controller = PID_Controller(roll_p, roll_i, roll_d, d_t)
pitch_p = float(self.param_serv.get_param("Control/PID/pitch_pid/p"))
pitch_i = float(self.param_serv.get_param("Control/PID/pitch_pid/i"))
pitch_d = float(self.param_serv.get_param("Control/PID/pitch_pid/d"))
self.pitch_pid_controller = PID_Controller(pitch_p, pitch_i, pitch_d,
d_t)
yaw_p = float(self.param_serv.get_param("Control/PID/yaw_pid/p"))
yaw_i = float(self.param_serv.get_param("Control/PID/yaw_pid/i"))
yaw_d = float(self.param_serv.get_param("Control/PID/yaw_pid/d"))
self.yaw_pid_controller = PID_Controller(yaw_p, yaw_i, yaw_d, d_t)
x_p = float(self.param_serv.get_param("Control/PID/x_pid/p"))
x_i = float(self.param_serv.get_param("Control/PID/x_pid/i"))
x_d = float(self.param_serv.get_param("Control/PID/x_pid/d"))
self.x_pid_controller = PID_Controller(x_p, x_i, x_d, d_t)
y_p = float(self.param_serv.get_param("Control/PID/y_pid/p"))
y_i = float(self.param_serv.get_param("Control/PID/y_pid/i"))
y_d = float(self.param_serv.get_param("Control/PID/y_pid/d"))
self.y_pid_controller = PID_Controller(y_p, y_i, y_d, d_t)
#z = depth pid
z_p = float(self.param_serv.get_param("Control/PID/z_pid/p"))
z_i = float(self.param_serv.get_param("Control/PID/z_pid/i"))
z_d = float(self.param_serv.get_param("Control/PID/z_pid/d"))
self.z_pid_controller = PID_Controller(z_p, z_i, z_d, d_t)
class Movement_PID:
'''
A movement controller for Perseverance that relies on PID controller to control
the 6 degrees of freedom.
'''
def __init__(self):
'''
Initialize the thrusters and PID controllers on Perseverance.
Parameters:
error_publisher: A MechOS publisher to send the publish the current error
Returns:
N/A
'''
#Get the mechos network parameters
configs = MechOS_Network_Configs(
MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
#Initialize parameter server client to get and set parameters related to
#the PID controller. This includes update time and PID contstants for
#each degree of freedom.
self.param_serv = mechos.Parameter_Server_Client(
configs["param_ip"], configs["param_port"])
self.param_serv.use_parameter_database(configs["param_server_path"])
#Initialize serial connection to the maestro
com_port = self.param_serv.get_param("COM_Ports/maestro")
maestro_serial_obj = serial.Serial(com_port, 9600)
#Initialize all 8 thrusters (max thrust 80%)
max_thrust = float(self.param_serv.get_param("Control/max_thrust"))
self.thrusters = [None, None, None, None, None, None, None, None]
self.thrusters[0] = Thruster(maestro_serial_obj, 1, [0, 0, 1],
[1, -1, 0], max_thrust, True)
self.thrusters[1] = Thruster(maestro_serial_obj, 2, [0, 1, 0],
[1, 0, 0], max_thrust, False)
self.thrusters[2] = Thruster(maestro_serial_obj, 3, [0, 0, 1],
[1, 1, 0], max_thrust, False)
self.thrusters[3] = Thruster(maestro_serial_obj, 4, [1, 0, 0],
[0, 1, 0], max_thrust, False)
self.thrusters[4] = Thruster(maestro_serial_obj, 5, [0, 0, 1],
[-1, 1, 0], max_thrust, True)
self.thrusters[5] = Thruster(maestro_serial_obj, 6, [0, 1, 0],
[-1, 0, 0], max_thrust, False)
self.thrusters[6] = Thruster(maestro_serial_obj, 7, [0, 0, 1],
[-1, -1, 0], max_thrust, False)
self.thrusters[7] = Thruster(maestro_serial_obj, 8, [1, 0, 0],
[0, -1, 0], max_thrust, False)
#Initialize the PID controllers for control system
self.set_up_PID_controllers()
def set_up_PID_controllers(self):
'''
Setup the PID controllers with the initial values set in the subs
parameter xml server file.
Parameters:
N/A
Returns:
N/A
'''
d_t = float(self.param_serv.get_param("Control/PID/dt"))
roll_p = float(self.param_serv.get_param("Control/PID/roll_pid/p"))
roll_i = float(self.param_serv.get_param("Control/PID/roll_pid/i"))
roll_d = float(self.param_serv.get_param("Control/PID/roll_pid/d"))
self.roll_pid_controller = PID_Controller(roll_p, roll_i, roll_d, d_t)
pitch_p = float(self.param_serv.get_param("Control/PID/pitch_pid/p"))
pitch_i = float(self.param_serv.get_param("Control/PID/pitch_pid/i"))
pitch_d = float(self.param_serv.get_param("Control/PID/pitch_pid/d"))
self.pitch_pid_controller = PID_Controller(pitch_p, pitch_i, pitch_d,
d_t)
yaw_p = float(self.param_serv.get_param("Control/PID/yaw_pid/p"))
yaw_i = float(self.param_serv.get_param("Control/PID/yaw_pid/i"))
yaw_d = float(self.param_serv.get_param("Control/PID/yaw_pid/d"))
self.yaw_pid_controller = PID_Controller(yaw_p, yaw_i, yaw_d, d_t)
x_p = float(self.param_serv.get_param("Control/PID/x_pid/p"))
x_i = float(self.param_serv.get_param("Control/PID/x_pid/i"))
x_d = float(self.param_serv.get_param("Control/PID/x_pid/d"))
self.x_pid_controller = PID_Controller(x_p, x_i, x_d, d_t)
y_p = float(self.param_serv.get_param("Control/PID/y_pid/p"))
y_i = float(self.param_serv.get_param("Control/PID/y_pid/i"))
y_d = float(self.param_serv.get_param("Control/PID/y_pid/d"))
self.y_pid_controller = PID_Controller(y_p, y_i, y_d, d_t)
#z = depth pid
z_p = float(self.param_serv.get_param("Control/PID/z_pid/p"))
z_i = float(self.param_serv.get_param("Control/PID/z_pid/i"))
z_d = float(self.param_serv.get_param("Control/PID/z_pid/d"))
self.z_pid_controller = PID_Controller(z_p, z_i, z_d, d_t)
def simple_thrust(self, thrusts):
'''
Individually sets each thruster PWM given the PWMs in the thrusts
list.
Parameters:
thrusts: A list of length 8 containing a thrust value (%) between
[-100, 100]. The list should be in order of the thruster ids.
Returns:
N/A
'''
for thruster_id, thrust in enumerate(thrusts):
self.thrusters[thruster_id].set_thrust(thrust)
def controlled_thrust(self, roll_control, pitch_control, yaw_control,
x_control, y_control, z_control):
'''
Function takes control outputs from calculated by the PID values and applys
them to the 6 degree control matrix to determine the thrust for each thruster
to reach desired point.
Parameters:
roll_control: The control output from the roll pid controller.
pitch_control: The control output from the pitch pid controller.
yaw_control: The control output from the yaw pid controller.
x_control: The control output from the x translation pid controller.
y_control: The control output from the y translation pid controller.
z_control: The control output from the z translation pid controller.
Returns:
N/A
'''
for thruster_id, thruster in enumerate(self.thrusters):
thrust = (roll_control * thruster.orientation[2] * thruster.location[1]) + \
(pitch_control * thruster.orientation[2] * thruster.location[0]) + \
(yaw_control * thruster.orientation[1] * thruster.location[0]) + \
(yaw_control * thruster.orientation[0] * thruster.location[1]) + \
(x_control * thruster.orientation[0]) + \
(y_control * thruster.orientation[1]) + \
(z_control * thruster.orientation[2])
self.thrusters[thruster_id].set_thrust(thrust)
def advance_move(self, curr_roll, curr_pitch, curr_yaw, curr_x_pos,
curr_y_pos, curr_z_pos, desired_roll, desired_pitch,
desired_yaw, desired_x_pos, desired_y_pos, desired_z_pos):
'''
Given the current position and desired positions of the AUV, obtain the pid
control outputs for all 6 degrees of freedom.
Parameters:
curr_roll: Current roll data
curr_pitch Current pitch data
curr_yaw: Current yaw data
curr_x_pos: Current x position
curr_y_pos: Current y position
curr_z_pos: Current z position
desired_roll: Desired roll position
desired_pitch: Desired pitch position
desired_yaw: Desired yaw position
desired_x_pos: Desired x position
desired_y_pos: Desired y position
desired_z_pos: Desired z position
Returns:
N/A
'''
#calculate the error of each degree of freedom
error = [0, 0, 0, 0, 0, 0]
error[0] = desired_roll - curr_roll #roll error
error[1] = desired_pitch - curr_pitch #pitch error
#Calculate yaw error. The logic includes calculating error for choosing shortest angle to travel
if (desired_yaw >= curr_yaw):
yaw_error = desired_yaw - curr_yaw
if (
yaw_error > 180
): #This will choose the shortest angle to take to desred position.
error[2] = yaw_error - 360
else:
error[2] = yaw_error
else:
yaw_error = curr_yaw - desired_yaw
if (abs(yaw_error) > 180):
error[2] = yaw_error + 360
else:
error[2] = yaw_error
#Calculate translation error
error[3] = desired_z_pos - curr_z_pos
error[4] = desired_x_pos - curr_x_pos
error[5] = desired_y_pos - curr_y_pos
roll_control = self.roll_pid_controller.control_step(error[0])
pitch_control = self.pitch_pid_controller.control_step(error[1])
yaw_control = self.yaw_pid_controller.control_step(error[2])
z_control = self.z_pid_controller.control_step(error[3])
x_control = self.x_pid_controller.control_step(error[4])
z_control = self.z_pid_controller.control_step(error[5])
#Write the contrls to thrusters
self.controlled_thrust(roll_control, pitch_control, yaw_control,
x_control, y_control, z_control)
return error
def simple_depth_move_no_yaw(self, curr_roll, curr_pitch, curr_z_pos,
desired_roll, desired_pitch, desired_z_pos):
'''
Without carrying about x axis position, y axis position, or yaw, use the PID controllers
to go down to a give depth (desired_z_pos) with a give orientation.
Parameters:
curr_roll: Current roll data
curr_pitch Current pitch data
curr_z_pos: Current z position
desired_roll: Desired roll position
desired_pitch: Desired pitch position
desired_z_pos: Desired z (depth) position
Returns:
error: The roll, pitch and depth error
'''
#Calculate error for each degree of freedom
error = [0, 0, 0, 0, 0, 0]
error[0] = desired_roll - curr_roll
roll_control = self.roll_pid_controller.control_step(error[0])
error[1] = desired_pitch - curr_pitch
pitch_control = self.pitch_pid_controller.control_step(error[1])
#depth error
print(curr_z_pos, desired_z_pos)
error[2] = desired_z_pos - curr_z_pos
z_control = self.z_pid_controller.control_step(error[2])
#Write controls to thrusters
#Set x, y, and yaw controls to zero since we don't care about the subs
#heading or planar orientation for a simple depth move
self.controlled_thrust(roll_control, pitch_control, 0, 0, 0, z_control)
return error
class Movement_PID:
'''
A movement controller for Perseverance that relies on PID controller to control
the 6 degrees of freedom.
'''
def __init__(self):
'''
Initialize the thrusters and PID controllers on Perseverance.
Parameters:
N/A
Returns:
N/A
'''
#Get the mechos network parameters
configs = MechOS_Network_Configs(
MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
#Initialize parameter server client to get and set parameters related to
#the PID controller. This includes update time and PID contstants for
#each degree of freedom.
self.param_serv = mechos.Parameter_Server_Client(
configs["param_ip"], configs["param_port"])
self.param_serv.use_parameter_database(configs["param_server_path"])
#Initialize serial connection to the maestro
com_port = self.param_serv.get_param("COM_Ports/maestro")
maestro_serial_obj = serial.Serial(com_port, 9600)
#Initialize all 8 thrusters (max thrust 80%)
max_thrust = float(self.param_serv.get_param("Control/max_thrust"))
self.thrusters = [None, None, None, None, None, None, None, None]
self.thrusters[0] = Thruster(maestro_serial_obj, 1, [0, 0, 1],
[1, -1, 0], max_thrust, True)
self.thrusters[1] = Thruster(maestro_serial_obj, 2, [0, 1, 0],
[1, 0, 0], max_thrust, True)
self.thrusters[2] = Thruster(maestro_serial_obj, 3, [0, 0, 1],
[1, 1, 0], max_thrust, False)
self.thrusters[3] = Thruster(maestro_serial_obj, 4, [1, 0, 0],
[0, 1, 0], max_thrust, False)
self.thrusters[4] = Thruster(maestro_serial_obj, 5, [0, 0, 1],
[-1, 1, 0], max_thrust, True)
self.thrusters[5] = Thruster(maestro_serial_obj, 6, [0, 1, 0],
[-1, 0, 0], max_thrust, True)
self.thrusters[6] = Thruster(maestro_serial_obj, 7, [0, 0, 1],
[-1, -1, 0], max_thrust, False)
self.thrusters[7] = Thruster(maestro_serial_obj, 8, [1, 0, 0],
[0, -1, 0], max_thrust, True)
#Used to notify when to hold depth based on the remote control trigger for depth.
self.remote_desired_depth = 0
self.remote_depth_recorded = False
self.remote_yaw_min_thrust = float(
self.param_serv.get_param("Control/Remote/yaw_min"))
self.remote_yaw_max_thrust = float(
self.param_serv.get_param("Control/Remote/yaw_max"))
self.remote_x_min_thrust = float(
self.param_serv.get_param("Control/Remote/x_min"))
self.remote_x_max_thrust = float(
self.param_serv.get_param("Control/Remote/x_max"))
self.remote_y_min_thrust = float(
self.param_serv.get_param("Control/Remote/y_min"))
self.remote_y_max_thrust = float(
self.param_serv.get_param("Control/Remote/y_max"))
#Initialize the PID controllers for control system
self.set_up_PID_controllers(True)
def set_up_PID_controllers(self, initialization=False):
'''
Setup the PID controllers with the initial values set in the subs
parameter xml server file. Also update the strength parameter for
each thrusters.
Parameters:
initialization: If it is the first time that this function is being called,
set initialization to true so it creates the pid controllers.
If false, it just updates the controller values.
Returns:
N/A
'''
d_t = float(self.param_serv.get_param("Control/PID/dt"))
roll_p = float(self.param_serv.get_param("Control/PID/roll_pid/p"))
roll_i = float(self.param_serv.get_param("Control/PID/roll_pid/i"))
roll_d = float(self.param_serv.get_param("Control/PID/roll_pid/d"))
self.roll_min_error = float(
self.param_serv.get_param("Control/PID/roll_pid/min_error"))
self.roll_max_error = float(
self.param_serv.get_param("Control/PID/roll_pid/max_error"))
self.max_roll = float(
self.param_serv.get_param("Control/Limits/max_roll"))
pitch_p = float(self.param_serv.get_param("Control/PID/pitch_pid/p"))
pitch_i = float(self.param_serv.get_param("Control/PID/pitch_pid/i"))
pitch_d = float(self.param_serv.get_param("Control/PID/pitch_pid/d"))
self.pitch_min_error = float(
self.param_serv.get_param("Control/PID/pitch_pid/min_error"))
self.pitch_max_error = float(
self.param_serv.get_param("Control/PID/pitch_pid/max_error"))
self.max_pitch = float(
self.param_serv.get_param("Control/Limits/max_pitch"))
yaw_p = float(self.param_serv.get_param("Control/PID/yaw_pid/p"))
yaw_i = float(self.param_serv.get_param("Control/PID/yaw_pid/i"))
yaw_d = float(self.param_serv.get_param("Control/PID/yaw_pid/d"))
self.yaw_min_error = float(
self.param_serv.get_param("Control/PID/yaw_pid/min_error"))
self.yaw_max_error = float(
self.param_serv.get_param("Control/PID/yaw_pid/max_error"))
x_p = float(self.param_serv.get_param("Control/PID/x_pid/p"))
x_i = float(self.param_serv.get_param("Control/PID/x_pid/i"))
x_d = float(self.param_serv.get_param("Control/PID/x_pid/d"))
self.x_min_error = float(
self.param_serv.get_param("Control/PID/x_pid/min_error"))
self.x_max_error = float(
self.param_serv.get_param("Control/PID/x_pid/max_error"))
y_p = float(self.param_serv.get_param("Control/PID/y_pid/p"))
y_i = float(self.param_serv.get_param("Control/PID/y_pid/i"))
y_d = float(self.param_serv.get_param("Control/PID/y_pid/d"))
self.y_min_error = float(
self.param_serv.get_param("Control/PID/y_pid/min_error"))
self.y_max_error = float(
self.param_serv.get_param("Control/PID/y_pid/max_error"))
#z = depth pid
z_p = float(self.param_serv.get_param("Control/PID/z_pid/p"))
z_i = float(self.param_serv.get_param("Control/PID/z_pid/i"))
z_d = float(self.param_serv.get_param("Control/PID/z_pid/d"))
self.z_min_error = float(
self.param_serv.get_param("Control/PID/z_pid/min_error"))
self.z_max_error = float(
self.param_serv.get_param("Control/PID/z_pid/max_error"))
self.min_z = float(self.param_serv.get_param("Control/Limits/min_z"))
self.max_z = float(self.param_serv.get_param("Control/Limits/max_z"))
#The bias term is a thruster vale to set to thruster 1, 3, 5, 7 to make the sub neutrally bouyant.
#This term is added to the proportional gain controller.
#(K_p * error) + bias
#Essentially this parameter will make the sub act neutrally bouyant below the activate bias depth.
self.z_bias = float(
self.param_serv.get_param("Control/PID/z_pid/bias"))
self.z_active_bias_depth = float(
self.param_serv.get_param("Control/PID/z_pid/active_bias_depth"))
#If running the script with initialization true, create PID_Controller objects
if (initialization):
print("[INFO]: Initializing PID Controllers.")
self.roll_pid_controller = PID_Controller(roll_p, roll_i, roll_d,
d_t)
self.pitch_pid_controller = PID_Controller(pitch_p, pitch_i,
pitch_d, d_t)
self.yaw_pid_controller = PID_Controller(yaw_p, yaw_i, yaw_d, d_t)
self.x_pid_controller = PID_Controller(x_p, x_i, x_d, d_t)
self.y_pid_controller = PID_Controller(y_p, y_i, y_d, d_t)
self.z_pid_controller = PID_Controller(z_p, z_i, z_d, d_t)
else:
print("[INFO]: Updating PID Controller Configurations.")
self.roll_pid_controller.set_gains(roll_p, roll_i, roll_d, d_t)
self.pitch_pid_controller.set_gains(pitch_p, pitch_i, pitch_d, d_t)
self.yaw_pid_controller.set_gains(yaw_p, yaw_i, yaw_d, d_t)
self.x_pid_controller.set_gains(x_p, x_i, x_d, d_t)
self.y_pid_controller.set_gains(y_p, y_i, y_d, d_t)
self.z_pid_controller.set_gains(z_p, z_i, z_d, d_t)
#TODO: Physically balance the sub so that this can be deprecated.
#Thruster Strengths (these are used to give more strengths to weeker thrusters in the case that the sub is imbalanced)
#Each index corresponds to the thruster id.
self.thruster_strengths = [0, 0, 0, 0, 0, 0, 0, 0]
for i in range(8):
param_path = "Control/Thruster_Strengths/T%d" % (i + 1)
self.thruster_strengths[i] = float(
self.param_serv.get_param(param_path))
#Get the depth at which the thruster strength offsets will be used (in ft)
self.thruster_offset_active_depth = float(
self.param_serv.get_param(
"Control/Thruster_Strengths/active_depth"))
def simple_thrust(self, thrusts):
'''
Individually sets each thruster PWM given the PWMs in the thrusts
list.
Parameters:
thrusts: A list of length 8 containing a thrust value (%) between
[-100, 100]. The list should be in order of the thruster ids.
Returns:
N/A
'''
for thruster_id, thrust in enumerate(thrusts):
self.thrusters[thruster_id].set_thrust(thrust)
def controlled_thrust(self, roll_control, pitch_control, yaw_control,
x_control, y_control, z_control, curr_z_pos):
'''
Function takes control outputs from calculated by the PID values and applys
them to the 6 degree control matrix to determine the thrust for each thruster
to reach desired point.
Parameters:
roll_control: The control output from the roll pid controller.
pitch_control: The control output from the pitch pid controller.
yaw_control: The control output from the yaw pid controller.
x_control: The control output from the x translation pid controller.
y_control: The control output from the y translation pid controller.
z_control: The control output from the z translation pid controller.
curr_z_pos: The current depth position of the sub. This is needed to know
when to activate thruster offsets to help hold the sub level.
Returns:
N/A
'''
for thruster_id, thruster in enumerate(self.thrusters):
#Multiplied roll control and x_control by negative one to account for the thrusters going in the
#wrong direction.
#Also only thruster 2 and 6 are used for yaw, achieve better results this way.
#To add in thrusters 4 and 8 for yaw, add the following comment line to the addition.
# (yaw_control * thruster.orientation[0] * thruster.location[1])
thrust = (-1 * roll_control * thruster.orientation[2] * thruster.location[1]) + \
(pitch_control * thruster.orientation[2] * thruster.location[0]) + \
(yaw_control * thruster.orientation[1] * thruster.location[0]) + \
(-1 * x_control * thruster.orientation[0]) + \
(y_control * thruster.orientation[1]) + \
(z_control * thruster.orientation[2])
#Write the thrust to the given thruster. Some thrusters have an additional offset to given them
#a higher strength. This is used to help balance out weigth distribution issues with the sub.
#if(curr_z_pos >= self.thruster_offset_active_depth):
# thrust = thrust + self.thruster_strengths[thruster_id]
#Since the center of mass of the sub is not in the center of the axises of the sub,
#some thruster will need to produce more torque to have movement about that axis,
#be stable.
thrust = thrust + (thrust * self.thruster_strengths[thruster_id])
if (curr_z_pos >= self.z_active_bias_depth):
thrust = thrust + (
self.z_bias * thruster.orientation[2]
) #Make sure only thrusters controlling z have this parameter
self.thrusters[thruster_id].set_thrust(thrust)
def bound_error(self, unbounded_error, min_error, max_error):
'''
This function will check if the unbounded error is within the range
of the max and min error. If it is not, it will force it to be.
Parameter:
unbounded_error: The error that has not yet been checked if it
is within the error limits
min_error: The minimum value that you want your error to be.
max_error: The maximum value that you want your error to be.
Returns:
N/A
'''
if (unbounded_error < min_error):
unbounded_error = min_error
return (unbounded_error)
elif (unbounded_error > max_error):
unbounded_error = max_error
return (unbounded_error)
else:
return (unbounded_error)
def advance_move(self, current_position, desired_position):
'''
Given the current position and desired positions of the AUV, obtain the pid
control outputs for all 6 degrees of freedom.
Parameters:
current_position: A list of the most up-to-date current position.
List format: [roll, pitch, yaw, x_pos, y_pos, depth]
desired_position: A list containing the desired positions of each
axis. Same list format as the current_position
parameter.
Returns:
error: A list of of the errors that where evaluted for each axis.
List Format: [roll_error, pitch_error, yaw_error, x_pos_error, y_pos_error, depth_error]
'''
#calculate the error of each degree of freedom
error = [0, 0, 0, 0, 0, 0]
#Make sure roll is within maximum limit
if (abs(desired_position[0]) > self.max_roll):
desired_position[0] = math.copysign(self.max_roll,
desired_position[0])
print(
"[WARNING]: Absolute of desired roll %0.2f is greater than the max limit %0.2f"
% (desired_position[0], self.max_roll))
error[0] = self.bound_error(desired_position[0] - current_position[0],
self.roll_min_error,
self.roll_max_error) #roll error
#Make sure pitch is within maximum limit
if (abs(desired_position[1]) > self.max_pitch):
desired_position[1] = math.copysign(self.max_pitch,
desired_position[1])
print(
"[WARNING]: Absolute of desired pitch %0.2f is greater than the max limit %0.2f"
% (desired_position[0], self.max_pitch))
error[1] = self.bound_error(desired_position[1] - current_position[1],
self.pitch_min_error,
self.pitch_max_error) #pitch error
#Calculate yaw error. The logic includes calculating error for choosing shortest angle to travel
desired_yaw = desired_position[2]
curr_yaw = current_position[2]
yaw_error = desired_yaw - curr_yaw
if (abs(yaw_error) > 180):
if (yaw_error < 0):
yaw_error = yaw_error + 360
else:
yaw_error = yaw_error - 360
error[2] = self.bound_error(yaw_error, self.yaw_min_error,
self.yaw_max_error)
#Calculate the error in the x and y position (relative to the sub) given the current north/east position.
#Using rotation matrix.
#North and East error
north_error = desired_position[3] - current_position[3]
east_error = desired_position[4] - current_position[4]
yaw_rad = math.radians(curr_yaw) #convert yaw from degrees to radians
x_error = (math.cos(yaw_rad) * north_error) + (math.sin(yaw_rad) *
east_error)
y_error = (-1 * math.sin(yaw_rad) * north_error) + (math.cos(yaw_rad) *
east_error)
error[3] = self.bound_error(x_error, self.x_min_error,
self.x_max_error)
error[4] = self.bound_error(y_error, self.y_min_error,
self.y_max_error)
if (desired_position[5] < self.min_z):
print(
"[WARNING]: Desired depth %0.2f is less than the min limit %0.2f"
% (desired_position[5], self.min_z))
desired_position[5] = self.min_z
elif (desired_position[5] > self.max_z):
print(
"[WARNING]: Desired depth %0.2f is greater than the max limit %0.2f"
% (desired_position[5], self.max_z))
desired_position[5] = self.max_z
error[5] = self.bound_error(desired_position[5] - current_position[5],
self.z_min_error,
self.z_max_error) #z_pos (depth)
#Get the thrusts from the PID controllers to move towards desired pos.
roll_control = self.roll_pid_controller.control_step(error[0])
pitch_control = self.pitch_pid_controller.control_step(error[1])
yaw_control = self.yaw_pid_controller.control_step(error[2])
x_control = self.x_pid_controller.control_step(error[3])
y_control = self.y_pid_controller.control_step(error[4])
z_control = self.z_pid_controller.control_step(error[5])
#Write the controls to thrusters
self.controlled_thrust(roll_control, pitch_control, yaw_control,
x_control, y_control, z_control,
current_position[5])
return error
def remote_move(self, current_position, remote_commands):
'''
Accepts spoofed error input for each degree of freedom from the xbox controller
to utilize the pid controllers for remote control movement.
Parameters:
current_position: A list of the most up-to-date current position.
List format: [roll, pitch, yaw, x_pos, y_pos, depth]
remote_commands: A list of the spoofed errors for [yaw, x_pos, y_pos, depth] to
utilize the remote control to perform movement with the PID
controller.
Returns:
N/A
'''
#always want roll and pitch to be level at 0 degrees
roll_error = self.bound_error(0.0 - current_position[0],
self.roll_min_error,
self.roll_max_error) #roll error
pitch_error = self.bound_error(0.0 - current_position[1],
self.pitch_min_error,
self.pitch_max_error) #pitch error
#Interpolate errors to the min and max errors set in the parameter server
yaw_control = np.interp(
remote_commands[0], [-1, 1],
[self.remote_yaw_min_thrust, self.remote_yaw_max_thrust])
x_control = np.interp(
remote_commands[1], [-1, 1],
[self.remote_x_min_thrust, self.remote_x_max_thrust])
y_control = np.interp(
remote_commands[2], [-1, 1],
[self.remote_y_min_thrust, self.remote_y_max_thrust])
#When the trigger is released for controlling depth, record the depth and hold.
if (remote_commands[4]):
if (self.remote_depth_recorded == False):
self.remote_desired_depth = current_position[5]
self.remote_depth_recorded = True
depth_error = self.bound_error(self.remote_desired_depth - current_position[5], \
self.z_min_error, self.z_max_error) #z_pos (depth)
else:
self.remote_depth_recorded = False
self.remote_desired_depth = 0
#Since the min and max error is not the same in terms of absolute value,
#interpolation needs to be handled seperate for up and down movements.
if (remote_commands[3] <= 0.0):
depth_error = np.interp(remote_commands[3], [-1, 0],
[self.z_min_error, 0])
else:
depth_error = np.interp(remote_commands[3], [0, 1],
[0, self.z_max_error])
#Get the thrusts from the PID controllers to move towards desired pos.
roll_control = self.roll_pid_controller.control_step(roll_error)
pitch_control = self.pitch_pid_controller.control_step(pitch_error)
z_control = self.z_pid_controller.control_step(depth_error)
self.controlled_thrust(roll_control, pitch_control, yaw_control,
-1 * x_control, y_control, z_control,
current_position[5])
return
#This is a helper function to be used initially for tuning the roll, pitch
#and depth PID values. Once tuned, please use advance_move instead.
def simple_depth_move_no_yaw(self, curr_roll, curr_pitch, curr_z_pos,
desired_roll, desired_pitch, desired_z_pos):
'''
Without carrying about x axis position, y axis position, or yaw, use the PID controllers
to go down to a give depth (desired_z_pos) with a give orientation.
Parameters:
curr_roll: Current roll data
curr_pitch Current pitch data
curr_z_pos: Current z position
desired_roll: Desired roll position
desired_pitch: Desired pitch position
desired_z_pos: Desired z (depth) position
Returns:
error: The roll, pitch and depth error
'''
#Calculate error for each degree of freedom
error = [0, 0, 0, 0, 0, 0]
error[0] = desired_roll - curr_roll
roll_control = self.roll_pid_controller.control_step(error[0])
error[1] = desired_pitch - curr_pitch
pitch_control = self.pitch_pid_controller.control_step(error[1])
#depth error
error[2] = desired_z_pos - curr_z_pos
z_control = self.z_pid_controller.control_step(error[2])
#Write controls to thrusters
#Set x, y, and yaw controls to zero since we don't care about the subs
#heading or planar orientation for a simple depth move
self.controlled_thrust(roll_control, pitch_control, 0, 0, 0, z_control,
curr_z_pos)
return error
def set_up_PID_controllers(self, initialization=False):
'''
Setup the PID controllers with the initial values set in the subs
parameter xml server file. Also update the strength parameter for
each thrusters.
Parameters:
initialization: If it is the first time that this function is being called,
set initialization to true so it creates the pid controllers.
If false, it just updates the controller values.
Returns:
N/A
'''
d_t = float(self.param_serv.get_param("Control/PID/dt"))
roll_p = float(self.param_serv.get_param("Control/PID/roll_pid/p"))
roll_i = float(self.param_serv.get_param("Control/PID/roll_pid/i"))
roll_d = float(self.param_serv.get_param("Control/PID/roll_pid/d"))
self.roll_min_error = float(
self.param_serv.get_param("Control/PID/roll_pid/min_error"))
self.roll_max_error = float(
self.param_serv.get_param("Control/PID/roll_pid/max_error"))
self.max_roll = float(
self.param_serv.get_param("Control/Limits/max_roll"))
pitch_p = float(self.param_serv.get_param("Control/PID/pitch_pid/p"))
pitch_i = float(self.param_serv.get_param("Control/PID/pitch_pid/i"))
pitch_d = float(self.param_serv.get_param("Control/PID/pitch_pid/d"))
self.pitch_min_error = float(
self.param_serv.get_param("Control/PID/pitch_pid/min_error"))
self.pitch_max_error = float(
self.param_serv.get_param("Control/PID/pitch_pid/max_error"))
self.max_pitch = float(
self.param_serv.get_param("Control/Limits/max_pitch"))
yaw_p = float(self.param_serv.get_param("Control/PID/yaw_pid/p"))
yaw_i = float(self.param_serv.get_param("Control/PID/yaw_pid/i"))
yaw_d = float(self.param_serv.get_param("Control/PID/yaw_pid/d"))
self.yaw_min_error = float(
self.param_serv.get_param("Control/PID/yaw_pid/min_error"))
self.yaw_max_error = float(
self.param_serv.get_param("Control/PID/yaw_pid/max_error"))
x_p = float(self.param_serv.get_param("Control/PID/x_pid/p"))
x_i = float(self.param_serv.get_param("Control/PID/x_pid/i"))
x_d = float(self.param_serv.get_param("Control/PID/x_pid/d"))
self.x_min_error = float(
self.param_serv.get_param("Control/PID/x_pid/min_error"))
self.x_max_error = float(
self.param_serv.get_param("Control/PID/x_pid/max_error"))
y_p = float(self.param_serv.get_param("Control/PID/y_pid/p"))
y_i = float(self.param_serv.get_param("Control/PID/y_pid/i"))
y_d = float(self.param_serv.get_param("Control/PID/y_pid/d"))
self.y_min_error = float(
self.param_serv.get_param("Control/PID/y_pid/min_error"))
self.y_max_error = float(
self.param_serv.get_param("Control/PID/y_pid/max_error"))
#z = depth pid
z_p = float(self.param_serv.get_param("Control/PID/z_pid/p"))
z_i = float(self.param_serv.get_param("Control/PID/z_pid/i"))
z_d = float(self.param_serv.get_param("Control/PID/z_pid/d"))
self.z_min_error = float(
self.param_serv.get_param("Control/PID/z_pid/min_error"))
self.z_max_error = float(
self.param_serv.get_param("Control/PID/z_pid/max_error"))
self.min_z = float(self.param_serv.get_param("Control/Limits/min_z"))
self.max_z = float(self.param_serv.get_param("Control/Limits/max_z"))
#The bias term is a thruster vale to set to thruster 1, 3, 5, 7 to make the sub neutrally bouyant.
#This term is added to the proportional gain controller.
#(K_p * error) + bias
#Essentially this parameter will make the sub act neutrally bouyant below the activate bias depth.
self.z_bias = float(
self.param_serv.get_param("Control/PID/z_pid/bias"))
self.z_active_bias_depth = float(
self.param_serv.get_param("Control/PID/z_pid/active_bias_depth"))
#If running the script with initialization true, create PID_Controller objects
if (initialization):
print("[INFO]: Initializing PID Controllers.")
self.roll_pid_controller = PID_Controller(roll_p, roll_i, roll_d,
d_t)
self.pitch_pid_controller = PID_Controller(pitch_p, pitch_i,
pitch_d, d_t)
self.yaw_pid_controller = PID_Controller(yaw_p, yaw_i, yaw_d, d_t)
self.x_pid_controller = PID_Controller(x_p, x_i, x_d, d_t)
self.y_pid_controller = PID_Controller(y_p, y_i, y_d, d_t)
self.z_pid_controller = PID_Controller(z_p, z_i, z_d, d_t)
else:
print("[INFO]: Updating PID Controller Configurations.")
self.roll_pid_controller.set_gains(roll_p, roll_i, roll_d, d_t)
self.pitch_pid_controller.set_gains(pitch_p, pitch_i, pitch_d, d_t)
self.yaw_pid_controller.set_gains(yaw_p, yaw_i, yaw_d, d_t)
self.x_pid_controller.set_gains(x_p, x_i, x_d, d_t)
self.y_pid_controller.set_gains(y_p, y_i, y_d, d_t)
self.z_pid_controller.set_gains(z_p, z_i, z_d, d_t)
#TODO: Physically balance the sub so that this can be deprecated.
#Thruster Strengths (these are used to give more strengths to weeker thrusters in the case that the sub is imbalanced)
#Each index corresponds to the thruster id.
self.thruster_strengths = [0, 0, 0, 0, 0, 0, 0, 0]
for i in range(8):
param_path = "Control/Thruster_Strengths/T%d" % (i + 1)
self.thruster_strengths[i] = float(
self.param_serv.get_param(param_path))
#Get the depth at which the thruster strength offsets will be used (in ft)
self.thruster_offset_active_depth = float(
self.param_serv.get_param(
"Control/Thruster_Strengths/active_depth"))
class TBC_Controller:
def __init__(self, PCR_Machine, Peltier, start_time=0, update_period=0.05, volume=10):
self.pcr = PCR_Machine
self.peltier = Peltier
self.time = self.checkpoint = self.start_time = start_time
self.period = update_period
self.volume = volume
self.set_point = self.pcr.sample_temp
self.stage = "Hold"
self.max_block_temp = 109
self.max_ramp_dist = 35
self.unachievable = 10
self.smpWinInRampUpFlag = False
self.smpWinInRampDownFlag = False
self.init_pid()
self.load_tuning_params()
self.pid.load(self.pid_const, "Hold")
self.max_up_ramp = self.calc_poly_eqn(self.upRrEqn, self.volume)
self.max_down_ramp = self.calc_poly_eqn(self.downRrEqn, self.volume)
def reset(self, set_point=60):
self.pid.reset()
self.start_time = self.time = 0
self.time_elapsed = 0
self.set_point = set_point
self.smpWinInRampUpFlag = False
self.smpWinInRampDownFlag = False
def calc_poly_eqn(self, eqn, vol):
return sum([vol**deg * coeff for deg, coeff in enumerate(eqn)])
def init_pid(self):
self.pid = PID_Controller()
self.pid.dt = self.period / 60 # dt is in minute
self.pid2 = PID_Controller()
self.pid2.dt = self.period / 60 # dt is in minute
def load_tuning_params(self):
self.blockMCP = 10.962
self.upRrEqn = [
3.95293071,
-0.03074861,
0.00008966,
0.0
]
self.downRrEqn = [
3.03500389,
-0.02403729,
0.00008891,
0.0
]
self.pid_const = {}
self.pid_const["Ramp Up" ] = {
"P" : 2.5,
"I" : 0.1,
"D" : 0.0001,
"KI" : 10,
"KD" : 20
}
self.pid_const["Overshoot Over" ] = {
"P" : 6,
"I" : 0.1,
"D" : 0.0,
"KI" : 2,
"KD" : 5
}
self.pid_const["Hold Over" ] = {
"P" : 2,
"I" : 0.02,
"D" : 0.01,
"KI" : 5,
"KD" : 10
}
self.pid_const["Land Over" ] = {
"P" : 7,
"I" : 0.05,
"D" : 0.00,
"KI" : 2,
"KD" : 5
}
self.pid_const["Hold" ] = {
"P" : 0.7,
"I" : 0.02,
"D" : 0.01,
"KI" : 5,
"KD" : 10
}
self.pid_const["Ramp Down" ] = {
"P" : 2.5,
"I" : 0.05,
"D" : 0.0001,
"KI" : 10,
"KD" : 20
}
self.pid_const["Overshoot Under"] = {
"P" : 3,
"I" : 0.1,
"D" : 0.00,
"KI" : 2,
"KD" : 5
}
self.pid_const["Hold Under" ] = {
"P" : 1.2,
"I" : 0.02,
"D" : 0.01,
"KI" : 5,
"KD" : 10
}
self.pid_const["Land Under" ] = {
"P" : 7,
"I" : 0.1,
"D" : 0.00,
"KI" : 2,
"KD" : 5
}
self.slow_upramp_qpid = -15
self.block_slow_rate = 0.5
self.sample_slow_rate = 0.0226
self.rampup_minP = 3
self.rampdown_minP = 3
self.smoothRegionOverRR = 1.6
self.smoothRegionUnderRR = 1.5
self.smoothRegionOverWin = 1.5
self.smoothRegionUnderWin = 1.75
if self.volume <= 5:
pass
elif self.volume <= 10:
self.maxHeatBlkOS = 2.8
self.maxCoolBlkOS = 3.4
self.maxHeatSmpWin = 2
self.maxCoolSmpWin = 2
self.qMaxHoldPid = 40
self.qMaxRampPid = 150
self.maxHeatIset = 6.5
self.maxCoolIset = 6.5
self.maxHeatBlkWin = 4
self.maxCoolBlkWin = 3
self.qHeatLoss = 10
self.heat_brake_const = 1.2
self.heatSpCtrlActivRR = 0.2
self.heatSpCtrlActivSP = 0.1
self.cool_brake_const = 1.5
self.coolSpCtrlActivRR = 0.3
self.coolSpCtrlActivSP =-0.1
elif self.volume <= 30:
pass
elif self.volume <= 50:
pass
else:
pass
def update_feedback(self):
# update pid process variable (PV)
if self.stage == "Ramp Up" or self.stage == "Ramp Down":
self.pid.PV = self.pcr.sample_rate
elif self.stage == "Overshoot Over" or self.stage == "Overshoot Under":
if self.smpWinInRampDownFlag or self.smpWinInRampUpFlag:
self.pid.PV = self.pcr.sample_rate
else:
self.pid.PV = self.pcr.block_rate
self.pid2.PV = self.pcr.block_temp * 0.5
elif self.stage == "Land Over" or self.stage == "Land Under":
self.pid.PV = self.pcr.block_rate
else:
self.pid.PV = self.pcr.block_temp * 0.5
self.time_elapsed = self.time - self.start_time
def prepare_overshoot_over(self):
self.pid.reset()
self.pid.load(self.pid_const, "Overshoot Over")
self.pid2.reset()
self.pid2.load(self.pid_const, "Hold Over")
if self.pcr.block_temp >= self.max_block_temp or self.set_point + self.calcHeatBlkOS >= self.max_block_temp:
self.pid2.SP = self.max_block_temp * 0.5
else:
self.pid2.SP = (self.set_point + self.calcHeatBlkOS) * 0.5
self.pid2.ffwd = self.qHeatLoss / self.qMaxRampPid * 100
self.pid2.y = self.pcr.block_temp * 0.5
self.stage = "Overshoot Over"
self.spCtrlFirstActFlag = False
self.rampUpStageRampTime = self.time_elapsed
def prepare_hold(self):
self.stage = "Hold"
self.pid.reset()
self.pid.load(self.pid_const, "Hold")
self.pid.SP = self.set_point * 0.5
self.pid.y = self.pcr.block_temp * 0.5
self.pid.ffwd = self.qHeatLoss / self.qMaxHoldPid * 100
def calcBlockRate(self, timeConst):
# assume ramp_time and ramp_dist are calculated before calling this function
if self.ramp_time == 0 or timeConst == 0:
return self.target_sample_rate
return self.ramp_dist / (self.ramp_time - timeConst * (1 - exp(-self.ramp_time / timeConst)))
def prepare_ramp_up(self):
self.stage = "Ramp Up"
self.target_block_rate = self.calcBlockRate(self.pcr.heat_const)
self.pid.load(self.pid_const, "Ramp Up")
self.pid.SP = self.target_sample_rate
initial_qpid = self.blockMCP * self.target_block_rate
self.pid.ffwd = initial_qpid / self.qMaxRampPid * 100
if self.target_block_rate < self.block_slow_rate:
self.pid.ffwd = self.slow_upramp_qpid / self.qMaxRampPid * 100
if self.pid.ffwd > 100:
self.pid.ffwd = 100
overshoot_dt_const = (self.ramp_dist - 2) / (self.max_ramp_dist - 2)
overshoot_rr_const = 1 if self.target_sample_rate >= self.max_up_ramp else self.target_sample_rate / self.max_up_ramp
if overshoot_dt_const > 1:
blkOS_const = overshoot_rr_const
smpWin_const = overshoot_rr_const
else:
blkOS_const = overshoot_dt_const * overshoot_rr_const
smpWin_const = overshoot_dt_const * overshoot_rr_const
self.calcHeatBlkOS = blkOS_const * self.maxHeatBlkOS
self.calcHeatSmpWin = smpWin_const * self.maxHeatSmpWin
self.calcHeatBlkWin = overshoot_rr_const * self.maxHeatBlkWin
self.smpWinInRampUpFlag = False
def prepare_ramp_down(self):
self.stage = "Ramp Down"
self.target_block_rate = self.calcBlockRate(self.pcr.cool_const) # target_block_rate is negative
self.pid.load(self.pid_const, "Ramp Down")
self.pid.SP = self.target_sample_rate
if abs(self.target_block_rate) < self.block_slow_rate:
initial_qpid = self.blockMCP * self.block_slow_rate
else:
initial_qpid = self.blockMCP * self.target_block_rate
self.pid.ffwd = -initial_qpid / self.qMaxRampPid * 100
if self.pid.ffwd < -100:
self.pid.ffwd = -100
overshoot_dt_const = (self.ramp_dist - 2)/ (self.max_ramp_dist - 2)
if abs(self.target_sample_rate) <= self.max_down_ramp:
overshoot_rr_const = 1
else:
overshoot_rr_const = self.target_sample_rate / self.max_down_ramp
if overshoot_dt_const > 1:
blkOS_const = overshoot_rr_const
else:
blkOS_const = overshoot_dt_const * overshoot_rr_const
smpWin_const = overshoot_dt_const * overshoot_rr_const
self.calcCoolBlkOS = blkOS_const * self.maxCoolBlkOS
self.calcCoolSmpWin = smpWin_const * self.maxCoolSmpWin
self.calcCoolBlkWin = overshoot_rr_const * self.maxCoolBlkWin
def ctrl_ramp_up(self):
if self.pcr.block_temp < self.set_point \
and self.pcr.block_temp + self.calcHeatBlkWin <= self.set_point + self.calcHeatBlkOS:
if self.time_elapsed >= self.ramp_time:
self.pid.SP = self.unachievable
else:
self.pid.SP = (self.set_point - self.pcr.sample_temp) / (self.ramp_time - self.time_elapsed)
if self.pid.SP > 1.05 * self.target_sample_rate:
self.pid.SP = 1.05 * self.target_sample_rate
if self.target_block_rate <= self.block_slow_rate:
if self.target_sample_rate / self.max_up_ramp < 0.1:
stash = self.pid.P
factor = self.target_sample_rate / self.max_up_ramp * 10
self.pid.P *= factor
if self.pid.P < self.rampup_minP:
self.pid.P = self.rampup_minP
self.qpid = self.qMaxRampPid * self.pid.update() * 0.01
self.pid.P = stash
else:
self.qpid = self.qMaxRampPid * self.pid.update() * 0.01
else:
self.qpid = self.qMaxRampPid * self.pid.update() * 0.01
else: # block temp. has overshot the set point
if self.pcr.sample_rate <= self.sample_slow_rate:
self.prepare_hold()
else:
self.smpWinInRampUpFlag = False
overshoot_gap = self.set_point + self.calcHeatBlkOS - self.pcr.block_temp
if self.pcr.block_rate > self.pcr.sample_rate:
time_to_maxOS = overshoot_gap / self.pcr.block_rate
self.rampUpStageRate = self.pcr.block_rate
else:
time_to_maxOS = overshoot_gap / self.pcr.sample_rate
self.rampUpStageRate = self.pcr.sample_rate
if self.calcHeatBlkOS > self.calcHeatBlkWin:
self.heat_brake = self.heat_brake_const * self.calcHeatBlkOS / time_to_maxOS
else:
self.heat_brake = self.heat_brake_const * self.calcHeatBlkWin / time_to_maxOS
self.prepare_overshoot_over()
if self.pcr.sample_temp >= self.set_point - self.calcHeatSmpWin:
if self.pid.PV <= 0:
return
self.smpWinInRampUpFlag = True
time_to_setpoint = (self.set_point - self.pcr.sample_temp) / self.pid.PV
self.heat_brake = self.heat_brake_const * self.calcHeatSmpWin / time_to_setpoint
self.rampUpStageRate = self.pid.PV
self.prepare_overshoot_over()
self.peltier.mode = "heat"
def prepare_hold_over(self):
if self.pcr.block_temp >= self.max_block_temp:
self.pid.SP = self.max_block_temp * 0.5
else:
self.pid.SP = (self.set_point + self.calcHeatBlkOS) * 0.5
if self.pid.SP > self.max_block_temp * 0.5:
self.pid.SP = self.max_block_temp * 0.5
self.pid.m = self.pid2.m
self.pid.b = self.pid2.b
self.pid.y = self.pid2.y
self.pid.ffwd = self.qHeatLoss / self.qMaxHoldPid * 100
self.pid.load(self.pid_const, "Hold Over")
self.stage = "Hold Over"
def ctrl_overshoot_over(self):
if not self.smpWinInRampUpFlag:
if self.pcr.sample_temp >= self.set_point - self.calcHeatSmpWin:
self.prepare_hold()
return
try:
if self.calcHeatBlkOS >= self.calcHeatBlkWin:
tempSP = self.rampUpStageRate * exp(-self.heat_brake * (self.time_elapsed - self.rampUpStageRampTime) / self.calcHeatBlkOS)
else:
tempSP = self.rampUpStageRate * exp(-self.heat_brake * (self.time_elapsed - self.rampUpStageRampTime) / self.calcHeatBlkWin)
except:
tempSP = 0
else:
try:
if self.calcHeatSmpWin != 0:
tempSP = self.rampUpStageRate * exp(-self.heat_brake * (self.time_elapsed - self.rampUpStageRampTime) / self.calcHeatSmpWin)
else:
tempSP = 0
except:
tempSP = 0
if self.pcr.sample_temp >= self.set_point - 0.2 or self.pcr.block_temp > self.set_point:
tempSP = self.pid.b
temp_b = self.pid2.b
temp_ffwd = self.pid2.ffwd
temp_y = self.pid2.y
self.prepare_hold()
self.pid.b = temp_b + tempSP
self.pid.m = self.pid.b
self.pid.b -= temp_ffwd
self.pid.y = temp_y
return
self.pid.SP = tempSP
qPower = tempSP / self.rampUpStageRate * self.qMaxRampPid + (1 - tempSP / self.rampUpStageRate) * self.qMaxHoldPid
self.pid.ffwd = self.pid.SP * self.blockMCP / qPower * 100
self.qpid = qPower * self.pid.update() / 100
if self.pid.SP <= self.heatSpCtrlActivRR * self.rampUpStageRate \
or self.pid.SP <= self.heatSpCtrlActivSP:
if not self.spCtrlFirstActFlag:
self.pid2.y = self.pcr.block_temp * 0.5
self.spCtrlFirstActFlag = True
self.qpid += qPower * self.pid2.update() / 100
if self.pcr.block_temp >= self.set_point + self.calcHeatBlkOS \
or self.pcr.block_temp >= self.max_block_temp:
self.prepare_hold_over()
self.peltier.mode = "heat"
def prepare_land_over(self):
self.pid.reset()
self.pid.load(self.pid_const, "Land Over")
self.stage = "Land Over"
def ctrl_hold_over(self):
self.qpid = self.qMaxHoldPid * self.pid.update() / 100
if self.pcr.sample_temp >= self.set_point - self.calcHeatSmpWin:
self.prepare_land_over()
self.peltier.mode = "heat"
def ctrl_land_over(self):
timeToSetPt = (self.set_point - self.pcr.sample_temp) / self.pcr.sample_rate
if timeToSetPt > 0:
self.pid.SP = (self.set_point - self.pcr.block_temp) / timeToSetPt
else:
self.pid.SP = -3
if self.pcr.block_temp - self.smoothRegionOverWin <= self.set_point:
if self.pid.SP < -self.smoothRegionOverRR:
self.pid.SP = -self.smoothRegionOverRR
self.pid.ffwd = self.pid.SP * self.blockMCP / self.qMaxRampPid * 100
self.qpid = -self.qMaxRampPid * self.pid.update() / 100
if self.pcr.block_temp - 0.25 <= self.set_point:
self.prepare_hold()
self.peltier.mode = "cool"
def ctrl_ramp_down(self):
if self.pcr.block_temp > self.set_point \
and self.pcr.block_temp - self.calcCoolBlkWin >= self.set_point - self.calcCoolBlkOS:
if self.time_elapsed >= self.ramp_time:
self.pid.SP = -self.unachievable
else:
self.pid.SP = (self.set_point - self.pcr.sample_temp) / (self.ramp_time - self.time_elapsed)
if self.pid.SP < -1.05 * self.target_sample_rate:
self.pid.SP = -1.05 * self.target_sample_rate
if self.target_block_rate <= self.block_slow_rate:
if self.target_sample_rate / self.max_down_ramp < 0.1:
stash = self.pid.P
factor = self.target_sample_rate / self.max_down_ramp * 10
self.pid.P *= factor
if self.pid.P < self.rampdown_minP:
self.pid.P = self.rampdown_minP
self.qpid = -self.qMaxRampPid * self.pid.update() * 0.01
self.pid.P = stash
else:
self.qpid = -self.qMaxRampPid * self.pid.update() * 0.01
else:
self.qpid = -self.qMaxRampPid * self.pid.update() * 0.01
else: # block temp. has overshot the set point
if self.pcr.sample_rate >= -self.sample_slow_rate:
self.prepare_hold()
else:
self.smpWinInRampDownFlag = False
overshoot_gap = self.set_point - self.calcCoolBlkOS - self.pcr.block_temp
if self.pcr.block_rate < self.pcr.sample_rate:
time_to_maxOS = overshoot_gap / self.pcr.block_rate
self.rampDownStageRate = abs(self.pcr.block_rate)
else:
time_to_maxOS = overshoot_gap / self.pcr.sample_rate
self.rampDownStageRate = abs(self.pcr.sample_rate)
if self.calcCoolBlkOS > self.calcCoolBlkWin:
self.cool_brake = self.cool_brake_const * self.calcCoolBlkOS / time_to_maxOS
else:
self.cool_brake = self.cool_brake_const * self.calcCoolBlkWin / time_to_maxOS
self.prepare_overshoot_under()
if self.pcr.sample_temp <= self.set_point + self.calcCoolSmpWin:
if self.pid.PV >= 0:
return
self.smpWinInRampDownFlag = True
time_to_setpoint = (self.set_point - self.pcr.sample_temp) / self.pid.PV
self.cool_brake = self.cool_brake_const * self.calcCoolSmpWin / time_to_setpoint
self.rampDownStageRate = self.pid.PV
self.prepare_overshoot_under()
self.peltier.mode = "cool"
def prepare_overshoot_under(self):
self.rampDownStageRampTime = self.time_elapsed
self.pid.load(self.pid_const, "Overshoot Under")
self.pid2.reset()
self.pid2.load(self.pid_const, "Hold Under")
self.pid2.SP = (self.set_point - self.calcCoolBlkOS) * 0.5
self.pid2.ffwd = -self.qHeatLoss / self.qMaxHoldPid * 100
self.pid2.y = self.pcr.block_temp * 0.5
self.spCtrlFirstActFlag = False
self.stage = "Overshoot Under"
def prepare_land_under(self):
self.pid.reset()
self.pid.load(self.pid_const, "Land Under")
self.stage = "Land Under"
def ctrl_overshoot_under(self):
if self.smpWinInRampDownFlag == False:
if self.calcCoolBlkOS >= self.calcCoolBlkWin:
tempSP = self.rampDownStageRate * exp(-self.cool_brake / self.calcCoolBlkOS * (self.time_elapsed - self.rampDownStageRampTime))
else:
tempSP = self.rampDownStageRate * exp(-self.cool_brake / self.calcCoolBlkWin * (self.time_elapsed - self.rampDownStageRampTime))
if self.pcr.sample_temp <= self.set_point + self.calcCoolSmpWin:
self.prepare_land_under()
return
else:
if self.calcCoolSmpWin != 0:
tempSP = self.rampDownStageRate * exp(-self.cool_brake / self.calcCoolSmpWin *(self.time_elapsed - self.rampDownStageRampTime))
else:
tempSP = 0
if self.pcr.sample_temp <= self.set_point + 1:
self.prepare_hold()
self.pid.m = self.pid2.m
self.pid.b = self.pid2.b
self.pid.y = self.pid2.y
return
qPower = (tempSP / self.rampDownStageRate) * self.qMaxRampPid
qPower += (1 - tempSP / self.rampDownStageRate) * self.qMaxHoldPid
self.pid.SP = -tempSP
self.pid.ffwd = self.pid.SP * self.blockMCP / qPower * 100
self.qpid = -qPower * self.pid.update() / 100
if self.pid.SP >= self.coolSpCtrlActivRR * self.rampDownStageRate \
or self.pid.SP >= self.coolSpCtrlActivSP:
if self.spCtrlFirstActFlag == False:
self.pid2.y = self.pcr.block_temp * 0.5
self.spCtrlFirstActFlag = True
self.qpid -= qPower * self.pid2.update() / 100
if self.pcr.block_temp <= self.set_point - self.calcCoolBlkOS:
self.prepare_hold_under()
self.peltier.mode = "cool"
def ctrl_land_under(self):
timeToSetPt = (self.set_point - self.pcr.sample_temp) / self.pcr.sample_rate
if timeToSetPt < 0:
self.pid.SP = (self.set_point - self.pcr.block_temp) / timeToSetPt
else:
self.pid.SP = 3
if self.pcr.block_temp + self.smoothRegionOverWin >= self.set_point:
if self.pid.SP > self.smoothRegionOverRR:
self.pid.SP = self.smoothRegionOverRR
self.pid.ffwd = self.pid.SP * self.blockMCP / self.qMaxRampPid * 100
self.qpid = self.qMaxRampPid * self.pid.update() / 100
if self.pcr.block_temp + 0.25 >= self.set_point:
self.prepare_hold()
self.peltier.mode = "heat"
def ctrl_hold_under(self):
self.qpid = self.qMaxHoldPid * self.pid.update() / 100
if self.pcr.sample_temp <= self.set_point + self.calcCoolSmpWin:
self.prepare_land_under()
self.peltier.mode = "heat"
def ctrl_hold(self):
self.qpid = self.qMaxHoldPid * self.pid.update() / 100
self.peltier.mode = "heat"
def prepare_hold_under(self):
self.pid.reset()
self.pid.load(self.pid_const, "Hold Under")
self.pid.SP = (self.set_point - self.calcCoolBlkOS) * 0.5
self.pid.m = self.pid2.m
self.pid.b = self.pid2.b
self.pid.y = self.pid2.y
self.pid.ffwd = -self.qHeatLoss / self.qMaxHoldPid * 100
self.stage = "Hold Under"
def run_control_stage(self):
if self.stage == "Ramp Up":
self.ctrl_ramp_up()
elif self.stage == "Overshoot Over":
self.ctrl_overshoot_over()
elif self.stage == "Hold Over":
self.ctrl_hold_over()
elif self.stage == "Land Over":
self.ctrl_land_over()
elif self.stage == "Ramp Down":
self.ctrl_ramp_down()
elif self.stage == "Overshoot Under":
self.ctrl_overshoot_under()
elif self.stage == "Overshoot Under":
self.ctrl_overshoot_under()
elif self.stage == "Hold Under":
self.ctrl_hold_under()
elif self.stage == "Land Under":
self.ctrl_land_under()
elif self.stage == "Hold":
self.ctrl_hold()
def is_timer_fired(self):
if round(self.time - self.checkpoint, 3) >= self.period:
self.checkpoint = self.time
return True
return False
def calc_feed_forward(self):
return 0
def ramp_to(self, new_set_point, sample_rate):
if self.set_point == new_set_point:
return
self.pid.reset()
self.start_time = self.time
self.time_elapsed = 0
self.set_point = new_set_point
self.smpWinInRampUpFlag = False
self.smpWinInRampDownFlag = False
if new_set_point - self.pcr.block_temp > 2:
self.ramp_dist = new_set_point - self.pcr.block_temp
self.target_sample_rate = sample_rate / 100 * self.max_up_ramp
self.ramp_time = self.ramp_dist / self.target_sample_rate
self.prepare_ramp_up()
elif new_set_point - self.pcr.block_temp < -2:
self.ramp_dist = self.pcr.block_temp - new_set_point
self.target_sample_rate = sample_rate / 100 * self.max_down_ramp
self.ramp_time = self.ramp_dist / self.target_sample_rate
self.prepare_ramp_down()
else:
self.prepare_hold()
def output(self):
Iset, Vset = self.peltier.output( self.qpid,
self.pcr.heat_sink_temp,
self.pcr.block_temp,
self.maxHeatIset,
self.maxCoolIset
)
self.pcr.Iset = Iset
self.pcr.Vset = Vset
def update(self):
self.update_feedback()
self.run_control_stage()
self.output()
def tick(self, tick):
self.time += tick
if self.is_timer_fired():
self.update()
class Lane_Follower:
'''
Control the robut to follow simple lanes.
'''
def __init__(self, node_name="lane_follower"):
'''
Initialize the lane follower.
Parameters:
node_name: The name of the lane follower node
Returns:
N/A
'''
rospy.init_node(node_name)
#TOPICS
lane_tracking_topic = rospy.get_namespace() + "lane_tracking"
odom_topic = rospy.get_namespace() + "raw/odometry"
desired_movement_topic = rospy.get_namespace() + "desired/movement"
#Lane tracking data from subscriber. Data comes from the lane detection.
#node.
rospy.Subscriber(lane_tracking_topic, Float32MultiArray,
self._lane_tracking_callback)
#Subscriber to the odometry data
rospy.Subscriber(odom_topic, Odometry, self._odom_data_callback)
#Publisher to the movement controller api
self.desired_movement_pub = rospy.Publisher(desired_movement_topic,
Float32MultiArray,
queue_size=1)
#Initialize the pid controller for tracking the lane
#TODO: Add parameters for lane tracking pid to config file
param_path = '/vision_params/lane_follower/pid/'
#Get the pid controller parameters from the parameter server
k_p = rospy.get_param(param_path + 'k_p')
k_i = rospy.get_param(param_path + 'k_i')
k_d = rospy.get_param(param_path + 'k_d')
integral_min = rospy.get_param(param_path + 'integral_min')
integral_max = rospy.get_param(param_path + 'integral_max')
max_control_effort = rospy.get_param(param_path + 'max_control_effort')
min_control_effort = rospy.get_param(param_path + 'min_control_effort')
self.lane_tracking_pid_controller = PID_Controller(
k_p=k_p,
k_i=k_i,
k_d=k_d,
integral_min=integral_min,
integral_max=integral_max,
max_control_effort=max_control_effort,
min_control_effort=min_control_effort)
#Service the enable or disable line following.
#This can be called to enable or disable the control of the robot based
#on line following
rospy.Service('enable_line_following', bool_call,
self._line_following_state)
self.rate = rospy.Rate(10)
#Initialize variables
self.lane_detected = False
self.camera_center_point = 0.0
self.lane_center_point = 0.0
self.lane_follow_enabled = True
self.measured_velocity = 0.0
self.measured_orientation = 0.0
#Base velocity
self.desired_velocity = 0.5
def _line_following_state(self, req):
'''
The service callback to enable or disable line following.
Parameters:
req: The request message
Returns:
N/A
'''
self.lane_follow_enabled = req.val
#Set the robots velocity to zero and hold the current orientation
self.desired_movement_msg.data = [0.0, self.measured_orientation]
self.desired_movement_pub.publish(self.desired_movement_msg)
return bool_callResponse()
def _lane_tracking_callback(self, lane_tracking_msg):
'''
Receive the lane tracking data msg. Unpack it
Parameters:
lane_tracking_msg. The lane tracking data message from the lane detection
node.
Format: [lane_detected_bool, set_point, measured_point]
Returns:
N/A
'''
ret = lane_tracking_msg.data[0]
if (ret == 1.0):
self.lane_detected = True
else:
self.lane_detected = False
self.camera_center_point = lane_tracking_msg.data[1]
self.lane_center_point = lane_tracking_msg.data[2]
def _odom_data_callback(self, odom_msg):
'''
Callback function for the measured odometry data estimated.
Unpack the data
Parameters:
measured_odom_msg: Odometry data message type nav_msgs/Odometry
Returns:
N/A
'''
self.measured_odom = odom_msg
self.measured_velocity = self.measured_odom.twist.twist.linear.x
#Orientation is the direction the robot faces
#Convert from quaternion to euler
quaternion = [
self.measured_odom.pose.pose.orientation.x,
self.measured_odom.pose.pose.orientation.y,
self.measured_odom.pose.pose.orientation.z,
self.measured_odom.pose.pose.orientation.w
]
[roll, pitch, yaw] = euler_from_quaternion(quaternion)
self.measured_orientation = yaw
def run(self):
'''
Main loop for running the lane following.
Parameters:
N/A
Returns:
N/A
'''
self.desired_movement_msg = Float32MultiArray()
try:
while not rospy.is_shutdown():
if (self.lane_follow_enabled):
#print(self.camera_center_point, self.lane_center_point)
#Get the pid control effort the determine how much to steer
control_effort, error = self.lane_tracking_pid_controller.update(\
self.camera_center_point, self.lane_center_point)
#Translate the control effort into a yaw angle for the robot to
#face
adjustment_angle = self.measured_orientation + control_effort
if (adjustment_angle < -1 * math.pi):
self.desired_orientation = 2.0 * math.pi + adjustment_angle
elif (adjustment_angle > math.pi):
self.desired_orientation = adjustment_angle - 2.0 * math.pi
else:
self.desired_orientation = adjustment_angle
#Write the velocity and orientation to the robot.
velocity = self.desired_velocity - abs(0.005 * error)
self.desired_movement_msg.data = [
velocity, self.desired_orientation
]
self.desired_movement_pub.publish(
self.desired_movement_msg)
else:
self.desired_movement_msg.data = [
0.0, self.desired_orientation
]
self.rate.sleep()
except rospy.ROSInterruptException:
pass
class Attitude_Control:
def __init__(self):
rospy.Subscriber("/Outdoor_Blimp/setpoint_position", PoseStamped,
self.process_setpoint)
rospy.Subscriber("/Outdoor_Blimp/imu/orientation", Vector3,
self.process_current)
self.blimp = Blimp()
self.set_yaw = 0
self.set_pitch = 0
self.set_roll = 0
self.set_trans_x = 0
self.set_trans_z = 0
self.curr_pitch = 0
self.curr_roll = 0
self.curr_yaw = 0
self.curr_trans_x = 0
self.curr_trans_z = 0
yaw_kp = rospy.get_param("~yaw_kp")
yaw_ki = rospy.get_param("~yaw_ki")
yaw_kd = rospy.get_param("~yaw_kd")
trans_x_kp = rospy.get_param("~trans_x_kp")
trans_x_ki = rospy.get_param("~trans_x_ki")
trans_x_kd = rospy.get_param("~trans_x_kd")
trans_z_kp = rospy.get_param("~trans_z_kp")
trans_z_ki = rospy.get_param("~trans_z_ki")
trans_z_kd = rospy.get_param("~trans_z_kd")
self.curr_roll = 0
self.yaw_control = PID_Controller(yaw_kp, yaw_ki, yaw_kd)
self.trans_x_control = PID_Controller(trans_x_kp, trans_x_ki,
trans_x_kd)
self.trans_z_control = PID_Controller(trans_z_kp, trans_z_ki,
trans_z_kd)
self.servos = pigpio.pi()
# self.pan_tilt = PanTilt(8, 25)
def control_loop(self):
# print("Set Yaw Angle: "+ repr(self.set_yaw))
# print("Current Yaw Angle: "+repr(self.curr_yaw))
mv = self.yaw_control.corrective_action(self.set_yaw, self.curr_yaw)
self.blimp.motors.turn(-mv)
# self.blimp.motors.downward()
self.trans_x_control.corrective_action(self.set_trans_x,
self.curr_trans_x)
self.trans_z_control.corrective_action(self.set_trans_z,
self.curr_trans_z)
# self.pan_tilt()
def pan_tilt(self):
roll = self.curr_roll - self.set_roll
pitch = self.curr_pitch - self.set_pitch
servo_pitch_val = int(1600 + 8.88 * (pitch))
servo_roll_val = int(1250 + 9.44 * roll + 0.0123 * roll**2)
# print("Roll Val: %5d" % servo_roll_val)
# print("Pitch Val %5d" % servo_pitch_val)
servo_roll_val = min(servo_roll_val, 2250)
servo_roll_val = max(servo_roll_val, 500)
servo_pitch_val = min(servo_pitch_val, 2500)
servo_pitch_val = max(servo_pitch_val, 800)
self.servos.set_servo_pulsewidth(24, servo_roll_val)
self.servos.set_servo_pulsewidth(23, servo_pitch_val)
# self.pan_tilt.roll(6)
def process_setpoint(self, poseStamped):
pose = poseStamped.pose
quart = pose.orientation
trans = pose.position
#exp_quart = [quart.x,quart.y,quart.z,quart.w]
#r,p,y = euler_from_quaternion(exp_quart)
self.set_trans_x = 0
self.set_trans_z = 0
self.set_yaw = quart.x
self.set_pitch = 0
self.set_roll = 0
#print("Set Yaw Yaw Angle: "+repr(y))
def process_current(self, orientation):
#pose = poseStamped.pose
#quart = pose.orientation
#trans = pose.position
#exp_quart = [quart.x,quart.y,quart.z,quart.w]
#r,p,y = euler_from_quaternion(exp_quart)
self.curr_trans_x = 0
self.curr_trans_z = 0
self.curr_yaw = orientation.x
self.curr_pitch = orientation.y
self.curr_roll = orientation.z
tf = 30 # end time, (sec) time = np.linspace(t0, tf, N) dt = time[1] - time[0] # delta t, (sec) ################################################################################## # Core simulation code # Inital conditions (i.e., initial state vector) y = [0, 0] #y[0] = initial altitude, (m) #y[1] = initial speed, (m/s) # Initialize array to store values soln = np.zeros((len(time),len(y))) # Create instance of PID_Controller class pid = PID_Controller() # Set the Kp value of the controller pid.setKP(kp) # Set the Ki value of the controller pid.setKI(ki) # Set the Kd value of the controller pid.setKD(kd) # Set altitude target r = 10 # meters pid.setTarget(r) # Simulate quadrotor motion
class Movement_Controller:
"""
Control the robot using PID controllers for linear velocity and
direction orientation.
"""
def __init__(self, node_name="movement_controller"):
'''
Initialize the movement controller as a ros node.
Parameters:
N/A
Returns:
N/A
'''
self.node_name = node_name
rospy.init_node(node_name)
#TOPICS - This format of name space is given for ability to simulate multiple
#by providing different names
measured_odom_topic = rospy.get_namespace() + "raw/odometry"
pwm_topic = rospy.get_namespace() + "pwm"
desired_movement_topic = rospy.get_namespace() + "desired/movement"
#Initialize subscriber for raw odometry
rospy.Subscriber(measured_odom_topic, Odometry,
self._odom_data_callback)
#Initialize subscriber for desired_odometry
rospy.Subscriber(desired_movement_topic, Float32MultiArray,
self._desired_movement_callback)
#Initialize PWM controllers
self._initialize_pid_controlers()
self.pwm_pub = rospy.Publisher(pwm_topic,
Int16MultiArray,
queue_size=10)
self.pwm_msg = Int16MultiArray()
self.pid_timer = rospy.Rate(100) #100Hz
#Initialize publisher for writing PWM
self.measured_velocity = 0.0
self.desired_velocity = 0.0
self.measured_orientation = 0.0
self.desired_orientation = 0.0
self.velocity_control = 0.0
self.steering_control = 0.0
def _initialize_pid_controlers(self):
'''
Initialize communication with the pid controllers.
Parameters:
N/A
Returns:
N/A
'''
pid_params = rospy.get_param("/pid")
velocity_pid_params = pid_params['velocity']
steering_pid_params = pid_params['steering']
velocity_k_p = velocity_pid_params['k_p']
velocity_k_i = velocity_pid_params['k_i']
velocity_k_d = velocity_pid_params['k_d']
self.velocity_pid_controller = PID_Controller(k_p=velocity_k_p,
k_i=velocity_k_i,
k_d=velocity_k_d,
integral_min=-10,
integral_max=10,
max_control_effort=255,
min_control_effort=-255)
steering_k_p = steering_pid_params['k_p']
steering_k_i = steering_pid_params['k_i']
steering_k_d = steering_pid_params['k_d']
self.steering_pid_controller = PID_Controller(k_p=steering_k_p,
k_i=steering_k_i,
k_d=steering_k_d,
max_control_effort=255,
min_control_effort=-255,
integral_min=-10,
integral_max=10,
angle_error=True)
#Initialize the service for updating the pid controller gains
rospy.Service('update_pid_gains', pid_gains,
self.handle_pid_gains_update)
return
def handle_pid_gains_update(self, req):
'''
Handler function for when a PID gain upate is requested. This will update
the PIDS in the parameter server.
Parameters:
reg: The request message containing the desired gains
Returns:
N/A
'''
self.velocity_pid_controller.set_gains(req.velocity_k_p,
req.velocity_k_i,
req.velocity_k_d)
self.steering_pid_controller.set_gains(req.steering_k_p,
req.steering_k_i,
req.steering_k_d)
#Update the parameter server on the correct pid values
rospy.set_param(
"/pid/velocity", {
'k_p': req.velocity_k_p,
'k_i': req.velocity_k_i,
'k_d': req.velocity_k_d
})
rospy.set_param(
"/pid/steering", {
'k_p': req.steering_k_p,
'k_i': req.steering_k_i,
'k_d': req.steering_k_d
})
return pid_gainsResponse()
def _odom_data_callback(self, odom_msg):
'''
Callback function for the measured odometry data estimated.
Unpack the data
Parameters:
measured_odom_msg: Odometry data message type nav_msgs/Odometry
Returns:
N/A
'''
self.measured_odom = odom_msg
self.measured_velocity = self.measured_odom.twist.twist.linear.x
#Orientation is the direction the robot faces
#Convert from quaternion to euler
quaternion = [
self.measured_odom.pose.pose.orientation.x,
self.measured_odom.pose.pose.orientation.y,
self.measured_odom.pose.pose.orientation.z,
self.measured_odom.pose.pose.orientation.w
]
[roll, pitch, yaw] = euler_from_quaternion(quaternion)
self.measured_orientation = yaw
def _desired_movement_callback(self, desired_movement_msg):
'''
Callback function for the desired movement for the robot to hold. The
message contains the desired velocity and orientation.
Parameters:
desired_movement_msg: The desired velocity (m/s) and orientation (rad/s).
The message is of type std_msgs/Float32MultiArray.
[desired_velocity, desired_orientation]
Returns:
N/A
'''
self.desired_velocity = desired_movement_msg.data[0]
#Orientation is the direction the robot faces
self.desired_orientation = desired_movement_msg.data[1]
def map_control_efforts_to_pwms(self, vel_control_effort,
steering_control_effort):
'''
Maps the control efforts for velocity and steering to individual motor
PWMS.
Parameters:
vel_control_effort: Velocity Control effort output from PID.
steering_control_effort: sterring control effor output from PID
Returns:
pwms: [pwm_1, pwm_2, pwm_3, pwm_4]
'''
vel_control = vel_control_effort
steering_control = steering_control_effort
pwm_1 = vel_control - steering_control
pwm_2 = vel_control + steering_control
pwm_3 = vel_control + steering_control
pwm_4 = vel_control - steering_control
pwms = [pwm_1, pwm_2, pwm_3, pwm_4]
for i in range(4):
if (pwms[i] < -255):
pwms[i] = -255
elif (pwms[i] > 255):
pwms[i] = 255
return pwms
def run(self):
'''
Run the main loop of the movement controller. Receive desired and
measured odometry data and control.
Parameters:
N/A
Returns:
N/A
'''
#Publish errror for debug
error_pub = rospy.Publisher('pid_errors',
Float32MultiArray,
queue_size=1)
error_msg = Float32MultiArray()
try:
while not rospy.is_shutdown():
#Take the control effort output from PID controller and map to
#PID's of individual motor
self.velocity_control, velocity_error = self.velocity_pid_controller.update(
self.desired_velocity, self.measured_velocity)
self.steering_control, steering_error = self.steering_pid_controller.update(
self.desired_orientation, self.measured_orientation)
error_msg.data = [velocity_error, steering_error]
error_pub.publish(error_msg)
motor_pwms = self.map_control_efforts_to_pwms(
self.velocity_control, self.steering_control)
self.pwm_msg.data = motor_pwms
self.pwm_pub.publish(self.pwm_msg)
#100Hz
self.pid_timer.sleep()
except rospy.ROSInterruptException:
pass
def __init__(self, node_name="lane_follower"):
'''
Initialize the lane follower.
Parameters:
node_name: The name of the lane follower node
Returns:
N/A
'''
rospy.init_node(node_name)
#TOPICS
lane_tracking_topic = rospy.get_namespace() + "lane_tracking"
odom_topic = rospy.get_namespace() + "raw/odometry"
desired_movement_topic = rospy.get_namespace() + "desired/movement"
#Lane tracking data from subscriber. Data comes from the lane detection.
#node.
rospy.Subscriber(lane_tracking_topic, Float32MultiArray,
self._lane_tracking_callback)
#Subscriber to the odometry data
rospy.Subscriber(odom_topic, Odometry, self._odom_data_callback)
#Publisher to the movement controller api
self.desired_movement_pub = rospy.Publisher(desired_movement_topic,
Float32MultiArray,
queue_size=1)
#Initialize the pid controller for tracking the lane
#TODO: Add parameters for lane tracking pid to config file
param_path = '/vision_params/lane_follower/pid/'
#Get the pid controller parameters from the parameter server
k_p = rospy.get_param(param_path + 'k_p')
k_i = rospy.get_param(param_path + 'k_i')
k_d = rospy.get_param(param_path + 'k_d')
integral_min = rospy.get_param(param_path + 'integral_min')
integral_max = rospy.get_param(param_path + 'integral_max')
max_control_effort = rospy.get_param(param_path + 'max_control_effort')
min_control_effort = rospy.get_param(param_path + 'min_control_effort')
self.lane_tracking_pid_controller = PID_Controller(
k_p=k_p,
k_i=k_i,
k_d=k_d,
integral_min=integral_min,
integral_max=integral_max,
max_control_effort=max_control_effort,
min_control_effort=min_control_effort)
#Service the enable or disable line following.
#This can be called to enable or disable the control of the robot based
#on line following
rospy.Service('enable_line_following', bool_call,
self._line_following_state)
self.rate = rospy.Rate(10)
#Initialize variables
self.lane_detected = False
self.camera_center_point = 0.0
self.lane_center_point = 0.0
self.lane_follow_enabled = True
self.measured_velocity = 0.0
self.measured_orientation = 0.0
#Base velocity
self.desired_velocity = 0.5
def init_pid(self):
self.pid = PID_Controller()
self.pid.dt = self.period / 60 # dt is in minute
self.pid2 = PID_Controller()
self.pid2.dt = self.period / 60 # dt is in minute