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 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
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()