def test_auto_mode():
pid = PID(1, 0, 0, setpoint=10, sample_time=None)
# ensure updates happen by default
assert pid(0) == 10
assert pid(5) == 5
# ensure no new updates happen when auto mode is off
pid.auto_mode = False
assert pid(1) == 5
assert pid(7) == 5
# should reset when reactivating
pid.auto_mode = True
assert pid._last_input is None
assert pid._integral == 0
assert pid(8) == 2
# last update time should be reset to avoid huge dt
from simple_pid.PID import _current_time
pid.auto_mode = False
time.sleep(1)
pid.auto_mode = True
assert _current_time() - pid._last_time < 0.01
# check that setting last_output works
pid.auto_mode = False
pid.set_auto_mode(True, last_output=10)
assert pid._integral == 10
def run(self):
global leftSpeed
global rightSpeed
global dx
global curPos
global curPos_d
dt = 0.1
pid_straight = PID(0.5, 0.5, 0.04, setpoint=0)
pid_straight.sample_time = dt
flag = False
while threadRun:
if straight and leftSpeed != 0 and rightSpeed != 0:
if flag == False:
pid_straight.set_auto_mode = True
flag = True
err = (abs(leftDistance) - abs(rightDistance)) * PULSE_LENGTH
pid_straight.output_limits = (-speed / 2, speed / 2)
fix = pid_straight(err)
#print(leftDistance, rightDistance, fix)
if leftSpeed > 0:
leftSpeed = speed + fix
elif leftSpeed < 0:
leftSpeed = -speed - fix
if rightSpeed > 0:
rightSpeed = speed - fix
elif rightSpeed < 0:
rightSpeed = -speed + fix
else:
if flag == True:
pid_straight.set_auto_mode = False
flag = False
time.sleep(dt)
def reflow_app():
global oven_state
global temp_0
global temp_0_rate
global Button_Start_pressed
global command_transfer
global cycle_started
timer1 = time.time()
speed = 1850
#Controls temperature rate.
#######################################
pid_rate1 = PID(3, 0.01, 0.1, setpoint=1)
pid_rate1.output_limits = (-5, 5)
pid_rate1.proportional_on_measurement = False
pid_rate1_F = 100
pid_rate1_max = 0
pid_rate1_min = 730
#######################################
#Controls the actual temperature of soaking time
pid_temp1 = PID(.5, 0.01, 0.2, setpoint=140)
pid_temp1.output_limits = (-2, .5)
pid_temp1.proportional_on_measurement = False
pid_temp1_F = 100
timer_soak = time.time() #30-90 seconds 30-120 acceptable
#######################################
#Controls the actual temperature of soaking time
pid_rate2 = PID(1.3, 0.02, 0.2, setpoint=.6)
pid_rate2.output_limits = (-5, 5)
pid_rate2.proportional_on_measurement = False
pid_rate2_F = 100
timer_dripping = time.time() #30-90 seconds 30-120 acceptable
#######################################
#Timestamps
TimeAboveLiquidus = .07 #Also used to time the reflow 45-60 seconds
Soak_last_pos = 0
pid_dt = time.time()
QThread.msleep(3000)
application.ui.label_test.setText(f"State: 0. Ready.")
while 1:
#First wait for the start button
#Then start the sequence
#either use the preheat zone and prewarm to 100
#Then go to the critical zone (my cabezone)
#When there use the
QThread.msleep(50)
pid_dt = time.time() - timer1
if (time.time() - timer1) > 1000 / 1000:
timer1 = time.time()
#print(timer1)
temp = temp_0
#application.ui.label_test.setText(f"State: {oven_state}")
###########################################################
if oven_state == 0:
###########################################################
application.ui.label_test.setText(f"State: 0. Ready.")
if Button_Start_pressed == 1:
Button_Start_pressed = 0
reinit_graph()
new_position = application.ui.spinBox_preheat.value()
print(
f"Oven program started, Position preheat: {new_position} Speed {speed}"
)
print(
str.encode('G1X' + str(new_position) + 'F' +
str(speed) + '\n'))
command_transfer = 'G1X' + str(new_position) + 'F' + str(
speed) + '\n'
application.ui.label_test.setText(f"State: 1 : Preheating")
oven_state = 1
global cycle_started
cycle_started = True
###########################################################
elif oven_state == 1: #Preheat
###########################################################
application.ui.label_test.setText(
f"State: 1 : Preheating: {(60-temp):,.2f}")
if temp > 60:
oven_state = 2
new_position = application.ui.spinBox_critical.value()
print(
f"Moving to critical point: {new_position} Speed {speed}"
)
print(
str.encode('G1X' + str(new_position) + 'F' +
str(speed) + '\n'))
command_transfer = 'G1X' + str(new_position) + 'F' + str(
speed) + '\n'
###########################################################
elif oven_state == 2: #Waiting to arrive to critical zone
###########################################################
new_position = application.ui.spinBox_critical.value()
application.ui.label_test.setText(
f"State: 2 : Moving to Critical {(new_position-position):,.2f}mm"
)
if position >= new_position - 2: #waiting until we arrive to new location..
oven_state = 3
print("Arrived to Critical point")
application.ui.label_test.setText(f"State: 2 : Arrived!")
###########################################################
elif oven_state == 3: #Increasing temp to 135 at 1C/s (to SoakZone)
###########################################################
#warming from Preheat to Soaking between 140-150C, 130-170C Acceptable.
#Here we will use a PID loop jog to the desired value (130C) at rate <2C/s
#After the that is achieve we will just to the next loop.
"""Add PID loop here"""
pid_rate1.sample_time = pid_dt
output = pid_rate1(
temp_0_rate
) #<-----------------------------------------------
command_transfer = 'G1X' + str(
int(position + output)) + 'F' + str(pid_rate1_F) + '\n'
print("PID Rate 1 output:", output)
application.ui.label_test.setText(
f"State: 3 : Heating to soaking T-0= {(130-temp):,.2f} PID {output:,.2f}"
)
Soak_last_pos = position + output
if temp > 130:
oven_state = 4
print("Temp of 130 reached")
timer_soak = time.time()
application.ui.label_test.setText(
f"State: 3 : Arrived to SoakZone!")
pid_temp1.set_auto_mode(True, last_output=0)
###########################################################
elif oven_state == 4: #Soaking stage. 45-60s Recommended. 30-100s Acceptable
###########################################################
#Here the setpoing is changed to maintain the temp between 140-150 using PWM value
#When the timer is done
"""Cascade PID loop"""
pid_temp1.sample_time = pid_dt
output = pid_temp1(
temp_0) #<-----------------------------------------------
pid_rate1.setpoint = output
pid_rate1.sample_time = pid_dt
output2 = pid_rate1(
temp_0_rate
) #<-----------------------------------------------
if position + output2 > 730:
command_transfer = 'G1X' + str(
int(position +
output2)) + 'F' + str(pid_rate1_F) + '\n'
#command_transfer = 'G1X'+str(int(position+output))+'F'+str(pid_temp1_F)+'\n'
#print(f"PID Temp 1 {output:,.2f}, T-0 = {(time.time() - timer_soak):,.2f}")
application.ui.label_test.setText(
f"State: 4 : Soaking: t-0 = {45-(time.time() - timer_soak):,.2f} PID{output:,.2f}->{output2:,.2f}"
)
if time.time() - timer_soak > 45: #45 seconds?
oven_state = 5
application.ui.label_test.setText(f"State: 4 : Timeout!")
command_transfer = 'G1X' + str(Soak_last_pos) + 'F' + str(
pid_rate1_F) + '\n'
###########################################################
elif oven_state == 5: #Here we slowly move the car at rate of .5-1Cs recommend up to 2.4C/s is acceptable.
###########################################################
"""Add PID loop here"""
pid_rate2.sample_time = pid_dt
output = pid_rate2(
temp_0_rate
) #<-----------------------------------------------
command_transfer = 'G1X' + str(
int(position + output)) + 'F' + str(pid_rate2_F) + '\n'
print("PID Rate 2 output:", output)
application.ui.label_test.setText(
f"State: 5 : Heating to Peak t-0= {(210-temp):,.2f} PID {output:,.2f}"
)
if temp >= 183:
if TimeAboveLiquidus == .07: #just to know we haven't been here
TimeAboveLiquidus = time.time()
print("###Warning above liquidus temp!###")
pid_rate2.setpoint = 1.2
pid_rate2.Ki = .1
if temp > 210:
oven_state = 6
application.ui.label_test.setText(f"State: 5 : Done!")
new_position = application.ui.spinBox_maxTravel.value()
print(
str.encode('G1X' + str(new_position) + 'F' +
str(speed) + '\n'))
command_transfer = 'G1X' + str(new_position) + 'F' + str(
speed) + '\n'
#maybe stop the car just in case
###########################################################
elif oven_state == 6: #here we just move the car to the reflow position and wait.
###########################################################
#We wait around 30-60 seconds...
application.ui.label_test.setText(
f"State: 6 : reflowing t-0= {30-(time.time() - TimeAboveLiquidus):,.2f}"
)
if time.time() - TimeAboveLiquidus > 30:
oven_state = 7
new_position = application.ui.spinBox_dripping.value()
print(
str.encode('G1X' + str(new_position) + 'F' +
str(speed) + '\n'))
command_transfer = 'G1X' + str(new_position) + 'F' + str(
speed) + '\n'
application.ui.label_test.setText(f"State: 7 : dripping")
#send the command to the dripping position.
###########################################################
elif oven_state == 7: #Here we waiting until we arrive to the dripping
###########################################################
new_position = application.ui.spinBox_dripping.value()
#We wait around 30-60 seconds...
application.ui.label_test.setText(
f"State: 7 : Waiting for drip... Moving?->{abs(position - new_position)}"
)
if abs(position - new_position
) < 11: #waiting until we arrive to new location..
oven_state = 3
print("Arrived to dripping")
application.ui.label_test.setText(
f"State: 7 : Arrived to dripping!")
oven_state = 8
timer_dripping = time.time()
###########################################################
elif oven_state == 8: #here we just move the car to the reflow position and wait.
###########################################################
application.ui.label_test.setText(
f"State: 8 : Dripping t-O : {time.time() - timer_dripping:,.2f}"
)
#We wait around 15 seconds...
if time.time() - timer_dripping > 25:
oven_state = 9
application.ui.label_test.setText(f"State: 8 : Timeout!")
new_position = application.ui.spinBox_coolDown.value()
command_transfer = 'G1X' + str(new_position) + 'F' + str(
speed) + '\n'
###########################################################
elif oven_state == 9: #Here we waint until it cools down to <80
###########################################################
application.ui.label_test.setText(
f"State: 9 : Cooling down {(80-temp):,.2f}")
if temp < 80:
application.ui.label_test.setText(
f"State: 9 : Cool down completed! DONE!")
oven_state = 0
cycle_started = False
new_position = application.ui.spinBox_home.value()
command_transfer = 'G1X' + str(new_position) + 'F' + str(
speed) + '\n'
#we Finish save data whatever and send card to home.
if close_everything == 1:
return 0
class animate(Thread):
def __init__(self, Kp=1, Ki=0.1, Kd=0.05, cps=1, function=None, queueSize=10, *args, **kwargs):
Thread.__init__(self)
assert function, 'You must supply a function to animate'
self._speed = 1/cps
self._pid = PID(Kp, Ki, Kd, setpoint = self._speed, sample_time=self._speed)
self._function = function
self._args = args
self._kwargs = kwargs
self.fps = 0
self._running = True
self._queue = Queue(maxsize=queueSize)
self._event = Event()
self._forceEvent = Event()
@property
def empty(self):
return self._queue.empty()
@property
def full(self):
return self._queue.full()
@property
def qsize(self):
return self._queue.qsize()
def pause(self):
self._event.clear()
def restart(self):
self._event.set()
def toggle(self):
if self._event.isSet():
self._event.clear()
else:
self._event.set()
def stop(self):
self._running = False
self._event.set()
self.get()
self.join()
def force(self, *args, **kwargs):
self._Force = True
# Set new arguments for animated function
self._args = args
self._kwargs = kwargs
# If needed, unblock run if the queue if full
try:
self._queue.get_nowait()
self._queue.task_done()
except Empty:
pass
# Wait until animate confirms force is finished
self._forceEvent.clear()
self._forceEvent.wait()
def get(self, wait=0):
try:
retval = self._queue.get(wait)
self._queue.task_done()
except Empty:
retval = None
return retval
def _emptyQueue(self):
while True:
try:
self._queue.get_nowait()
self._queue.task_done()
except Empty:
break
def _invoke(self, *args, **kwargs):
# Invoke function
if args and kwargs:
retval = self._function(*args, **kwargs)
elif args:
retval = self._function(*args)
elif kwargs:
retval = self._function(**kwargs)
else:
retval = self._function()
return retval
def run(self):
correction = 0
loopTime = self._speed
renderTimer = time.time()
renderCounter = 0
self._event.set()
self._Force = False
while self._running:
# Compute current FPS every 5 seconds
renderCounter += 1
if renderTimer + 5 < time.time():
self.fps = renderCounter/5
renderCounter = 0
renderTimer = time.time()
# Disable pid while waiting to be activated
self._pid.set_auto_mode(False)
self._event.wait()
# Activated! Re-enable PID, begin processing
self._pid.set_auto_mode(True, last_output=correction)
correction = self._pid(loopTime)
startLoop = time.time()
if self._speed + correction > 0:
time.sleep(self._speed + correction)
# Disable PID while trying to place new render in queue
self._pid.set_auto_mode(False)
putStart = time.time()
if self._Force:
self._Force = False
self._emptyQueue()
# Must put value in queue before clearing the forceRender Event
# so that the forced render receives the newly computed value
retval = self._invoke(*self._args, **self._kwargs)
self._queue.put( retval )
self._forceEvent.set()
else:
retval = self._invoke(*self._args, **self._kwargs)
self._queue.put( retval )
# Correct the loop timer if queue was blocked and restart the PID
startLoop = startLoop + (time.time()-putStart)
self._pid.set_auto_mode(True, last_output=correction)
loopTime = time.time() - startLoop
if boiler_power > 0:
# print(self.water_temp, " + ", boiler_power, dt)
# Boiler can only produce heat, not cold
self.water_temp += 3 * boiler_power * dt
# Some heat dissipation
self.water_temp -= 90 * dt
return self.water_temp
client = mqtt.Client("pid")
pid = PID(0.2, 0.15, 0, sample_time=1800, proportional_on_measurement=False)
pid.setpoint = 0
pid.output_limits = (25, 45)
pid.set_auto_mode(False)
run = True
PID_TOPIC = "sensors/pannu/pid"
SETPOINT_TOPIC = "sensors/indoor/command"
INPUT_TOPIC = "sensors/indoor"
# use wall clock instead of relative process time
def fix_pid_time(now):
pid._last_time = now
start_time = time.time()
fix_pid_time(start_time)
class Node:
def __init__(self):
rospy.init_node("joint_controller_node")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Setting up joint_controller_node")
self.pitch_pub = rospy.Publisher("roboclaw/pitch_vel",
Twist,
queue_size=10)
self.height_pub = rospy.Publisher("roboclaw/height_vel",
Twist,
queue_size=10)
self.joint_pub = rospy.Publisher('arm_joint_state',
JointState,
queue_size=10)
self.pitch_sub = rospy.Subscriber('roboclaw/pitch', JointState,
self.pitch_pos_callback)
self.height_sub = rospy.Subscriber('roboclaw/height', JointState,
self.height_pos_callback)
self.joint_status_pub = rospy.Subscriber('joint_states', JointState,
self.joint_states_callback)
self.rate = rospy.Rate(5)
self.height_min_length = 15.71
self.height_max_length = 25.55
self.height_stroke_length = self.height_max_length - self.height_min_length
self.height_angle_offset = 369.69
self.pitch_min_length = 11.69
self.pitch_max_length = 17.6
self.pitch_stroke_length = self.pitch_max_length - self.pitch_min_length
self.start = False
self.h1_pid = PID(5,
0,
0,
output_limits=(-127, 127),
auto_mode=False,
sample_time=1 / 5)
self.h2_pid = PID(5,
0,
0,
output_limits=(-127, 127),
auto_mode=False,
sample_time=1 / 5)
self.p1_pid = PID(5,
0,
0,
output_limits=(-127, 127),
auto_mode=False,
sample_time=1 / 5)
self.p2_pid = PID(5,
0,
0,
output_limits=(-127, 127),
auto_mode=False,
sample_time=1 / 5)
self.MAX_ENC = 2047.0
self.quantize_div = 12.0
self.deadband = 11.0
self.h_params = {"m1": 50.0, "m2": 50.0}
self.p_params = {"m1": 50.0, "m2": 50.0}
self.p1_vals = [0.0, 0.0, 0.0, 0.0, 0.0]
self.p2_vals = [0.0, 0.0, 0.0, 0.0, 0.0]
self.h1_vals = [0.0, 0.0, 0.0, 0.0, 0.0]
self.h2_vals = [0.0, 0.0, 0.0, 0.0, 0.0]
self.height_pos = {"m1": 0.0, "m2": 0.0}
self.pitch_pos = {"m1": 0.0, "m2": 0.0}
def run(self):
rospy.loginfo("waiting for data")
while self.height_pos['m1'] == 0 and self.pitch_pos['m1'] == 0:
self.rate.sleep()
rospy.sleep(1)
rospy.loginfo("got data")
self.h1_pid.set_auto_mode(True, last_output=0.0)
self.h2_pid.set_auto_mode(True, last_output=0.0)
self.p1_pid.set_auto_mode(True, last_output=0.0)
self.p2_pid.set_auto_mode(True, last_output=0.0)
h1_old, h2_old, p1_old, p2_old = self.call_pids()
while not rospy.is_shutdown():
h1, h2, p1, p2 = self.call_pids()
h1, h2, p1, p2 = self.apply_deadband(h1, h2, p1, p2)
h1, h2, p1, p2 = self.safety_check(h1, h2, p1, p2, 40, 20)
if h1_old != h1 or h2_old != h2:
rospy.loginfo("pid output h1 %d, h2 %d", h1, h2)
if p1_old != p1 or p2_old != p2:
rospy.loginfo("pid output p1 %d, p2 %d", p1, p1)
h1_old = h1
h2_old = h2
p1_old = p1
p2_old = p2
self.publish_height_vel(-h1, h2)
self.publish_pitch_vel(-p1, -p2)
self.publish_arm_state()
self.rate.sleep()
def shutdown(self):
rospy.loginfo("Shutting down joint_controller_node")
def call_pids(self):
h1 = self.h1_pid(self.quantize(self.height_pos['m2']))
h2 = self.h2_pid(self.quantize(self.height_pos['m1']))
p1 = self.p1_pid(self.quantize(self.pitch_pos['m1']))
p2 = self.p2_pid(self.quantize(self.pitch_pos['m2']))
return h1, h2, p1, p2
def quantize(self, val):
return int(val / self.quantize_div)
def get_avg(self, lst):
return sum(lst) / len(lst)
def apply_deadband(self, h1, h2, p1, p2):
if abs(h1) < self.deadband: h1 = 0.0
if abs(h2) < self.deadband: h2 = 0.0
if abs(p1) < self.deadband: p1 = 0.0
if abs(p2) < self.deadband: p2 = 0.0
return h1, h2, p1, p2
def safety_check(self, h1, h2, p1, p2, thresh_p, thresh_o):
had_error = False
if abs(
self.quantize(self.height_pos['m1']) -
self.quantize(self.height_pos['m2'])) > thresh_p:
had_error = True
#h1 = -20
#h2 = -20
if abs(
self.quantize(self.pitch_pos['m1']) -
self.quantize(self.pitch_pos['m2'])) > thresh_p:
had_error = True
#p1 = 0
#p2 = 0
if abs(h1 - h2) > thresh_o:
had_error = True
h1 = -40
h2 = -40
if abs(p1 - p2) > thresh_o:
had_error = True
p1 = 0
p2 = 0
if had_error:
rospy.logerr("ERROR!!!")
return h1, h2, p1, p2
def pitch_add_meas(self, p1, p2):
self.p1_vals.insert(0, p1)
self.p1_vals.pop()
self.p2_vals.insert(0, p2)
self.p2_vals.pop()
def height_add_meas(self, h1, h2):
self.h1_vals.insert(0, h1)
self.h1_vals.pop()
self.h2_vals.insert(0, h2)
self.h2_vals.pop()
def pitch_pos_callback(self, msg):
if msg.position[0] < 0.001 or msg.position[1] < 0.001:
self.pitch_add_meas(self.p1_vals[0], self.p2_vals[0])
else:
self.pitch_add_meas(msg.position[0], msg.position[1])
self.pitch_pos['m1'] = self.get_avg(self.p1_vals)
self.pitch_pos['m2'] = self.get_avg(self.p2_vals)
def height_pos_callback(self, msg):
if msg.position[0] < 0.001 or msg.position[1] < 0.001:
self.height_add_meas(self.h1_vals[0], self.h2_vals[0])
else:
self.height_add_meas(msg.position[0], msg.position[1])
self.height_pos['m1'] = self.get_avg(self.h1_vals)
self.height_pos['m2'] = self.get_avg(self.h2_vals)
def joint_states_callback(self, msg):
height_angle_target = msg.position[2]
height_inch_target = self.height_angle_to_dist(
math.radians(self.height_angle_offset) - height_angle_target)
height_enc_target = self.height_inch_to_enc(height_inch_target,
self.h_params['m1'])
height_enc_target = self.height_inch_to_enc(height_inch_target,
self.h_params['m2'])
self.h1_pid.setpoint = self.quantize(height_enc_target)
self.h2_pid.setpoint = self.quantize(height_enc_target)
pitch_angle_target = msg.position[3]
pitch_inch_target = self.pitch_angle_to_dist(pitch_angle_target)
pitch_enc_target = self.pitch_inch_to_enc(pitch_inch_target,
self.p_params['m1'])
pitch_enc_target = self.pitch_inch_to_enc(pitch_inch_target,
self.p_params['m2'])
self.p1_pid.setpoint = self.quantize(pitch_enc_target)
self.p2_pid.setpoint = self.quantize(pitch_enc_target)
def publish_arm_state(self):
arm_msg = JointState()
arm_msg.header = Header()
arm_msg.header.stamp = rospy.Time.now()
arm_msg.name = ['base_to_lever_arm', 'lever_arm_to_digging_arm']
height_pos_inch = self.height_enc_to_inch(self.height_pos['m1'],
self.h_params['m1'])
pitch_pos_inch = self.pitch_enc_to_inch(self.pitch_pos['m1'],
self.p_params['m1'])
height_angle = math.radians(
self.height_angle_offset) - self.height_dist_to_angle(
height_pos_inch)
pitch_angle = self.pitch_dist_to_angle(pitch_pos_inch)
arm_msg.position = [height_angle, pitch_angle]
self.joint_pub.publish(arm_msg)
def publish_pitch_vel(self, m1, m2):
vel = Twist()
vel.linear.x = m1
vel.linear.y = m2
vel.linear.z = 0
self.pitch_pub.publish(vel)
def publish_height_vel(self, m1, m2):
vel = Twist()
vel.linear.x = m1
vel.linear.y = m2
vel.linear.z = 0
self.height_pub.publish(vel)
def height_enc_to_inch(self, enc, enc_min):
percent = float(enc - enc_min) / float(self.MAX_ENC - enc_min)
return self.height_min_length + self.height_stroke_length * percent
def height_inch_to_enc(self, inch, enc_min):
percent = float(inch - self.height_min_length) / float(
self.height_stroke_length)
return float(enc_min) + float(self.MAX_ENC - enc_min) * percent
def pitch_enc_to_inch(self, enc, enc_min):
percent = float(enc - enc_min) / float(self.MAX_ENC - enc_min)
return self.pitch_min_length + self.pitch_stroke_length * percent
def pitch_inch_to_enc(self, inch, enc_min):
percent = float(inch - self.pitch_min_length) / float(
self.pitch_stroke_length)
return enc_min + (self.MAX_ENC - enc_min) * percent
def height_angle_to_dist(self, theta):
return math.sqrt(793.5625 - 708 * math.cos(math.radians(90) - theta))
def height_dist_to_angle(self, pos): #pos in inches
return math.radians(
90 - math.degrees(math.acos((793.5625 - (pos * pos)) / 708)))
def pitch_angle_to_dist(self, theta):
return math.sqrt(((30 - 11 * math.cos(theta)) - 14.875)**2 +
((11 * math.sin(theta) - 3) - 6.75)**2)
def pitch_dist_to_angle(self, pos):
return math.asin(0.05557 * pos * math.sin(
math.acos((pos * pos - 202.828125) /
(22 * pos * pos)))) + math.radians(32.807)
class PIDRunner(QObject):
status_sig = pyqtSignal(list)
pid_output_signal = pyqtSignal(dict)
def __init__(self,model_class,
move_done_signals=[],
grab_done_signals=[],
move_modules_commands=[],
detector_modules_commands=[],
params=dict([]), filter=dict([]),
det_averaging = []
):
"""
Init the PID instance with params as initial conditions
Parameters
----------
params: (dict) Kp=1.0, Ki=0.0, Kd=0.0,setpoint=0, sample_time=0.01, output_limits=(None, None),
auto_mode=True,
proportional_on_measurement=False)
"""
super().__init__()
self.model_class = model_class
self.move_done_signals = move_done_signals
self.grab_done_signals = grab_done_signals
self.det_averaging = det_averaging
self.move_modules_commands = move_modules_commands
self.detector_modules_commands = detector_modules_commands
self.current_time = 0
self.input = 0
self.output = None
self.output_to_actuator = None
self.output_limits = None, None
self.filter = filter #filter=dict(enable=self.settings.child('main_settings', 'filter', 'filter_enable').value(), value=self.settings.child('main_settings', 'filter', 'filter_step').value())
self.pid = PID(**params) ##PID(object):
self.pid.set_auto_mode(False)
self.refreshing_ouput_time = 200
self.running = True
self.timer = self.startTimer(self.refreshing_ouput_time)
self.det_done_datas = OrderedDict()
self.move_done_positions = OrderedDict()
self.move_done_flag = False
self.det_done_flag = False
self.paused = True
self.timeout_timer = QtCore.QTimer()
self.timeout_timer.setInterval(10000)
self.timeout_scan_flag = False
self.timeout_timer.timeout.connect(self.timeout)
def timerEvent(self, event):
if self.output_to_actuator is not None:
self.pid_output_signal.emit(dict(output=self.output_to_actuator, input=[self.input]))
else:
self.pid_output_signal.emit(dict(output=[0], input=[self.input]))
def timeout(self):
self.status_sig.emit(["Update_Status", 'Timeout occured', 'log'])
self.timeout_scan_flag = True
def wait_for_det_done(self):
self.timeout_scan_flag=False
self.timeout_timer.start()
while not(self.det_done_flag or self.timeout_scan_flag):
#wait for grab done signals to end
QtWidgets.QApplication.processEvents()
self.timeout_timer.stop()
def wait_for_move_done(self):
self.timeout_scan_flag=False
self.timeout_timer.start()
while not(self.move_done_flag or self.timeout_scan_flag):
#wait for move done signals to end
QtWidgets.QApplication.processEvents()
self.timeout_timer.stop()
@pyqtSlot(ThreadCommand)
def queue_command(self, command=ThreadCommand()):
"""
"""
if command.command == "start_PID":
self.start_PID(*command.attributes)
elif command.command == "run_PID":
self.run_PID(*command.attributes)
elif command.command == "pause_PID":
self.pause_PID(*command.attributes)
elif command.command == "stop_PID":
self.stop_PID()
elif command.command == 'update_options':
self.set_option(**command.attributes)
elif command.command == 'input':
self.update_input(*command.attributes)
elif command.command == 'update_timer':
if command.attributes[0] == 'refresh_plot_time':
self.killTimer(self.timer)
self.refreshing_ouput_time = command.attributes[1]
self.timer = self.startTimer(self.refreshing_ouput_time)
elif command.attributes[0] =='timeout':
self.timeout_timer.setInterval(command.attributes[1])
elif command.command == 'update_filter':
self.filter = command.attributes[0]
# elif command.command == "move_Abs":
# self.set_option(dict(setpoint=command.attributes[0]))
def update_input(self, measurements):
self.input =self.model_class.convert_input(measurements)
def start_PID(self, input = None):
try:
for sig in self.move_done_signals:
sig.connect(self.move_done)
for sig in self.grab_done_signals:
sig.connect(self.det_done)
self.current_time = time.perf_counter()
self.status_sig.emit(["Update_Status", 'PID loop starting', 'log'])
while self.running:
#print('input: {}'.format(self.input))
## GRAB DATA FIRST AND WAIT ALL DETECTORS RETURNED
self.det_done_flag = False
self.det_done_datas = OrderedDict()
for ind_det, cmd in enumerate(self.detector_modules_commands):
cmd.emit(ThreadCommand("single",
[self.det_averaging[ind_det]]))
QtWidgets.QApplication.processEvents()
self.wait_for_det_done()
self.input = self.model_class.convert_input(self.det_done_datas)
## EXECUTE THE PID
output = self.pid(self.input)
## PROCESS THE OUTPUT IF NEEDED
if output is not None and self.output is not None:
if self.filter['enable']:
if np.abs(output-self.output) <= self.filter['value']:
self.output = output
else:
self.output += np.abs(self.filter['value'])*np.sign(output - self.output)
self.pid._last_output = self.output
else:
self.output = output
else:
self.output = output
## APPLY THE PID OUTPUT TO THE ACTUATORS
#print('output: {}'.format(self.output))
if self.output is None:
self.output = self.pid.setpoint
dt = time.perf_counter() - self.current_time
self.output_to_actuator = self.model_class.convert_output(self.output, dt, stab=True)
if not self.paused:
self.move_done_positions = OrderedDict()
for ind_mov, cmd in enumerate(self.move_modules_commands):
cmd.emit(ThreadCommand('move_Abs',[self.output_to_actuator[ind_mov]]))
QtWidgets.QApplication.processEvents()
self.wait_for_move_done()
self.current_time = time.perf_counter()
QtWidgets.QApplication.processEvents()
QThread.msleep(int(self.pid.sample_time*1000))
self.status_sig.emit(["Update_Status", 'PID loop exiting', 'log'])
for sig in self.move_done_signals:
sig.disconnect(self.move_done)
for sig in self.grab_done_signals:
sig.disconnect(self.det_done)
except Exception as e:
self.status_sig.emit(["Update_Status", str(e), 'log'])
pyqtSlot(OrderedDict) #OrderedDict(name=self.title,data0D=None,data1D=None,data2D=None)
def det_done(self,data):
"""
"""
try:
if data['name'] not in list(self.det_done_datas.keys()):
self.det_done_datas[data['name']]=data
if len(self.det_done_datas.items())==len(self.grab_done_signals):
self.det_done_flag=True
except Exception as e:
self.status_sig.emit(["Update_Status", str(e), 'log'])
pyqtSlot(str,float)
def move_done(self,name,position):
"""
| Update the move_done_positions attribute if needed.
| If position attribute is setted, for all move modules launched, update scan_read_positions with a [modulename, position] list.
============== ============ =================
**Parameters** **Type** **Description**
*name* string the module name
*position* float ???
============== ============ =================
"""
try:
if name not in list(self.move_done_positions.keys()):
self.move_done_positions[name]=position
if len(self.move_done_positions.items())==len(self.move_done_signals):
self.move_done_flag=True
except Exception as e:
self.status_sig.emit(["Update_Status",str(e),'log'])
def set_option(self,**option):
for key in option:
if hasattr(self.pid, key):
if key == 'sample_time':
setattr(self.pid, key, option[key]/1000)
else:
setattr(self.pid,key, option[key])
if key == 'setpoint' and not self.pid.auto_mode:
dt = time.perf_counter() - self.current_time
self.output = option[key]
self.output_to_actuator = self.model_class.convert_output(self.output, dt, stab=False)
for ind_move, cmd in enumerate(self.move_modules_commands):
cmd.emit(ThreadCommand('move_Abs', [self.output_to_actuator[ind_move]]))
self.current_time = time.perf_counter()
if key == 'output_limits':
self.output_limits = option[key]
def run_PID(self, last_value):
self.status_sig.emit(["Update_Status", 'Stabilization started', 'log'])
self.running = True
self.pid.set_auto_mode(True, last_value)
def pause_PID(self, pause_state):
if pause_state:
self.pid.set_auto_mode(False, self.output)
self.status_sig.emit(["Update_Status", 'Stabilization paused', 'log'])
else:
self.pid.set_auto_mode(True, self.output)
self.status_sig.emit(["Update_Status", 'Stabilization restarted from pause', 'log'])
self.paused = pause_state
def stop_PID(self):
self.running = False
self.status_sig.emit(["Update_Status", 'PID loop exiting', 'log'])
class Node:
def __init__(self):
rospy.init_node("joint_controller_node")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Setting up joint_controller_node")
self.pitch_pub = rospy.Publisher("roboclaw/pitch_vel",Twist,queue_size=10)
self.height_pub = rospy.Publisher("roboclaw/height_vel",Twist,queue_size=10)
self.joint_pub = rospy.Publisher('arm_joint_state',JointState,queue_size=10)
self.pitch_sub = rospy.Subscriber('roboclaw/pitch',JointState,self.pitch_pos_callback)
self.height_sub = rospy.Subscriber('roboclaw/height',JointState,self.height_pos_callback)
self.joint_status_pub = rospy.Subscriber('joint_states',JointState,self.joint_states_callback)
self.rate = rospy.Rate(5)
self.height_min_length = 15.71
self.height_max_length = 25.55
self.height_stroke_length = self.height_max_length - self.height_min_length
self.height_angle_offset = 369.69
self.pitch_min_length = 11.69
self.pitch_max_length = 17.6
self.pitch_stroke_length = self.pitch_max_length - self.pitch_min_length
self.start = False
self.h1_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.h2_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.p1_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.p2_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.MAX_ENC = 2047.0
self.quantize_div = 12.0
self.deadband = 11.0
self.h_params = {"m1":50.0,"m2":50.0}
self.p_params = {"m1":50.0,"m2":50.0}
self.p1_vals = [0.0,0.0,0.0,0.0,0.0]
self.p2_vals = [0.0,0.0,0.0,0.0,0.0]
self.h1_vals = [0.0,0.0,0.0,0.0,0.0]
self.h2_vals = [0.0,0.0,0.0,0.0,0.0]
self.height_pos = {"m1":0.0,"m2":0.0}
self.pitch_pos = {"m1":0.0,"m2":0.0}
def run(self):
rospy.loginfo("waiting for data")
while self.height_pos['m1'] == 0 and self.pitch_pos['m1'] == 0:
self.rate.sleep()
rospy.sleep(1)
rospy.loginfo("got data")
self.h1_pid.set_auto_mode(True,last_output=0.0)
self.h2_pid.set_auto_mode(True,last_output=0.0)
self.p1_pid.set_auto_mode(True,last_output=0.0)
self.p2_pid.set_auto_mode(True,last_output=0.0)
h1_old, h2_old, p1_old, p2_old = self.call_pids()
while not rospy.is_shutdown():
h1,h2,p1,p2 = self.call_pids()
h1,h2,p1,p2 = self.apply_deadband(h1,h2,p1,p2)
h1,h2,p1,p2 = self.safety_check(h1,h2,p1,p2,14,20)
if h1_old!=h1 or h2_old!=h2:
rospy.loginfo("pid output h1 %d, h2 %d", h1,h2)
if p1_old!=p1 or p2_old!=p2:
rospy.loginfo("pid output p1 %d, p2 %d", p1,p1)
h1_old = h1
h2_old = h2
p1_old = p1
p2_old = p2
self.publish_height_vel(-h1,h2)
self.publish_pitch_vel(-p1,-p2)
self.publish_arm_state()
self.rate.sleep()
def shutdown(self):
rospy.loginfo("Shutting down joint_controller_node")
def call_pids(self):
h1 = self.h1_pid(self.quantize(self.height_pos['m1']))
h2 = self.h2_pid(self.quantize(self.height_pos['m2']))
p1 = self.p1_pid(self.quantize(self.pitch_pos['m1']))
p2 = self.p2_pid(self.quantize(self.pitch_pos['m2']))
return h1,h2,p1,p2
def quantize(self, val):
return int(val/self.quantize_div)
def get_avg(self,lst):
return sum(lst) / len(lst)
def apply_deadband(self,h1,h2,p1,p2):
if abs(h1) < self.deadband: h1 = 0.0
if abs(h2) < self.deadband: h2 = 0.0
if abs(p1) < self.deadband: p1 = 0.0
if abs(p2) < self.deadband: p2 = 0.0
return h1,h2,p1,p2
def safety_check(self, h1, h2, p1, p2, thresh_p, thresh_o):
had_error = False
if abs(self.quantize(self.height_pos['m1'])-self.quantize(self.height_pos['m2'])) > thresh_p:
had_error = True
h1 = 0
h2 = 0
if abs(self.quantize(self.pitch_pos['m1'])-self.quantize(self.pitch_pos['m2'])) > thresh_p:
had_error = True
p1 = 0
p2 = 0
if abs(h1 - h2)>thresh_o:
had_error = True
h1 = 0
h2 = 0
if abs(p1 - p2)>thresh_o:
had_error = True
p1 = 0
p2 = 0
if had_error:
rospy.loginfo("ERROR!!!")
return h1, h2, p1, p2
def pitch_add_meas(self, p1, p2):
self.p1_vals.insert(0,p1)
self.p1_vals.pop()
self.p2_vals.insert(0,p2)
self.p2_vals.pop()
def height_add_meas(self, h1, h2):
self.h1_vals.insert(0,h1)
self.h1_vals.pop()
self.h2_vals.insert(0,h2)
self.h2_vals.pop()
def pitch_pos_callback(self, msg):
if msg.position[0] < 0.001 or msg.position[1] < 0.001:
self.pitch_add_meas(self.p1_vals[0],self.p2_vals[0])
else:
self.pitch_add_meas(msg.position[0],msg.position[1])
self.pitch_pos['m1'] = self.get_avg(self.p1_vals)
self.pitch_pos['m2'] = self.get_avg(self.p2_vals)
def height_pos_callback(self, msg):
if msg.position[0] < 0.001 or msg.position[1] < 0.001:
self.height_add_meas(self.h1_vals[0],self.h2_vals[0])
else:
self.height_add_meas(msg.position[0],msg.position[1])
self.height_pos['m1'] = self.get_avg(self.h1_vals)
self.height_pos['m2'] = self.get_avg(self.h2_vals)
def joint_states_callback(self,msg):
height_angle_target = msg.position[2]
height_inch_target = self.height_angle_to_dist(math.radians(self.height_angle_offset)-height_angle_target)
height_enc_target = self.height_inch_to_enc(height_inch_target,self.h_params['m1'])
height_enc_target = self.height_inch_to_enc(height_inch_target,self.h_params['m2'])
self.h1_pid.setpoint = self.quantize(height_enc_target)
self.h2_pid.setpoint = self.quantize(height_enc_target)
pitch_angle_target = msg.position[3]
pitch_inch_target = self.pitch_angle_to_dist(pitch_angle_target)
pitch_enc_target = self.pitch_inch_to_enc(pitch_inch_target,self.p_params['m1'])
pitch_enc_target = self.pitch_inch_to_enc(pitch_inch_target,self.p_params['m2'])
self.p1_pid.setpoint = self.quantize(pitch_enc_target)
self.p2_pid.setpoint = self.quantize(pitch_enc_target)
def publish_arm_state(self):
arm_msg = JointState()
arm_msg.header = Header()
arm_msg.header.stamp = rospy.Time.now()
arm_msg.name = ['base_to_lever_arm', 'lever_arm_to_digging_arm']
height_pos_inch = self.height_enc_to_inch(self.height_pos['m1'],self.h_params['m1'])
pitch_pos_inch = self.pitch_enc_to_inch(self.pitch_pos['m1'],self.p_params['m1'])
height_angle = math.radians(self.height_angle_offset) - self.height_dist_to_angle(height_pos_inch)
pitch_angle = self.pitch_dist_to_angle(pitch_pos_inch)
arm_msg.position = [height_angle, pitch_angle]
self.joint_pub.publish(arm_msg)
def publish_pitch_vel(self,m1,m2):
vel = Twist()
vel.linear.x = m1
vel.linear.y = m2
vel.linear.z = 0
self.pitch_pub.publish(vel)
def publish_height_vel(self,m1,m2):
vel = Twist()
vel.linear.x = m1
vel.linear.y = m2
vel.linear.z = 0
self.height_pub.publish(vel)
def height_enc_to_inch(self,enc,enc_min):
percent = float(enc-enc_min)/float(self.MAX_ENC-enc_min)
return self.height_min_length + self.height_stroke_length * percent
def height_inch_to_enc(self,inch,enc_min):
percent = float(inch-self.height_min_length)/float(self.height_stroke_length)
return float(enc_min) + float(self.MAX_ENC-enc_min) * percent
def pitch_enc_to_inch(self,enc,enc_min):
percent = float(enc-enc_min)/float(self.MAX_ENC-enc_min)
return self.pitch_min_length + self.pitch_stroke_length * percent
def pitch_inch_to_enc(self,inch,enc_min):
percent = float(inch-self.pitch_min_length)/float(self.pitch_stroke_length)
return enc_min + (self.MAX_ENC-enc_min) * percent
def height_angle_to_dist(self, theta):
return math.sqrt(793.5625 - 708*math.cos(math.radians(90)-theta))
def height_dist_to_angle(self, pos): #pos in inches
return math.radians(90 - math.degrees(math.acos((793.5625-(pos*pos))/708)))
def pitch_angle_to_dist(self, theta):
return math.sqrt(((30-11*math.cos(theta))-14.875)**2 + ((11*math.sin(theta)-3)-6.75)**2)
def pitch_dist_to_angle(self, pos):
return math.asin(0.05557*pos*math.sin(math.acos((pos*pos - 202.828125)/(22*pos*pos))))+math.radians(32.807)
class animate(Thread):
"""
Animate function.
Create a thread to call a function at a specified number of calls per second (CPS)
placing the results in a queue for consumption
:param function: the function (or method) that will be called
:type function: function or method
:param cps: Calls per second. The rate to call the provided function
:type cps: int
:param queueSize: The size of the queue used to record return results from function
:type queueSize: int
:param args: The arguments to pass to the function
:type args: tuple
:param kwargs: The keyworded arguments to pass to the function
:type kwargs: dict
To begin animation, call the start method. Example::
a = animate(function=func, cps=10)
a.start
..note:
The animate function uses a queue to pass results back. This allows for the
results to be consumed asynchronous. If the queue fills up though,
the function will no longer be called until space opens up in the queue.
"""
def __init__(self, function=None, cps=1, queueSize=100, *args, **kwargs):
Thread.__init__(self)
assert function, "You must supply a function to animate"
self._speed = cps
Kp = 5
Ki = 0.2
Kd = 0.05
self._pid = PID(
Kp,
Ki,
Kd,
setpoint=self._speed,
sample_time=0.1,
output_limits=(cps * 0.1, None),
)
self._function = function
self._args = args
self._kwargs = kwargs
self.fps = 0
self._running = True
self._queue = Queue(maxsize=queueSize)
self._event = Event()
self._forceEvent = Event()
@property
def empty(self):
"""
Return whether queue is empty.
:returns: True of empty, False if not
"""
return self._queue.empty()
@property
def full(self):
"""
Return whether queue is full.
:returns: True if full, False if not
"""
return self._queue.full()
@property
def qsize(self):
"""
Return estimate of queue size.
:returns: Estimate of queue size
"""
return self._queue.qsize()
def pause(self):
"""Temporarily pause calling the function."""
self._event.clear()
def restart(self):
"""
Restart Animation.
Restart calling the function (if paused). Otherwise this will have no effect.
"""
self._event.set()
def toggle(self):
"""Toggle calling the function from on to off or off to on."""
if self._event.isSet():
self._event.clear()
else:
self._event.set()
def stop(self):
"""Shut down the animate object including terminating its internal thread."""
self._running = False
self._event.set()
self.get()
self.join()
def clear(self):
"""Clear the queue of all curent values."""
self._emptyQueue()
def force(self, *args, **kwargs):
"""
Change the parameters that are passed to the function.
:param args: The positional arguments to provide to the animated function (optional)
:type args: tuple
:param kwargs: The keyword arguments to provide to teh animated function (optional)
:type kwargs: dict
..note:
force has the side effect of clearing any existing values within the queue.
"""
self._Force = True
# Set new arguments for animated function
self._args = args
self._kwargs = kwargs
# If needed, unblock run if the queue if full
try:
self._queue.get_nowait()
self._queue.task_done()
except Empty:
pass
# Wait until animate confirms force is finished
self._forceEvent.clear()
self._forceEvent.wait()
def get(self, wait=0):
"""
Get the current value from the results queue.
:param wait: Number of seconds to wait for a response. If zero (the default)
return immediately even if no value is currently available
:type wait: float
:returns: The current result. Type of value is determined by what the
animated function returns.
:rtype: object
..note:
Be careful with long wait values. The animate thread will block during
the wait. If you attempt to shut down the animate object (using stop),
it will not finish closing down until the wait has completed.
"""
try:
retval = self._queue.get(wait)
self._queue.task_done()
except Empty:
retval = None
return retval
def _emptyQueue(self):
while True:
try:
self._queue.get_nowait()
self._queue.task_done()
except Empty:
break
def _invoke(self, *args, **kwargs):
# Invoke function
if args and kwargs:
retval = self._function(*args, **kwargs)
elif args:
retval = self._function(*args)
elif kwargs:
retval = self._function(**kwargs)
else:
retval = self._function()
return retval
def run(self):
"""
Animate function.
Main loop for the animate thread
"""
correction = 0
loopTime = 1 / self._speed
tpf = 1 / self._speed
renderTimer = time.time()
renderCounter = 0
renderCounterLimit = 1
self._event.set()
self._Force = False
while self._running:
# Compute current FPS every 5 seconds
renderCounter += 1
if renderCounter > renderCounterLimit:
self.fps = renderCounter / (time.time() - renderTimer)
renderCounter = 0
renderCounterLimit = 10
renderTimer = time.time()
if not self._event.isSet():
# Disable pid while waiting to be activated
self._pid.set_auto_mode(False)
self._event.wait()
# Activated! Re-enable PID, begin processing
self._pid.set_auto_mode(True, last_output=correction)
fps = 1 / loopTime # Time per frame
correction = self._pid(fps)
startLoop = time.time()
tpf = (1 / (self._speed + correction)
if self._speed + correction > 0 else 0)
time.sleep(tpf)
# Disable PID while trying to place new render in queue
putStart = time.time()
if self._Force:
self._Force = False
self._emptyQueue()
# Must put value in queue before clearing the force Event
# so that the forced function receives the newly computed value
try:
retval = self._invoke(*self._args, **self._kwargs)
self._queue.put(retval)
except NoResult:
pass
self._forceEvent.set()
else:
try:
retval = self._invoke(*self._args, **self._kwargs)
try:
self._queue.put_nowait(retval)
except Full:
self._pid.set_auto_mode(False)
self._queue.put(retval)
self._pid.set_auto_mode(True, last_output=correction)
except NoResult:
pass
# Correct startLoop to account for time blocked by full queue
startLoop = startLoop + (time.time() - putStart)
loopTime = time.time() - startLoop
class Control:
"""PID controller with FIWARE interface"""
def __init__(self):
"""Initialization"""
# # use envirnment variables
# os.environ["CONFIG_FILE"] = "False"
# define parameters
self.params = {}
self.params['controller_name'] = os.getenv("CONTROLLER_NAME", 'PID_1')
self.params['type'] = "PID_Controller"
self.params['setpoint'] = float(os.getenv("SETPOINT", '293.15'))
# reverse mode can be activated by passing negative tunings to controller
self.params['Kp'] = float(os.getenv("KP", '1.0'))
self.params['Ki'] = float(os.getenv("KI", '0'))
self.params['Kd'] = float(os.getenv("KD", '0'))
self.params['lim_low'] = float(os.getenv("LIM_LOW", '0'))
self.params['lim_upper'] = float(os.getenv("LIM_UPPER", '100'))
self.params['pause_time'] = float(os.getenv("PAUSE_TIME", 0.2))
self.params['sensor_entity_name'] = os.getenv("SENSOR_ENTITY_NAME", '')
self.params['sensor_type'] = os.getenv("SENSOR_TYPE", None)
self.params['sensor_attr'] = os.getenv("SENSOR_ATTR", '')
self.params['actuator_entity_name'] = os.getenv(
"ACTUATOR_ENTITY_NAME", '')
self.params['actuator_type'] = os.getenv("ACTUATOR_TYPE", '')
self.params['actuator_command'] = os.getenv("ACTUATOR_COMMAND", '')
self.params['actuator_command_value'] = self.params[
'actuator_command'] + '_info'
self.params['service'] = os.getenv("FIWARE_SERVICE", '')
self.params['service_path'] = os.getenv("FIWARE_SERVICE_PATH", '')
self.params['cb_url'] = os.getenv("CB_URL", "http://localhost:1026")
# settings for security mode
self.security_mode = os.getenv("SECURITY_MODE",
'False').lower() in ('true', '1', 'yes')
self.params['token'] = (None, None)
# Get token from keycloak in security mode
if self.security_mode:
self.kp = KeycloakPython()
self.params['token'] = self.kp.get_access_token()
# Create simple pid instance
self.pid = PID(self.params['Kp'],
self.params['Ki'],
self.params['Kd'],
setpoint=self.params['setpoint'],
output_limits=(self.params['lim_low'],
self.params['lim_upper']))
# Additional parameters
self.auto_mode = True
self.u = None # control variable u
self.y_act = self.params[
'setpoint'] # set the initial measurement to set point
# Create the fiware header
fiware_header = FiwareHeader(service=self.params['service'],
service_path=self.params['service_path'])
# Create orion context broker client
self.ORION_CB = ContextBrokerClient(url=self.params['cb_url'],
fiware_header=fiware_header)
def create_entity(self):
"""Creates entitiy of PID controller in orion context broker"""
try:
self.ORION_CB.get_entity(entity_id=self.params['controller_name'],
entity_type=self.params['type'])
print('Entity name already assigned')
except requests.exceptions.HTTPError as err:
msg = err.args[0]
if "NOT FOUND" not in msg.upper():
raise # throw other errors except "entity not found"
print('[INFO]: Create new PID entity')
pid_entity = ContextEntity(id=f"{self.params['controller_name']}",
type=self.params['type'])
cb_attrs = []
for attr in ['Kp', 'Ki', 'Kd', 'lim_low', 'lim_upper', 'setpoint']:
cb_attrs.append(
NamedContextAttribute(name=attr,
type="Number",
value=self.params[attr]))
pid_entity.add_attributes(attrs=cb_attrs)
self.ORION_CB.post_entity(entity=pid_entity, update=True)
def update_params(self):
"""Read PID parameters of entity in context broker and updates PID control parameters"""
# read PID parameters from context broker
for attr in ['Kp', 'Ki', 'Kd', 'lim_low', 'lim_upper', 'setpoint']:
self.params[attr] = float(
self.ORION_CB.get_attribute_value(
entity_id=self.params['controller_name'],
entity_type=self.params['type'],
attr_name=attr))
# update PID parameters
self.pid.tunings = (self.params['Kp'], self.params['Ki'],
self.params['Kd'])
self.pid.output_limits = (self.params['lim_low'],
self.params['lim_upper'])
self.pid.setpoint = self.params['setpoint']
# read measured values from CB
try:
# read the current actuator value u (synchronize the value with the actuator)
self.u = self.ORION_CB.get_attribute_value(
entity_id=self.params['actuator_entity_name'],
entity_type=self.params['actuator_type'],
attr_name=self.params['actuator_command_value'])
if not isinstance(self.u, (int, float)):
self.u = None
# read the value of process variable y from sensor
y = self.ORION_CB.get_attribute_value(
entity_id=self.params['sensor_entity_name'],
entity_type=self.params['sensor_type'],
attr_name=self.params['sensor_attr'])
# set 0 if empty
if y == " ":
y = '0'
# convert to float
self.y_act = float(y)
except requests.exceptions.HTTPError as err:
msg = err.args[0]
if "NOT FOUND" not in msg.upper():
raise
self.auto_mode = False
print("Controller connection fails")
else:
# If no errors are raised
self.auto_mode = True
# Update the actual actuator value to allow warm star after interruption
self.pid.set_auto_mode(self.auto_mode, last_output=self.u)
def run(self):
"""Calculation of PID output"""
try:
if self.auto_mode: # if connection is good, auto_mode = True -> controller active
# calculate PID output
self.u = self.pid(self.y_act)
# build command
command = NamedCommand(name=self.params['actuator_command'],
value=round(self.u, 3))
self.ORION_CB.post_command(
entity_id=self.params['actuator_entity_name'],
entity_type=self.params['actuator_type'],
command=command)
except requests.exceptions.HTTPError as err:
msg = err.args[0]
if "NOT FOUND" not in msg.upper():
raise
self.auto_mode = False
print("Controller connection fails")
def update_token(self):
"""
Update the token if necessary. Write the latest token into the
header of CB client.
"""
token = self.kp.check_update_token_validity(
input_token=self.params['token'], min_valid_time=60)
if all(token): # if a valid token is returned
self.params['token'] = token
# Update the header with token
self.ORION_CB.headers.update(
{"Authorization": f"Bearer {self.params['token'][0]}"})
def control_loop(self):
"""The control loop"""
try:
while True:
# Update token if run in security mode
if self.security_mode:
self.update_token()
else:
pass
self.update_params()
self.run()
time.sleep(self.params['pause_time'])
finally:
print("control loop fails")
os.abort()
class PIDController(object):
"""
Convert ackermann_drive messages to carla VehicleCommand with a PID controller
"""
def __init__(self):
"""
Constructor
"""
self.frequency = 1000.0 #Hz
self.info = {}
# target values
self.info['target'] = {}
self.info['target']['speed'] = 0.
self.info['target']['accel'] = 0.
# current values
self.info['current'] = {}
self.info['current']['time_sec'] = time.time()
self.info['current']['speed'] = 0.
self.info['current']['accel'] = 0.
# control values
self.info['status'] = {}
self.info['status']['status'] = 'n/a'
self.info['status']['speed_control_activation_count'] = 0
self.info['status']['speed_control_accel_delta'] = 0.
self.info['status']['speed_control_accel_target'] = 0.
self.info['status']['accel_control_pedal_delta'] = 0.
self.info['status']['accel_control_pedal_target'] = 0.
self.info['status']['brake_upper_border'] = 0.
self.info['status']['throttle_lower_border'] = 0.
# restriction values
self.info['restrictions'] = {}
# control output
self.info['output'] = {}
self.info['output']['throttle'] = 0.
self.info['output']['brake'] = 1.0
# set initial maximum values
self.vehicle_info_updated()
# PID controller
# the controller has to run with the simulation time, not with real-time
# we might want to use a PID controller to reach the final target speed
self.speed_controller = PID(
Kp=1.5,
Ki=0.0,
Kd=0.5,
sample_time=0.001,
output_limits=(0.0, self.info['restrictions']['max_speed']))
self.accel_controller = PID(
Kp=1.1,
Ki=0.0,
Kd=0.1,
sample_time=0.001,
output_limits=(-self.info['restrictions']['max_decel'],
self.info['restrictions']['max_accel']))
@property
def throttle(self):
return self.info['output']['throttle']
@property
def brake(self):
return self.info['output']['brake']
def destroy(self):
"""
Function (override) to destroy this object.
Finish the PID controllers.
:return:
"""
self.speed_controller = None
self.accel_controller = None
def updatePIDParameters(self, sKp, sKi, sKd, aKp, aKi, aKd):
if (self.speed_controller.Kp, self.speed_controller.Ki,
self.speed_controller.Kd) != (sKp, sKi, sKd):
self.speed_controller.tunings = (sKp, sKi, sKd)
if (self.accel_controller.Kp, self.accel_controller.Ki,
self.accel_controller.Kd) != (aKp, aKi, aKd):
self.accel_controller.tunings = (aKp, aKi, aKd)
def vehicle_status_updated(self, velocity, pitch):
"""
Stores the car status for the next controller calculation
:return:
"""
# set target values
self.vehicle_status = {
'velocity': velocity,
'orientation': {
'pitch': pitch
}
}
def vehicle_info_updated(self, carMass=None):
"""
Stores the car info for the next controller calculation
:return:
"""
# set target values
self.vehicle_info = {}
if carMass is not None:
self.vehicle_info['mass'] = carMass
# calculate restrictions
self.info['restrictions']['max_speed'] = phys.get_vehicle_max_speed(
self.vehicle_info)
self.info['restrictions'][
'max_accel'] = phys.get_vehicle_max_acceleration(self.vehicle_info)
self.info['restrictions'][
'max_decel'] = phys.get_vehicle_max_deceleration(self.vehicle_info)
self.info['restrictions']['min_accel'] = 1.0
# clipping the pedal in both directions to the same range using the usual lower
# border: the max_accel to ensure the the pedal target is in symmetry to zero
self.info['restrictions']['max_pedal'] = min(
self.info['restrictions']['max_accel'],
self.info['restrictions']['max_decel'])
def set_target_speed(self, target_speed):
"""
set target speed
"""
if abs(target_speed) > self.info['restrictions']['max_speed']:
self.info['target']['speed'] = numpy.clip(
target_speed, -self.info['restrictions']['max_speed'],
self.info['restrictions']['max_speed'])
else:
self.info['target']['speed'] = target_speed
def set_target_accel(self, target_accel):
"""
set target accel
"""
epsilon = 0.1
# if speed is set to zero, then use max decel value
if self.info['target']['speed'] < epsilon:
self.info['target'][
'accel'] = -self.info['restrictions']['max_decel']
else:
self.info['target']['accel'] = numpy.clip(
target_accel, -self.info['restrictions']['max_decel'],
self.info['restrictions']['max_accel'])
def vehicle_control_cycle(self):
"""
Perform a vehicle control cycle and sends out CarlaEgoVehicleControl message
"""
# perform actual control
self.run_speed_control_loop()
self.run_accel_control_loop()
self.update_drive_vehicle_control_command()
def run_speed_control_loop(self):
"""
Run the PID control loop for the speed
The speed control is only activated if desired acceleration is moderate
otherwhise we try to follow the desired acceleration values
Reasoning behind:
An autonomous vehicle calculates a trajectory including position and velocities.
The ackermann drive is derived directly from that trajectory.
The acceleration and jerk values provided by the ackermann drive command
reflect already the speed profile of the trajectory.
It makes no sense to try to mimick this a-priori knowledge by the speed PID
controller.
=>
The speed controller is mainly responsible to keep the speed.
On expected speed changes, the speed control loop is disabled
"""
epsilon = 0.00001
target_accel_abs = abs(self.info['target']['accel'])
if target_accel_abs < self.info['restrictions']['min_accel']:
if self.info['status']['speed_control_activation_count'] < 2:
self.info['status']['speed_control_activation_count'] += 1
else:
if self.info['status']['speed_control_activation_count'] > 0:
self.info['status']['speed_control_activation_count'] -= 1
# set the auto_mode of the controller accordingly
self.speed_controller.set_auto_mode(
self.info['status']['speed_control_activation_count'] >= 2)
if self.speed_controller.auto_mode:
self.speed_controller.setpoint = self.info['target']['speed']
self.info['status'][
'speed_control_accel_target'] = self.speed_controller(
self.info['current']['speed'])
# clipping borders
clipping_lower_border = -target_accel_abs
clipping_upper_border = target_accel_abs
# per definition of ackermann drive: if zero, then use max value
if target_accel_abs < epsilon:
clipping_lower_border = -self.info['restrictions']['max_decel']
clipping_upper_border = self.info['restrictions']['max_accel']
self.info['status']['speed_control_accel_target'] = numpy.clip(
self.info['status']['speed_control_accel_target'],
clipping_lower_border, clipping_upper_border)
else:
self.info['status']['speed_control_accel_delta'] = 0.
self.info['status']['speed_control_accel_target'] = self.info[
'target']['accel']
def run_accel_control_loop(self):
"""
Run the PID control loop for the acceleration
"""
# setpoint of the acceleration controller is the output of the speed controller
self.accel_controller.setpoint = self.info['status'][
'speed_control_accel_target']
self.info['status'][
'accel_control_pedal_target'] = self.accel_controller(
self.info['current']['accel'])
# @todo: we might want to scale by making use of the the abs-jerk value
# If the jerk input is big, then the trajectory input expects already quick changes
# in the acceleration; to respect this we put an additional proportional factor on top
self.info['status']['accel_control_pedal_target'] = numpy.clip(
self.info['status']['accel_control_pedal_target'],
-self.info['restrictions']['max_pedal'],
self.info['restrictions']['max_pedal'])
def update_drive_vehicle_control_command(self):
"""
Apply the current speed_control_target value to throttle/brake commands
"""
# the driving impedance moves the 'zero' acceleration border
# Interpretation: To reach a zero acceleration the throttle has to pushed
# down for a certain amount
self.info['status'][
'throttle_lower_border'] = phys.get_vehicle_driving_impedance_acceleration(
self.vehicle_info, self.vehicle_status, False)
# the engine lay off acceleration defines the size of the coasting area
# Interpretation: The engine already prforms braking on its own;
# therefore pushing the brake is not required for small decelerations
self.info['status']['brake_upper_border'] = self.info['status']['throttle_lower_border'] + \
phys.get_vehicle_lay_off_engine_acceleration(self.vehicle_info)
if self.info['status']['accel_control_pedal_target'] > self.info[
'status']['throttle_lower_border']:
self.info['status']['status'] = "accelerating"
self.info['output']['brake'] = 0.0
# the value has to be normed to max_pedal
# be aware: is not required to take throttle_lower_border into the scaling factor,
# because that border is in reality a shift of the coordinate system
# the global maximum acceleration can practically not be reached anymore because of
# driving impedance
self.info['output']['throttle'] = (
(self.info['status']['accel_control_pedal_target'] -
self.info['status']['throttle_lower_border']) /
abs(self.info['restrictions']['max_pedal']))
elif self.info['status']['accel_control_pedal_target'] > self.info[
'status']['brake_upper_border']:
self.info['status']['status'] = "coasting"
# no control required
self.info['output']['brake'] = 0.0
self.info['output']['throttle'] = 0.0
else:
self.info['status']['status'] = "braking"
# braking required
self.info['output']['brake'] = (
(self.info['status']['brake_upper_border'] -
self.info['status']['accel_control_pedal_target']) /
abs(self.info['restrictions']['max_pedal']))
self.info['output']['throttle'] = 0.0
# finally clip the final control output (should actually never happen)
self.info['output']['brake'] = numpy.clip(self.info['output']['brake'],
0., 1.)
self.info['output']['throttle'] = numpy.clip(
self.info['output']['throttle'], 0., 1.)
def update_current_values(self):
"""
Function to update vehicle control current values.
we calculate the acceleration on ourselves, because we are interested only in
the acceleration in respect to the driving direction
In addition a small average filter is applied
:return:
"""
current_time_sec = time.time()
delta_time = current_time_sec - self.info['current']['time_sec']
current_speed = self.vehicle_status['velocity']
if delta_time > (1 / self.frequency):
delta_speed = current_speed - self.info['current']['speed']
current_accel = delta_speed / delta_time
# average filter
self.info['current']['accel'] = (
self.info['current']['accel'] * 4 + current_accel) / 5
self.info['current']['time_sec'] = current_time_sec
self.info['current']['speed'] = current_speed
def run(self):
self.update_current_values()
self.vehicle_control_cycle()
def run(alt, launchaoa, climbrate, maxaoa, heading, rate, speed):
conn = krpc.connect()
t = conn.mech_jeb.translatron
a = conn.mech_jeb.smart_ass
v = conn.space_center.active_vessel
c = v.control
a.interface_mode = conn.mech_jeb.SmartASSInterfaceMode.surface
a.autopilot_mode = conn.mech_jeb.SmartASSAutopilotMode.surface
a.force_pitch = True
a.force_yaw = True
a.force_roll = True
a.surface_heading = heading
a.surface_pitch = launchaoa
a.surface_roll = 0
a.update(False)
f = v.flight(v.orbit.body.reference_frame)
s_alt = conn.add_stream(getattr, f, 'mean_altitude')
s_alt.rate = rate
s_alt.start()
s_vs = conn.add_stream(getattr, f, 'vertical_speed')
s_vs.rate = rate
s_vs.start()
s_spd = conn.add_stream(getattr, f, 'speed')
s_spd.rate = rate
s_spd.start()
start_alt = s_alt()
alt_pid = PID(Kp=.10, Ki=.00, Kd=.00, sample_time=None, setpoint=alt)
vs_pid = PID(Kp=.15, Ki=.05, Kd=.01, sample_time=None, setpoint=0)
vs_pid.output_limits = (-1.0 * maxaoa, 1.0 * maxaoa)
spd_pid = PID(Kp=.20, Ki=.05, Kd=.25, sample_time=None, setpoint=speed)
spd_pid.output_limits = (0, 1.0)
climb_alts = [0, 14000, 18000, 20000]
climb_speeds = [climbrate, climbrate, 50, 0]
if start_alt > 1500:
start_alt = None
try:
s_alt.condition.acquire()
while True:
s_alt.wait()
c_alt = s_alt()
c_vs = s_vs()
c_spd = s_spd()
if start_alt:
if c_alt < (start_alt + 25):
print('{:8.1f} {:6.2f} {:6.2f}'.format(
c_alt, c_vs, a.surface_pitch))
continue
print('going closed loop')
start_alt = None
conn.space_center.active_vessel.control.gear = False
alt_pid.set_auto_mode(True, last_output=c_vs)
vs_pid.set_auto_mode(True, last_output=a.surface_pitch)
climbrate = numpy.interp(c_alt, climb_alts, climb_speeds)
alt_pid.output_limits = (-1.0 * climbrate, 1.0 * climbrate)
p_alt = alt_pid(c_alt)
vs_pid.setpoint = p_alt
p_vs = vs_pid(c_vs)
a.surface_pitch = p_vs
a.update(False)
if speed > 0:
c.throttle = spd_pid(c_spd)
print(
"alt: {:8.1f} avs: {:6.2f} dvs: {:5.2f} dpitch: {:5.2f} speed: {:5.2f}"
.format(c_alt, c_vs, p_alt, p_vs, c_spd))
finally:
s_alt.condition.release()
s_alt.remove()
