def nextNode(self):
last_node = self.current_solution.pop(0)
self.old_path_nodes.append(last_node)
self.start = self.current_solution[0]
#drone goes to next node
print("going for next node")
x = self.current_solution[0].x * 0.05
y = self.current_solution[0].y * 0.05 - 4.5
while self.distance(self.mav.drone_pose.pose.position.x,
self.mav.drone_pose.pose.position.y, x, y) > 0.2:
if rospy.is_shutdown():
break
pid_x = PID(3, 0, 10000000) #10 , 1000000
pid_y = PID(3, 0, 10000000) #10000000
pid_x.output_limits = pid_y.output_limits = (-0.8, 0.8)
pid_x.setpoint = x
pid_y.setpoint = y
vel_x = pid_x(self.mav.drone_pose.pose.position.x)
vel_y = pid_y(self.mav.drone_pose.pose.position.y)
self.mav.set_position_target(type_mask=MASK_VELOCITY,
x_velocity=vel_x,
y_velocity=vel_y,
z_velocity=1 -
self.mav.drone_pose.pose.position.z,
yaw_rate=0)
def follow_trajectory(self,trajectory,initial_height):
for point in trajectory:
point_x, point_y = self.cartesian_pose(point)
while self.distance(self.pose_data.pose.position.x, self.pose_data.pose.position.y, self.cartesian_pose(point)[0], self.cartesian_pose(point)[1]) > 0.2:
if rospy.is_shutdown():
break
pid_x = PID(2,0,10000000) #10 , 1000000
pid_y = PID(2,0,10000000)
pid_z = PID(2,0,10000000)
pid_x.output_limits = pid_y.output_limits = pid_z.output_limits = (-0.8, 0.8)
pid_x.setpoint = point_x
pid_y.setpoint = point_y
pid_z.setpoint = initial_height
vel_x = pid_x(self.pose_data.pose.position.x)
vel_y = pid_y(self.pose_data.pose.position.y)
vel_z = pid_z(self.mav.drone_pose.pose.position.z)
""" PROPORTIONAl CONTROL
if self.cartesian_pose(point)[0] - self.pose_data.pose.position.x < 0:
vel_x = self.cartesian_pose(point)[0] - self.pose_data.pose.position.x
vel_x = self.speed_multiplier*vel_x
if vel_x < -0.8:
vel_x = -0.8
elif self.cartesian_pose(point)[0] - self.pose_data.pose.position.x > 0:
vel_x = self.cartesian_pose(point)[0] - self.pose_data.pose.position.x
vel_x = self.speed_multiplier*vel_x
if vel_x > 0.8:
vel_x = 0.8
if self.cartesian_pose(point)[1] - self.pose_data.pose.position.y < 0:
vel_y = self.cartesian_pose(point)[1] - self.pose_data.pose.position.y
vel_y = self.speed_multiplier*vel_y
if vel_y < -0.8:
vel_y = -0.8
elif self.cartesian_pose(point)[1] - self.pose_data.pose.position.y > 0:
vel_y = self.cartesian_pose(point)[1] - self.pose_data.pose.position.y
vel_y = self.speed_multiplier*vel_y
if vel_y > 0.8:
vel_y = 0.8
vel_z = initial_height - self.mav.drone_pose.pose.position.z
"""
self.mav.set_position_target(type_mask=MASK_VELOCITY,
x_velocity=vel_x,
y_velocity=vel_y,
z_velocity=vel_z,
yaw_rate=-self.pose_data.pose.orientation.z)
def pid_init(q): # this is going to be either the main.py file or its own.
pid = PID(1,1,1) #init PID object with P, I and D parameters
pid.sample_time = .1 #how often the pid creates an output
while True: #pid loop
state = q.get() #get the current data for run and setpoint
if state.run:
pid.setpoint = state.setpoint() #set the setpoint based on GUI
current_temp = get_temp()
#do the serial read shit based on number of thermocouples, and
#current_temp = [1,1,1,1,1,1,1,1,1,1,1,1,1] #full array of TC values
state.temp = current_temp #update state variable
state.time_pid = time.time() #update time when this was taken
if state.controllocation: #if we are pulling data from top plate or substrate
output = pid(stat.mean(state.temp[6:1])) #get the PID output where 1:4 is a placeholder for the state varible controlling what TC are controlling the temp
else:
output = pid(stat.mean(state.temp[0:3]))
else: #if run=false
output = 0 #no input to SCR
#do gpio pwm shit to the low pass filter to control the power
state.power = output #return what the SCR should be getting
gpio_pwm(output)
q.put(state) #update temperature and state variables
q.task_done() #not sure what .task_done is. part of queue lib.
time.sleep(pid.sample_time) # slow down the PID loop
def pid_loop(state):
i = 0
pidhist = config.pid_hist_len * [0.]
temphist = config.temp_hist_len * [0.]
temperr = config.temp_hist_len * [0]
temp = 25.
lastsettemp = state['brewtemp']
lasttime = time()
sensor = MAX31855(SPI(board.SCK, MOSI=board.MOSI, MISO=board.MISO),
DigitalInOut(board.D5))
pid = PID(Kp=config.pidc_kp,
Ki=config.pidc_ki,
Kd=config.pidc_kd,
setpoint=state['brewtemp'],
sample_time=config.time_sample,
proportional_on_measurement=False,
output_limits=(-config.boundary, config.boundary))
while True:
try:
temp = sensor.temperature
del temperr[0]
temperr.append(0)
del temphist[0]
temphist.append(temp)
except RuntimeError:
del temperr[0]
temperr.append(1)
if sum(temperr) >= 5 * config.temp_hist_len:
print("Temperature sensor error!")
call(["killall", "python3"])
avgtemp = sum(temphist) / config.temp_hist_len
if avgtemp <= 0.9 * state['brewtemp']:
pid.tunings = (config.pidc_kp, config.pidc_ki, config.pidc_kd)
else:
pid.tunings = (config.pidw_kp, config.pidw_ki, config.pidw_kd)
if state['brewtemp'] != lastsettemp:
pid.setpoint = state['brewtemp']
lastsettemp = state['brewtemp']
pidout = pid(avgtemp)
pidhist.append(pidout)
del pidhist[0]
avgpid = sum(pidhist) / config.pid_hist_len
state['i'] = i
state['temp'] = temp
state['pterm'], state['iterm'], state['dterm'] = pid.components
state['avgtemp'] = round(avgtemp, 2)
state['pidval'] = round(pidout, 2)
state['avgpid'] = round(avgpid, 2)
sleeptime = lasttime + config.time_sample - time()
sleep(max(sleeptime, 0.))
i += 1
lasttime = time()
def maisch_pid(profile, INTERVAL):
pid = PID(10, 0, 0, setpoint=0)
start_time = time.time()
while True:
now = time.time()
diff = now - start_time
pid.setpoint = get_setpoint(profile, diff)
temp = fetch_temp()
value = pid(temp)
value = min(value, 100)
prop = value / 100.0
log(diff, temp, pid.setpoint, value=value)
if value <= 0:
off()
time.sleep(INTERVAL)
elif value == 100:
on()
time.sleep(INTERVAL)
else:
on()
time.sleep(prop * INTERVAL)
off()
time.sleep((1 - prop) * INTERVAL)
def controller(position):
if len(position.data) == 1:
#linear = 0
control.data = [999, 0]
print("No QR-code")
else:
distance = position.data[0]
angle = position.data[1]
pid_linear = PID(-0.5, -1, -0.05, setpoint=distance)
pid_linear.output_limits = (-15, 45)
pid_angular = PID(1.5, 2, 0.01, setpoint=angle)
pid_angular.output_limits = (-25, 25)
pid_linear.setpoint = SETPOINT #distance in mm
pid_angular.setpoint = 0 # zero angle needed
#if current_time - start_time > 0.5:
#start_time = time.time()
#last_time = start_time
pid_linear.sample_time = 0.01 # update every 0.001 seconds
pid_angular.sample_time = 0.01 # update every 0.001 seconds
linear = pid_linear(distance)
if linear > 0:
linear += 15
elif linear < 0:
linear -= 15
angular = pid_angular(angle)
if angular > 0:
angular += 5
elif angular < 0:
angular -= 5
control.data = [int(linear),
int(angular)] # final package for publishing
print(int(linear), int(angular))
pub.publish(control)
def get_pid():
print "pid is set"
pid = PID(0.001, 0.0000005, 0.0000005,setpoint=0.0)
pid.setpoint = 0.0
pid.sample_time = 0.01
pid.auto_mode = True
pid.output_limits = (-0.7, 0.7)
pid.proportional_on_measurement = True
return pid
def pid_init(
q, data_q): # this is going to be either the main.py file or its own.
pid = PID(3000, 0, 0) #init PID object with P, I and D parameters
pid.sample_time = .1 #how often the pid creates an output
pid.output_limits = (0, 65535)
data_state = data_state_class()
output = 0
pwm_object = gpio_pwm(0)
counter = 0
max_var = 0
while True: #pid loop
if not q.empty():
control_state = q.get(
block=True,
timeout=1) #get the current data for run and setpoint
#print("in pid while before data_state run",data_state.temp)
if control_state.run:
pid.setpoint = control_state.setpoint #set the setpoint based on GUI
current_temp_tup = get_temp(output, pwm_object, max_var, counter)
counter = counter + 1
#do the serial read shit based on number of thermocouples, and
current_temp = current_temp_tup[0]
pwm_object = current_temp_tup[1]
max_var = current_temp_tup[2]
#current_temp = [1,1,1,1,1,1,1,1,1,1,1,1,1] #full array of TC values
data_state.temp = current_temp #update data_state variable
data_state.time = time.time() #update time when this was taken
if control_state.controllocation: #if we are pulling data from top plate or substrate
output = pid(
stat.mean(data_state.temp[6:11])
) #get the PID output where 1:4 is a placeholder for the data_state varible controlling what TC are controlling the temp
else:
output = pid(stat.mean(data_state.temp[0:1]))
else: #if run=false
output = 0 #no input to SCR
#do gpio pwm shit to the low pass filter to control the power
data_state.power = output #return what the SCR should be getting
#print("output in pid" ,output)
pwm_object.duty_cycle = output
print(pwm_object.duty_cycle)
# print("temp",data_state.temp)
data_q.put(data_state) #update temperature and data_state variables
#q.taskdone() #not sure what .task_done is. part of queue lib.
time.sleep(pid.sample_time) # slow down the PID loop
def pid_ramping(t_ramp=3 * 60,
T=400,
max_time=5 * 60,
p=0.01,
i=0.01,
d=0.001):
time, temp_rb, volt_out = [], [], []
T_init = temp2.value
pid = PID(p, i, d, setpoint=T_init)
pid.sample_time = 0.01
pid.output_limits = (0, 3.5)
RE(bps.mv(volt_override, 0))
RE(bps.mv(heater_enable2, 1))
RE(bps.mv(heater_override2, 1))
time_start = ttime.time()
ramp_rate = (T - T_init) / t_ramp
while True:
dt = ttime.time() - time_start
if dt < t_ramp:
pid.setpoint = ramp_rate * dt + T_init
else:
pid.setpoint = T
print(pid.setpoint)
current_value = temp2.value
output = pid(current_value)
time.append(dt)
temp_rb.append(current_value)
volt_out.append(output)
RE(bps.mv(volt_override, output))
if dt > max_time:
break
# ttime.sleep(0.1)
RE(bps.mv(volt_override, 0))
RE(bps.mv(heater_enable2, 0))
RE(bps.mv(heater_override2, 0))
print('Time', t)
while noData == True:
Stop()
Serial()
Serial()
LB = dataList[0]
FB = dataList[1]
RB = dataList[2]
leftEnc = dataList[11]
rightEnc = dataList[13]
leftTicks = leftEnc - prev_leftEnc
rightTicks = rightEnc - prev_rightEnc
prev_leftEnc = leftEnc
prev_rightEnc = rightEnc
print(leftTicks, ' L Ticks R ', rightTicks)
leftMotor_PID.setpoint = LTPI # motor_PID setpoints set Ticks per interval for speed
rightMotor_PID.setpoint = RTPI
print(leftMotor_PID.setpoint, 'Setpoints', rightMotor_PID.setpoint)
PID_data['t'].append(t)
PID_data['LT'].append(leftTicks)
PID_data['RT'].append(rightTicks)
PID_data['LDC'].append(leftDutyCycle)
PID_data['RDC'].append(rightDutyCycle)
PID_data['A'].append(leftMotor_PID.setpoint)
PID_data['B'].append(rightMotor_PID.setpoint)
if LB == 0 or FB == 0 or RB == 0: # if bumpers are hit, Stop.
if Stopped == False:
Stop()
leftMotor_PID.setpoint = 0
rightMotor_PID.setpoint = 0
def main(args, stdin, stdout, stderr):
"Run a temperature/parameter scan and store output to a file."
# if args are passed as namespace, convert it to dict
try:
args = vars(args)
except:
pass
if not stdin.isatty():
# if a state table is passed on stdin, read it
print("reading states from stdin", file=stderr)
states = pd.read_csv(stdin, sep='\t')
else:
print("ERR: you need to pass the state table on stdin!", file=stderr)
exit(1)
## run the experiment
# open output file, do not overwrite!
with open(args['file_log'], 'x') as hand_log:
# variables tracking the expt schedule
vars_sched = ["clock", "watch", "state"]
# externally measured and derived variables
vars_measd = [
"T_int", "T_ext", "T_act", "P", "I", "D", "P_act", "vol",
"intensity", "T_amb", "H_amb", "dewpt", "air"
]
# compose and write header - states.head() are the setpoint variables
list_head = vars_sched + list(states.head()) + vars_measd
line_head = "\t".join(list_head)
hand_log.write(line_head + '\n')
hand_log.flush()
# now we're opening serial connections, which need to be closed cleanly on exit
try:
# init instruments
print("connecting...", file=stderr)
print("fluorospectrometer √", file=stderr)
amcu = auxmcu.AuxMCU(port=args['port_amcu'])
print("aux microcontroller √", file=stderr)
bath = isotemp6200.IsotempController(port=args['port_bath'])
print("temperature controller √", file=stderr)
pump = isco260D.ISCOController(port=args['port_pump'])
print("pressure controller √", file=stderr)
spec = rf5301.RF5301(port=args['port_spec'])
## hardware init
print("initializing...", file=stderr)
# set spec slits and gain
print("spec", end='', file=stderr)
# open the shutter, unless in auto
if not args["auto_shut"]:
while not spec.shutter(True):
pass
print('.', end='', file=stderr)
print(' √', file=stderr)
# initialize the filter wheels
print("auxiliary", file=stderr)
amcu.lamp(True)
#amcu.wheels_init()
# start bath circulator
print("bath", end='', file=stderr)
# init topside PID
pid = PID(1, 0, 85, setpoint=states.loc[states.index[0], "T_set"])
# windup preventer
pid.output_limits = (-20, 20)
# enter topside cal coefficients
bath.cal_ext.reset(*args["rtd_cal"])
# set controller gains
while not all(bath.pid('H', 0.8, 0, 0)):
pass
print('.', end='', file=stderr)
while not all(bath.pid('C', 1, 0, 0)):
pass
print('.', end='', file=stderr)
# set precision (number of decimal places)
while not bath.temp_prec(2):
pass
print('.', end='', file=stderr)
# set controller to listen to external RTD
while not bath.probe_ext(True):
pass
print('.', end='', file=stderr)
# finally, start the bath
while not bath.on():
bath.on(True)
print(' √', file=stderr)
# clear and start pump
print("pump", end='', file=stderr)
while not pump.remote():
pass
print('.', end='', file=stderr)
while not pump.clear():
pass
print('.', end='', file=stderr)
while not pump.run():
pass
print('.', end='', file=stderr)
# get initial volume
vol_start = None
while not vol_start:
vol_start = pump.vol_get()
print(" √ V0 = {} mL".format(vol_start), file=stderr)
# declare async queues
queue_bath = deque(maxlen=1)
queue_pump = deque(maxlen=1)
queue_spec = deque(maxlen=1)
queue_amcu = deque(maxlen=1)
# start polling threads
# all device instances have RLocks!
bath_free = threading.Event()
pump_free = threading.Event()
spec_free = threading.Event()
amcu_free = threading.Event()
[
event.set()
for event in (bath_free, pump_free, spec_free, amcu_free)
]
threading.Thread(name="pollbath",
target=poll,
args=(bath, bath_free, queue_bath, pid)).start()
threading.Thread(name="pollpump",
target=poll,
args=(pump, pump_free, queue_pump)).start()
threading.Thread(name="pollspec",
target=poll,
args=(spec, spec_free, queue_spec)).start()
threading.Thread(name="pollamcu",
target=poll,
args=(amcu, amcu_free, queue_amcu)).start()
## run experiment
# dict links setp vars to meas vars
#NTS 20210113 there's gotta be a better place for these mappings
setp2meas = {"T_set": "T_act", "P_set": "P_act"}
# start experiment timer (i.e. stopwatch)
time_start = time.time()
# iterate over test states
for state_num in range(states.shape[0]):
# make dicts for this state, the last, and the next
# takes about 1 ms
state_curr = states.iloc[state_num + 0].to_dict()
if state_num:
state_prev = states.iloc[state_num - 1].to_dict()
else:
# if first state
state_prev = {key: 0 for key in state_curr.keys()}
chg_prev = {key: True for key in state_curr.keys()}
if state_num < states.shape[0] - 1:
state_next = states.iloc[state_num + 1].to_dict()
else:
# if final state
state_next = {key: 0 for key in state_curr.keys()}
chg_next = {key: True for key in state_curr.keys()}
# which params have changed since previous state?
chg_prev = {
key: (state_curr[key] != state_prev[key])
for key in state_curr.keys()
}
chg_next = {
key: (state_curr[key] != state_next[key])
for key in state_curr.keys()
}
time_state = time.time() # mark time when state starts
waited = False # did the state have to wait for stability?
sat = False # did the dye have to relax?
readings = 0 # reset n counter
# status update
print("state {}/{}:".format(state_num + 1, states.shape[0]),
file=stderr)
print(state_curr, file=stderr)
# data logging loop
data_dict = {}
# init the data dict. Persistent the first time.
for dq in (queue_bath, queue_pump, queue_spec, queue_amcu):
while True:
# ensure that *something* gets popped out so the dict is complete
try:
data_dict.update(dq.popleft())
break
except:
pass
# set temp via topside PID
while not isclose(pid.setpoint, state_curr['T_set']):
print("setting temperature to {}˚C".format(
state_curr['T_set']),
file=stderr,
end=' ')
# pid object is picked up by poll()
pid.setpoint = state_curr['T_set']
print('√', file=stderr)
# set pressure persistently
pump_free.clear()
while not isclose(pump.press_set(), state_curr['P_set']):
print("setting pressure to {} bar".format(
state_curr['P_set']),
file=stderr,
end=' ')
if pump.press_set(state_curr['P_set']):
print('√', file=stderr)
pump_free.set()
## set the excitation wavelength
#while not isclose(spec.ex_wl(), state_curr['wl_ex']):
# print("setting excitation WL to {} nm".format(state_curr['wl_ex']), file=stderr, end=' ')
# if spec.ex_wl(state_curr['wl_ex']): print('√', file=stderr)
#
## set the emission wavelength
#while not isclose(spec.em_wl(), state_curr['wl_em']):
# print("setting emission WL to {} nm".format(state_curr['wl_em']), file=stderr, end=' ')
# if spec.ex_wl(state_curr['wl_em']): print('√', file=stderr)
# temporary WL setters
# Persistence implemented over cycles to improve efficiency;
# note that this checks the previous data row.
if not ((state_curr['wl_ex'] == data_dict['wl_ex']) and
(state_curr['wl_em'] == data_dict['wl_em'])):
#if not (isclose(spec.ex_wl(), state_curr['wl_ex']) and isclose(spec.em_wl(), state_curr['wl_em'])):
if (state_curr['wl_ex'] == 350) and (state_curr['wl_em']
== 428):
print("setting wavelengths for DPH",
file=stderr,
end=' ')
spec_free.clear()
spec.wl_set_dph()
spec_free.set()
print('√', file=stderr)
# set polarizers
if not ((state_curr['pol_ex'] == data_dict['pol_ex']) and
(state_curr['pol_em'] == data_dict['pol_em'])):
amcu_free.clear()
# take the line from other thread!
amcu.__ser__.send_break(duration=amcu.__ser__.timeout +
0.1)
time.sleep(amcu.__ser__.timeout + 0.1)
# excitation
if state_curr['pol_ex'] != data_dict['pol_ex']:
print("setting ex polarization to {}".format(
state_curr['pol_ex']),
file=stderr,
end=' ')
while not state_curr['pol_ex'] == amcu.ex(
state_curr['pol_ex']):
pass
print('√', file=stderr)
# emission
if state_curr['pol_em'] != data_dict['pol_em']:
print("setting em polarization to {}".format(
state_curr['pol_em']),
file=stderr,
end=' ')
while not state_curr['pol_em'] == amcu.em(
state_curr['pol_em']):
pass
print('√', file=stderr)
amcu_free.set()
# wait until wheel is solidly in position before writing any data
time.sleep(amcu.__ser__.timeout)
# init a log table for the state
trails = pd.DataFrame(columns=list_head)
while True:
time_cycle = time.time()
# DATA FIRST
for dq in (queue_bath, queue_pump, queue_spec, queue_amcu):
try:
data_dict.update(dq.popleft())
break
except:
pass
# add internal data
data_dict.update({
"clock": time.strftime("%Y%m%d %H%M%S"),
"watch": time.time() - time_start,
"state": state_num,
"T_set": state_curr["T_set"],
"P_set": state_curr["P_set"],
"msg": state_curr["msg"]
})
# SAFETY SECOND
# check for pressure system leak
if (data_dict["vol"] - vol_start) > args["vol_diff"]:
pump_free.clear()
pump.clear()
raise Exception("Pump has discharged > {} mL!".format(
args["vol_diff"]))
# control the air system
# if it's cold and air is off
if (data_dict['T_act'] <= data_dict["dewpt"] +
args["dew_tol"]) and not data_dict['air']:
pump_free.clear()
if waited: print(file=stderr)
print("\nturning air ON", file=stderr, end=' ')
while not pump.digital(0, 1):
pass
print("√", file=stderr)
pump_free.set()
data_dict['air'] = True
# if it's warm and the air is on
elif (data_dict['T_act'] >
(dewpt(data_dict['H_amb'], data_dict['T_amb']) +
args["dew_tol"])) and data_dict['air']:
# and air is on
pump_free.clear()
if waited: print(file=stderr)
print("\nturning air OFF", file=stderr, end=' ')
while not pump.digital(0, 0):
pass
print("√", file=stderr)
pump_free.set()
data_dict['air'] = False
# does the state change require equilibration?
vars_wait = [var for var in args["eqls"] if chg_prev[var]]
# if this in an empty dict, all(in_range.values()) will be true
in_range = {var: False for var in vars_wait}
# put new data into a trailing buffer
trails = trails.append(data_dict, ignore_index=True)
# cut the buffer down to the min equil time of the slowest variable
if vars_wait:
trails = trails[trails['watch'] >= (
trails['watch'].iloc[-1] -
max([min(args["eqls"][var])
for var in vars_wait]))]
# if the fluor reading has changed, write line to logfile
# this check takes about 0.1 ms
if (trails.shape[0]
== 1) or (trails["intensity"].iloc[-1] !=
trails["intensity"].iloc[-2]):
# write data to file
hand_log.write('\t'.join(
[str(data_dict[col]) for col in list_head]) + '\n')
hand_log.flush()
for var in vars_wait:
# if variable's timeout is past
if (time_cycle - time_state) >= max(args["eqls"][var]):
in_range[var] = True
# else, if min equilibration has elapsed
elif (time_cycle - time_state) >= min(
args["eqls"][var]):
#print("{}: {}\r".format(var, data_dict[var]), end='')
# see if the trace of the variable is in range
trace = trails[trails['watch'] >= (
trails['watch'].iloc[-1] -
min(args["eqls"][var]))][
setp2meas[var]].tolist()
#print(trace)
# and green- or redlight the variable as appropriate
in_range[var] = ((max(trace) <
(state_curr[var] +
args["tols"][setp2meas[var]]))
and
(min(trace) >
(state_curr[var] -
args["tols"][setp2meas[var]])))
# if all equilibrations have cleared
if all(in_range.values()):
if waited: print(file=stderr) # newline
waited = False
# open the shutter
if (not readings) and args["auto_shut"] and len(
vars_wait) and not sat:
spec_free.clear()
while not spec.shutter(True):
pass
spec_free.set()
time_open = time.time()
# let the dye relax after a long dark period (temp xsition)
if ("T_set" not in vars_wait) or (
(time.time() - args["shut_sit"]) >= time_open):
# take some readings
if (trails.shape[0]
== 1) or (trails["intensity"].iloc[-1] !=
trails["intensity"].iloc[-2]):
if readings:
print("reading {}: {} AU \r".format(
readings,
trails['intensity'].iloc[-1]),
end='',
file=stderr)
readings += 1
# break out of loop to next state
if (readings > args["n_read"]):
if args["auto_shut"] and any([
chg_next[var]
for var in args["eqls"].keys()
]):
spec_free.clear()
while not spec.shutter(False):
pass
spec_free.set()
print(file=stderr)
break
else:
print("dye relaxed for {}/{} s\r".format(
round(time.time() - time_open),
args["shut_sit"]),
end='',
file=stderr)
sat = True
# gotta get a newline in here somewhere!
else:
# what are we waiting for?
print("waiting {} s to get {} s of stability\r".format(
round(time.time() - time_state),
max([min(args["eqls"][var])
for var in vars_wait])),
end='',
file=stderr)
waited = True
# prescribed sleep
try:
time.sleep((args["cycle_time"] / 1000) -
(time.time() - (time_cycle)))
except:
pass
# shut down when done
pump_free.clear()
pump.digital(0, 0)
pump.pause()
pump.disconnect()
bath_free.clear()
bath.on(False)
bath.disconnect()
spec_free.clear()
spec.shutter(True)
spec.disconnect()
amcu_free.clear()
amcu.__ser__.send_break(duration=amcu.__ser__.timeout + 0.1)
time.sleep(amcu.__ser__.timeout + 0.1)
amcu.lamp(False)
amcu.ex('0')
amcu.em('0')
sys.exit(0)
except:
pump_free.clear()
pump.pause()
pump.digital(0, 0) # turn the air off!
spec_free.clear()
spec.ack()
spec.shutter(False)
amcu_free.clear()
amcu.__ser__.send_break(duration=amcu.__ser__.timeout + 0.1)
time.sleep(amcu.__ser__.timeout + 0.1)
amcu.lamp(False)
amcu.ex('0')
amcu.em('0')
traceback.print_exc()
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
def lander():
global first_run, notfound_count, found_count, marker_size, start_time
global p, i, d, pid
if first_run == 0:
print("First run of lander!!")
first_run = 1
start_time = time.time()
frame = cap.read()
frame = cv2.resize(frame, (horizontal_res, vertical_res))
frame_np = np.array(frame)
gray_img = cv2.cvtColor(frame_np, cv2.COLOR_BGR2GRAY)
ids = ''
corners, ids, rejected = aruco.detectMarkers(image=gray_img,
dictionary=aruco_dict,
parameters=parameters)
if vehicle.mode != 'LAND':
vehicle.mode = VehicleMode("LAND")
while vehicle.mode != 'LAND':
print('WAITING FOR DRONE TO ENTER LAND MODE')
time.sleep(1)
try:
if ids is not None and ids[0] == id_to_find:
ret = aruco.estimatePoseSingleMarkers(corners,
marker_size,
cameraMatrix=cameraMatrix,
distCoeffs=cameraDistortion)
(rvec, tvec) = (ret[0][0, 0, :], ret[1][0, 0, :])
x = '{:.2f}'.format(tvec[0])
y = '{:.2f}'.format(tvec[1])
z = '{:.2f}'.format(tvec[2])
y_sum = 0
x_sum = 0
x_sum = corners[0][0][0][0] + corners[0][0][1][0] + corners[0][0][
2][0] + corners[0][0][3][0]
y_sum = corners[0][0][0][1] + corners[0][0][1][1] + corners[0][0][
2][1] + corners[0][0][3][1]
x_avg = x_sum * .25
y_avg = y_sum * .25
x_ang = (x_avg - horizontal_res * .5) * (horizontal_fov /
horizontal_res)
y_ang = (y_avg - vertical_res * .5) * (vertical_fov / vertical_res)
########### PID CONTROL ##########
pid_x = PID(0.25, 0.5, 0.01, setpoint=x_ang)
pid_y = PID(0.25, 0.5, 0.01, setpoint=y_ang)
pid_x.setpoint = 0
pid_y.setpoint = 0
x_ang_control = pid_x(x_ang)
y_ang_control = pid_y(y_ang)
px, ix, dx = pid_x.components
py, iy, dy = pid_y.components
if vehicle.mode != 'LAND':
vehicle.mode = VehicleMode('LAND')
while vehicle.mode != 'LAND':
time.sleep(1)
print("------------------------")
print("Vehicle now in LAND mode")
print("------------------------")
#send_land_message(x_ang,y_ang)
send_land_message(-x_ang_control, -y_ang_control)
else:
#send_land_message(x_ang,y_ang)
send_land_message(-x_ang_control, -y_ang_control)
pass
print("X CENTER PIXEL: " + str(x_avg) + " Y CENTER PIXEL: " +
str(y_avg))
print("FOUND COUNT: " + str(found_count) + " NOTFOUND COUNT: " +
str(notfound_count))
print("MARKER POSITION: x=" + x + " y= " + y + " z=" + z)
found_count = found_count + 1
print("")
print(x_ang, y_ang)
print(-x_ang_control, -y_ang_control)
print("########################################################")
else:
notfound_count = notfound_count + 1
except Exception as e:
print('Target likely not found. Error: ' + str(e))
notfound_count = notfound_count + 1
def motorPositionCtrl(dest):
currentPosition = motor.getPosition()
if abs(dest - currentPosition) < 10:
return True
elif dest > currentPosition:
# motor forward
motor.setDirection(1)
pidPosition = PID(30, 0, 0)
pidSpeed = PID(0.2, 0, 0)
pidPosition.output_limits = (0, 500)
pidSpeed.output_limits = (0, 100)
pidPosition.setpoint = dest
pLast = motor.getPosition()
timeLast = time.perf_counter()
isStoppedCounter = 0
while True:
timeNow = time.perf_counter()
pNow = motor.getPosition()
## Position Loop
pLoopOut = pidPosition(pNow)
## Speed Loop
pidSpeed.setpoint = pLoopOut
dtime = timeNow - timeLast
speed = (pNow - pLast) / dtime
if (dtime > 5e-3):
sLoopOut = pidSpeed(speed)
motor.updatePWM(sLoopOut)
if pLast == pNow:
isStoppedCounter += 1
else:
isStoppedCounter = 0
if isStoppedCounter > 5:
motor.setDirection(0)
motor.updatePWM
break
timeLast = timeNow
pLast = pNow
print(motor.getPosition())
time.sleep(0.1)
else:
# motor backward
motor.setDirection(-1)
pidPosition = PID(15, 0, 0)
pidSpeed = PID(0.15, 0, 0)
pidPosition.output_limits = (0, 500)
pidSpeed.output_limits = (0, 100)
realCurrentPosition = motor.getPosition()
pidPosition.setpoint = 2 * realCurrentPosition - dest
pLast = realCurrentPosition
timeLast = time.perf_counter()
isStoppedCounter = 0
while True:
timeNow = time.perf_counter()
pNow = 2 * realCurrentPosition - motor.getPosition()
## Position Loop
pLoopOut = pidPosition(pNow)
## Speed Loop
pidSpeed.setpoint = pLoopOut
dtime = timeNow - timeLast
speed = (pNow - pLast) / dtime
if (dtime > 5e-3):
sLoopOut = pidSpeed(speed)
motor.updatePWM(sLoopOut)
if pLast == pNow:
isStoppedCounter += 1
else:
isStoppedCounter = 0
if isStoppedCounter > 5:
motor.setDirection(0)
motor.updatePWM
break
timeLast = timeNow
pLast = pNow
print(motor.getPosition())
#print(pNow)
time.sleep(0.1)
print(">> rtc={0}, 1wire={1}, lcd={2}".format(
realTime.getI2cErrors(), oneWireBus.getI2cErrors(),
lcd.getI2cErrors()))
# Set error flag for GUI
i2cError = True
else:
i2cError = False
#----------------------- Gas Controll Logic -----------------------
# Determine if the gas solenoid valve should be activated
if gasSwitchState == FIRE_SWITCH_AUTO:
# Automatic Gas Fire operation
if first_conversion_complete_flag:
# Update PID setpoint if needed
if not target_temperature == pid.setpoint:
pid.setpoint = target_temperature
# Translate current temp from sensors
current_temperature = sensor_temperature[0]
# Run algorithm
heatOutput = pid(current_temperature)
print(">> PID=" + str(heatOutput))
elif gasSwitchState == FIRE_SWITCH_ON:
heatOutput = True
else:
heatOutput = False
# Set indicator value
gasLedValue = heatOutput
def getCPUtemperature():
res = os.popen('vcgencmd measure_temp').readline()
temp = (res.replace("temp=", "").replace("'C\n", ""))
#print("temp is {0}".format(temp)) #Uncomment here for testing
return temp
PWM_FREQ = 25 #change this if fan stutters or else behave strange
fanPin = 21 # The pin ID, edit here to change it
GPIO.setmode(GPIO.BCM)
GPIO.setup(fanPin, GPIO.OUT)
GPIO.setwarnings(False)
myPWM = GPIO.PWM(fanPin, PWM_FREQ)
myPWM.start(40)
pid = PID(-2, -0.8, -2)
pid.setpoint = 45
output = 0
pid.auto_mode = True
# assume we have a system we want to control in controlled_system
pid.sample_time = 0.5 # update every 0.01 seconds
pid.output_limits = (0, 100) # output value will be between 0 and 10
while True:
input_temp = float(getCPUtemperature())
print(input_temp)
# compute new output from the PID according to the systems current value
output = int(pid(input_temp))
# feed the PID output to the system and get its current value
def mainThread(debug=False):
""" Controls the robot including joystick input, computer vision, line following, etc.
Arguments:
debug: (optional) log debugging data
"""
DC = ControllerUtils.DriveController(flip=[1, 0, 1, 0, 0, 0, 0, 0])
# Get Controller
gamepad = None
while (not gamepad) and execute['mainThread']:
time.sleep(5)
try:
gamepad = ControllerUtils.identifyController()
except Exception as e:
print(e)
newestImage = []
newestSensorState = {
'imu': {
'calibration': {
'sys': 2,
'gyro': 3,
'accel': 0,
'mag': 0
},
'gyro': {
'x': 0,
'y': 0,
'z': 0
},
'vel': {
'x': -0.01,
'y': 0.0,
'z': -0.29
}
},
'temp': 25
}
# Initalize PID controllers
Kp = 1
Kd = 0.1
Ki = 0.05
xRotPID = PID(Kp, Kd, Ki, setpoint=0)
yRotPID = PID(Kp, Kd, Ki, setpoint=0)
zRotPID = PID(Kp, Kd, Ki, setpoint=0)
mode = "user-control"
override = False
print("mode:", mode)
print("override:", override)
lastMsgTime = time.time()
minTime = 1.0 / 10.0
while execute['mainThread']:
while not mainQueue.empty():
recvMsg = mainQueue.get()
if recvMsg['tag'] == 'cam':
#newestImage = CommunicationUtils.decodeImage(recvMsg['data'])
pass
elif recvMsg['tag'] == 'sensor':
newestSensorState = recvMsg['data']
# Get Joystick Input
event = gamepad.read_one()
if event:
if (ControllerUtils.isOverrideCode(event, action="down")):
override = True
print("override:", override)
elif (ControllerUtils.isOverrideCode(event, action="up")
and override):
override = False
print("override:", override)
if (ControllerUtils.isStopCode(event)):
handlePacket(CommunicationUtils.packet("stateChange", "close"))
time.sleep(1)
stopAllThreads()
elif (ControllerUtils.isZeroMotorCode(event)):
handlePacket(
CommunicationUtils.packet("motorData",
DC.zeroMotors(),
metadata="drivetrain"))
elif (ControllerUtils.isStabilizeCode(event)):
if (mode != "stabilize"):
# Reset PID controllers
xRotPID.reset()
yRotPID.reset()
zRotPID.reset()
xRotPID.tunings = (Kp, Ki, Kd)
yRotPID.tunings = (Kp, Ki, Kd)
zRotPID.tunings = (Kp, Ki, Kd)
# Assuming the robot has been correctly calibrated, (0,0,0) should be upright
xRotPID.setpoint = 0
yRotPID.setpoint = 0
zRotPID.setpoint = 0
mode = "stabilize"
print("mode:", mode)
else:
mode = "user-control"
print("mode:", mode)
if (mode == "user-control" or override):
DC.updateState(event)
speeds = DC.calcThrust()
if (time.time() - lastMsgTime > minTime):
handlePacket(
CommunicationUtils.packet("motorData",
speeds,
metadata="drivetrain"))
lastMsgTime = time.time()
elif (mode == "stabilize" and not override):
xTgt = xRotPID(newestSensorState["imu"]["gyro"]["x"])
yTgt = yRotPID(newestSensorState["imu"]["gyro"]["y"])
zTgt = zRotPID(newestSensorState["imu"]["gyro"]["z"])
speeds = DC.calcPIDRot(xTgt, yTgt, zTgt)
if (time.time() - lastMsgTime > minTime):
handlePacket(
CommunicationUtils.packet("motorData",
speeds,
metadata="drivetrain"))
lastMsgTime = time.time()
pid.sample_time = 0.1
plt.figure(1)
plt.ion()
plt.show()
# timing functions
tm = np.zeros(101)
sleep_max = 0.101
start_time = time.time()
prev_time = start_time
# Simulate CSTR
for i in range(len(t)-1):
# PID control
pid.setpoint=sp[i]
u[i+1] = pid(T[i]) + u_bias
ts = [t[i],t[i+1]]
y = odeint(cstr,x0,ts,args=(u[i+1],Tf,Caf))
Ca[i+1] = y[-1][0]
T[i+1] = y[-1][1]
x0[0] = Ca[i+1]
x0[1] = T[i+1]
# plot results
plt.clf()
# Plot the results
plt.subplot(3,1,1)
plt.plot(t[0:i+1],u[0:i+1],'b--',linewidth=3)
plt.ylabel('Cooling T (K)')
print('Deploying fins')
extend_fins(vessel)
for part in vessel.parts.control_surfaces:
part.inverted = True
print('Calculating the suicide burn altitude')
while altitude() > target_alt:
mass = vessel.mass
v_speed = abs(velocity()[0])
target_alt = sb_alt(v_speed, mass, n_engines=n_engines)
#print(target_alt, v_speed)
#Block 6
print('Suicide burn')
vessel.control.throttle = 1
while altitude() > 100:
if altitude() < 300 and not vessel.control.gear:
vessel.control.gear = True
time.sleep(0.05)
print('Landing burn')
throttle_control.setpoint(0)
while altitude() > 26:
v_speed = velocity()[0]
output = throttle_control.update(altitude() + (v_speed * 6)) * 0.07
vessel.control.throttle = output
print(output, altitude(), v_speed)
print('The', name, 'has landed.')
vessel.control.throttle = 0
def pid_start(self,
func_target_x,
func_target_y,
repetitions,
timeout,
output_active=True,
subFrame_size=20,
spot_frame_size=1.5):
set_v = [50.0, 50.0, 50.0]
#set_v = self.controller.get_xyz_voltage()
if output_active:
self.set_voltages(*set_v)
time.sleep(5)
x_off, y_off = self.setup_AOI(subFrame_size, subFrame_size,
spot_frame_size)
# If centroid is not in the center of the subframe, adjust subframe
# but only do it after the distance is greater that these thresholds:
min_mov_x = self.fwhm_x / 3.0
min_mov_x = 10 if min_mov_x < 10 else min_mov_x
min_mov_y = self.fwhm_y / 3.0
min_mov_y = 10 if min_mov_y < 10 else min_mov_y
pid_x = PID(self.pid_P,
self.pid_I,
self.pid_D,
setpoint=func_target_x(0))
pid_y = PID(self.pid_P,
self.pid_I,
self.pid_D,
setpoint=func_target_y(0))
pid_x.output_limits = (-self.fwhm_x * 1.5, self.fwhm_x * 1.5
) # in pixels
pid_y.output_limits = (-self.fwhm_x * 1.5, self.fwhm_x * 1.5)
errs = np.zeros((repetitions, 2))
pos = np.zeros((repetitions, 2))
t = np.zeros(repetitions)
total_light = np.zeros(repetitions)
slit_light = np.zeros(repetitions)
# self.fgs_cam.set_AOI_size(40, 40)
# self.fgs_cam.set_AOI_position(672, 566)
# self.fgs_cam.set_clock(self.fgs_cam.get_clock_limits()[1])
# self.fgs_cam.set_exposure(5)
# self.fgs_cam.set_framerate(100)
self.get_bias_level_method2()
im = self.get_frame(bias_correct=True)
tot_weight = im.sum()
#time.sleep(1)
plt.imshow(im)
plt.show()
cx_ini, cy_ini = self.find_centroid(im,
x_offset=0,
y_offset=0,
convergence_limit=0.5,
max_iterations=10,
window_size=spot_frame_size)
cx_ini = int(round(cx_ini))
cy_ini = int(round(cy_ini))
slit_halfsize = int(round(self.fwhm_x * spot_frame_size / 2.0))
print("Slit half size: {}".format(slit_halfsize))
#Withot the image acquisition, and output off, the cycle below can be performed
# ~ 3000 times per second on my laptop. So speed is limited by frame rate
# Almost all of the delay (~ 0.035 sec in 100 repetitions) comes from the find_centroid method.
# The update_AOI, without a centroid param, also has a second find_centroid.
t_0 = time.time()
for i in range(0, repetitions):
t[i] = time.time()
img_fgs = self.get_frame(bias_correct=True)
total_light[i] = img_fgs.sum()
slit_im = img_fgs[cx_ini - slit_halfsize:cx_ini + slit_halfsize,
cy_ini - slit_halfsize:cy_ini + slit_halfsize]
slit_light[i] = np.sum(slit_im)
if img_fgs.sum() < (tot_weight / 2.0):
print("Low weight")
x_off, y_off = self.setup_AOI(subFrame_size, subFrame_size,
spot_frame_size)
img_fgs = self.get_frame(bias_correct=True)
cx, cy = self.find_centroid(img_fgs,
x_offset=x_off,
y_offset=y_off,
convergence_limit=0.5,
max_iterations=10,
window_size=spot_frame_size)
errs[i, :] = [
cx - func_target_x(t[i] - t_0), cy - func_target_y(t[i] - t_0)
]
pos[i, :] = [cx, cy]
if output_active:
pid_x.setpoint = func_target_x(t[i] - t_0)
pid_y.setpoint = func_target_y(t[i] - t_0)
x_control = pid_x(cx)
y_control = pid_y(cy)
delta_voltages = self.calculate_piezo_offset(
x_control, y_control)
set_v[0] = set_v[0] + delta_voltages[0]
set_v[1] = set_v[1] + delta_voltages[1]
set_v[2] = set_v[2] + delta_voltages[2]
self.set_voltages(*set_v)
centroid = [cx - x_off, cy - y_off]
x_off, y_off, _ = self.update_AOI(image=img_fgs,
min_diff_threshold=np.max(
[min_mov_x, min_mov_y]),
centroid=centroid,
spot_frame_size=spot_frame_size)
if (t[i] - t_0) > timeout:
break
norm_t = t - t_0
pos = pos[:i - 1, :]
errs = errs[:i - 1, :]
norm_t = norm_t[:i - 1]
return pos, errs, norm_t, total_light, slit_light
while (robot.step(timestep) != -1): led_state = int(robot.getTime()) % 2 front_left_led.set(led_state) front_right_led.set(int(not(led_state))) roll = imu.getRollPitchYaw()[0] + M_PI / 2.0 pitch = imu.getRollPitchYaw()[1] yaw = compass.getValues()[0] roll_acceleration = gyro.getValues()[0] pitch_acceleration = gyro.getValues()[1] xGPS = gps.getValues()[2] yGPS = gps.getValues()[0] zGPS = gps.getValues()[1] vertical_input = throttlePID(zGPS) yaw_input = yawPID(yaw) rollPID.setpoint = targetX pitchPID.setpoint = targetY roll_input = float(params["k_roll_p"]) * roll + roll_acceleration + rollPID(xGPS) pitch_input = float(params["k_pitch_p"]) * pitch - pitch_acceleration + pitchPID(-yGPS) front_left_motor_input = float(params["k_vertical_thrust"]) + vertical_input - roll_input - pitch_input + yaw_input front_right_motor_input = float(params["k_vertical_thrust"]) + vertical_input + roll_input - pitch_input - yaw_input rear_left_motor_input = float(params["k_vertical_thrust"]) + vertical_input - roll_input + pitch_input - yaw_input rear_right_motor_input = float(params["k_vertical_thrust"]) + vertical_input + roll_input + pitch_input + yaw_input mavic2proHelper.motorsSpeed(robot, front_left_motor_input, -front_right_motor_input, -rear_left_motor_input, rear_right_motor_input)
# Create PID controller
pid = PID(Kp=Kc,Ki=Kc/tauI,Kd=Kc*tauD,\
setpoint=23,sample_time=1.0,output_limits=(0,100))
n = 600
tm = np.linspace(0, n - 1, n) # Time values
T1 = np.zeros(n)
Q1 = np.zeros(n)
# step setpoint from 23.0 to 60.0 degC
SP1 = np.ones(n) * 23.0
SP1[10:] = 60.0
iae = 0.0
print('Time OP PV SP')
for i in range(n):
pid.setpoint = SP1[i]
T1[i] = lab.T1
iae += np.abs(SP1[i] - T1[i])
Q1[i] = pid(T1[i]) # PID control
lab.Q1(Q1[i])
print(i, round(Q1[i], 2), T1[i], pid.setpoint)
time.sleep(pid.sample_time) # wait 1 sec
lab.close()
# Save data file
data = np.vstack((tm, Q1, T1, SP1)).T
np.savetxt('PID_control_'+ci+'.csv',data,delimiter=',',\
header='Time,Q1,T1,SP1',comments='')
# Create Figure
plt.figure(xi, figsize=(10, 7))
# pid_tilt.output_limits = (-30, 90)
# rate = rospy.Rate(50) # 50hz
pid_pan.sample_time = 0.02
# pid_tilt.sample_time = 0.02
counter = 0
while not rospy.is_shutdown():
if counter < len(y):
ref = y[counter]
counter += 1
error = ref - v_pan
# print("Error: ", error)
if abs(error) < 3:
v_pan = ref
pid_pan.setpoint = ref
control_pan = pid_pan(v_pan)
# print("Control: ", control_pan)
# print("Control: ",int(control_pan))
# control_tilt = pid_tilt(v_tilt)
if abs(error) >= 3:
x.pan_speed(int(control_pan))
pub.publish(ref)
else:
x.pan_speed(0)
pub.publish(ref)
# x.tilt_speed(int(control_tilt))
# pid_tilt.setpoint = tilt_angle
vrep.simx_opmode_oneshot_wait)
ret, handler_helice = vrep.simxGetObjectHandle(clientID, "Helice",
vrep.simx_opmode_oneshot_wait)
# Variaveis para plot do gráfico
list_setpoint = []
list_angle = []
list_time = []
# Configura objeto para o controle PID
pid = PID(Kp=0.8, Ki=1.5, Kd=1, setpoint=90)
pid.sample_time = 0.05
pid.output_limits = (0, 10)
# Loop de Controle
pid.setpoint = 90
start = millis()
while (millis() - start < 20000):
# Realiza coleta do tempo atual (ms)
cur_time = millis()
# Realiza leitura do ângulo da junta e converte para graus, com precisão decimal de 2 casas
ret, ang = vrep.simxGetJointPosition(clientID, handler_junta,
vrep.simx_opmode_oneshot_wait)
ang = round(math.degrees(ang), 2)
# Atualiza PID com ângulo atual como entrada
output = pid(ang)
output = round(output, 2)
print("Output", output)
def run_temperature_controller():
globals.log('info', 'Temperature Controller Started')
#Initialize Pid
pid = PID(Kp=aggressive_kp,
Ki=aggressive_ki,
Kd=aggressive_kd,
setpoint=globals.target_barrel_temp,
output_limits=(0, 100),
auto_mode=True,
proportional_on_measurement=True
) # See documentation on proportial on measurement flag
#Initialize temperature offset
temperature_offset = 0
if (simulated_mode == False):
#Initialize Fan
pwm = PWM.get_platform_pwm()
pwm.start(fan1_pin, globals.fan_speed, fan1_frequency)
#if a second fan is defined, initialize that as well
if (fan2_pin):
pwm.start(fan2_pin, globals.fan_speed, fan2_frequency)
#if lid switch pin is defined, set it up as input
if (lid_switch_pin):
gpio_platform.setup(lid_switch_pin, GPIO.IN)
#initialize thermocouples
TC1 = MAX6675.MAX6675(CLK_pin, TC1_CS_pin, DO_pin)
TC2 = MAX6675.MAX6675(CLK_pin, TC2_CS_pin, DO_pin)
TC3 = MAX6675.MAX6675(CLK_pin, TC3_CS_pin, DO_pin)
TC4 = MAX6675.MAX6675(CLK_pin, TC4_CS_pin, DO_pin)
TC5 = MAX6675.MAX6675(CLK_pin, TC5_CS_pin, DO_pin)
#Initialize Temperatures
#read sensors
TC1_temp = TC1.readTempC()
time.sleep(0.1)
TC2_temp = TC2.readTempC()
time.sleep(0.1)
TC3_temp = TC3.readTempC()
time.sleep(0.1)
TC4_temp = TC4.readTempC()
time.sleep(0.1)
TC5_temp = TC5.readTempC()
else:
#if in simulated mode, set to 20 degrees baseline
TC1_temp = 20
TC2_temp = 20
TC3_temp = 20
TC4_temp = 20
TC5_temp = 20
#and create empty pwm object
pwm = None
#set variables used in the loop
TC5_temp_last = TC5_temp
temp_weighted_avg_last = ((TC1_temp + TC2_temp + TC3_temp + TC4_temp) / 4)
#keep looping until instructed otherwise
while (globals.stop_threads == False):
#update setpoint if it differs
if (pid.setpoint != globals.target_barrel_temp):
globals.log(
'debug',
'Temperature Controller - PID Setpoint changed to: {}'.format(
globals.target_barrel_temp))
pid.setpoint = globals.target_barrel_temp
#read sensors and if in simulated mode, calculate based on fan speed
if (simulated_mode == False):
TC1_temp = TC1.readTempC()
time.sleep(0.1)
TC2_temp = TC2.readTempC()
time.sleep(0.1)
TC3_temp = TC3.readTempC()
time.sleep(0.1)
TC4_temp = TC4.readTempC()
time.sleep(0.1)
TC5_temp = TC5.readTempC()
else:
simulated_temp = simulate_temperature(TC1_temp)
TC1_temp = TC2_temp = TC3_temp = TC4_temp = simulated_temp
TC5_temp += 0.01
#Calculations
temp_weighted_avg = (
(TC1_temp + TC2_temp + TC3_temp + TC4_temp) / 4
) + globals.temperature_offset # + 45 From SmokeyTheBarrel: temperature compensation between outside and center of the barrel
temp_weighted_avg_last = (2 * temp_weighted_avg_last +
temp_weighted_avg) / 3
globals.current_barrel_temp = temp_weighted_avg_last
temperature_gap = globals.target_barrel_temp - temp_weighted_avg_last
globals.current_temp_gap = temperature_gap
TC5_temp_last = (2 * TC5_temp_last + TC5_temp) / 3
globals.current_meat_temp = TC5_temp_last
#use conservative pid profile for small temperature differences
if (temperature_gap >= -10 and temperature_gap <= 10):
set_pid_profile(pid, 'conservative')
#otherwise use the aggresive profile
else:
set_pid_profile(pid, 'aggressive')
#if lid switch pin is defined, check its state. If the lid is open, kill the fan. If the pin is not defined calculate and set the speed as required
if (lid_switch_pin):
lid_state = gpio_platform.input(lid_switch_pin)
if (lid_state == GPIO.HIGH):
set_fan_speed(pwm, 0)
globals.lid_open = True
else:
#calculate new fan speed and set the fan speed
#only do the pid calculation when the lid is closed to prevent confusing the pid
pid_output = pid(temp_weighted_avg_last)
set_fan_speed(pwm, pid_output)
globals.lid_open = False
else:
#calculate new fan speed and set the fan speed
pid_output = pid(temp_weighted_avg_last)
set_fan_speed(pwm, pid_output)
#if the calibrate option has been selected
if (globals.calibrate_temperature == True):
globals.log(
'info',
'Temperature Calibration Started, stopping PID controller')
#stop the pid
pid.auto_mode = False
#run the calibrate function
globals.temperature_offset = calibrate_temperature_offset(
TC1, TC2, TC3, TC4, TC5, pwm)
#reenable the pid
pid.auto_mode = True
globals.log(
'info',
'Temperature Calibration Finished, PID controller started')
#sleep for a second
time.sleep(1)
###### End of loop
if (simulated_mode == False):
pwm.stop(fan1_pin)
if (fan2_pin):
pwm.stop(fan2_pin)
globals.log('info', 'Temperature Controller Stopped')
360) # something might be wrong here
robot_distance_to_target = distance(robot_center_position,
current_target_position)
pid_left_right.tunings = (
0.3, 0.001, 0.01
) # tuning depends on the robot configuration, vision delay, etc
if math.fabs(robot_heading_to_target -
robot_heading) > math.radians(5): # I-term anti windup
pid_left_right.Ki = 0
pid_left_right.output_limits = (
-0.3, 0.3
) # tuning depends on the robot configuration, vision delay, etc
pid_left_right.sample_time = 0.01 # update every 0.01 seconds
pid_left_right.setpoint = robot_heading_to_target
ch1 = pid_left_right(robot_heading) * 100 + 100 # steering
pid_speed.tunings = (0.01, 0.0001, 0
) # depends on the robot configuration
if robot_distance_to_target > 5: # I-term anti windup
pid_speed.Ki = 0
pid_speed.output_limits = (-0.3, 0.3
) # depends on the robot configuration
pid_speed.sample_time = 0.01 # update every 0.01 seconds
pid_speed.setpoint = 0
if math.fabs(robot_heading_to_target - robot_heading) < math.radians(
max_forward_heading
): # don't drive forward until error in heading is less than max_forward_heading
ch2 = -pid_speed(robot_distance_to_target) * 100 + 100
pid.output_limits = (0, 100)
start_time = time.time()
last_time = start_time
# keep track of values for plotting
setpoint, y, x = [], [], []
while time.time() - start_time < 10:
current_time = time.time()
dt = current_time - last_time
power = pid(water_temp)
water_temp = boiler.update(power, dt)
x += [current_time - start_time]
y += [water_temp]
setpoint += [pid.setpoint]
if current_time - start_time > 1:
pid.setpoint = 100
last_time = current_time
plt.plot(x, y, label='measured')
plt.plot(x, setpoint, label='target')
plt.xlabel('time')
plt.ylabel('temperature')
plt.legend()
plt.show()
import json
import pyvesc
from pyvesc.messages import GetValues, SetRPM, SetCurrent, SetRotorPositionMode, GetRotorPosition,SetDutyCycle,GetValues
import serial
import time
from simple_pid import PID
import time
import paho.mqtt.client as mqtt
pid = PID(1, 0.1, 0.05, setpoint=1)
pid.sample_time = 0.001 # update every 0.01 seconds
pid.setpoint = 0
normal_tuning=(700,1000,0.000000001)
#break_tuning=(100,500,0)
pid.tunings = normal_tuning #(700, 1000, 0.000000001)
pid.output_limits = (-100000, 100000) # output value will be between 0 and 10
aca=0
motor_speed=0
motor_tach=0
force_break=0
# Set your serial port here (either /dev/ttyX or COMX)
serialport = '/dev/ttyACM2'
out_json={"MOTOR":"RIGTH","SPEED":0,"TACHOMETER":0}
def on_connect(client, userdata, flags, rc):
print("Connected with result code {0}".format(str(rc))) # Print result of connection attempt
client.subscribe("actions") # Subscribe to the topic “digitest/test1”, receive any messages published on it
def _control_thread(self):
"""
Calculates ideal distance moved since target velocity changed
Uses pulse width modulation to match ideal distance moved
"""
pid = PID(10000, 0, 100, setpoint=0)
pid.output_limits = (-1, 1)
frequency = config.LA_CONTROL_FREQUENCY # [Hz]
period = 1 / frequency
time_target_velocity_changed = time.time()
target_velocity = self.target_velocity
duty_cycle = self._calculate_duty_cycle_from_speed(
abs(target_velocity))
enabled = False
self.direction = 0
while True:
if target_velocity != self.target_velocity + self.target_velocity_correction_total:
target_velocity = self.target_velocity + self.target_velocity_correction_total
duty_cycle = self._calculate_duty_cycle_from_speed(
abs(target_velocity))
self.actual_distance_since_target_velocity_changed = 0
time_target_velocity_changed = time.time()
pid.setpoint = target_velocity
actual_velocity = self.actual_distance_since_target_velocity_changed / (
time.time() - time_target_velocity_changed)
duty_cycle = pid(actual_velocity)
## if time.time() % 1 >= 0.97 and self.side == "R":
## print("actual", actual_velocity, "target", target_velocity)
## print("length", self.length)
if -0.5 < duty_cycle < 0.5:
if enabled:
i2c.digitalWrite(self.enable_pin, False)
enabled = False
time.sleep(period)
else:
if duty_cycle > 0 and not self.direction == 1:
#self._print("extending %s" % duty_cycle)
i2c.digitalWrite(self.extend_pin, True)
i2c.digitalWrite(self.retract_pin, False)
self.direction = 1
elif duty_cycle < 0 and not self.direction == -1:
#self._print("retracting %s" % duty_cycle)
i2c.digitalWrite(self.extend_pin, False)
i2c.digitalWrite(self.retract_pin, True)
self.direction = -1
duty_cycle = abs(duty_cycle)
if duty_cycle == 1:
if not enabled:
i2c.digitalWrite(self.enable_pin, True)
enabled = True
time.sleep(period)
else:
i2c.digitalWrite(self.enable_pin, True)
enabled = True
time.sleep(period * duty_cycle)
i2c.digitalWrite(self.enable_pin, False)
enabled = False
time.sleep(period * (1 - duty_cycle))
pid_pan.output_limits = (-167.0, 167.0)
pid_tilt.output_limits = (-29, 90)
x.set_speed_mode()
x.wait()
# rate = rospy.Rate(50) # 50hz
pid_pan.sample_time = 0.02
pid_tilt.sample_time = 0.02
while not rospy.is_shutdown():
ref = 45
error = ref - v_pan
if abs(error) < 3:
v_pan = ref
pid_pan.setpoint = ref
control_pan = pid_pan(v_pan)
# print(control_pan)
pid_tilt.setpoint = 0
control_tilt = pid_tilt(v_tilt)
x.pan_angle(control_pan)
v_pan = position[0] * 180 / np.pi
x.tilt_angle(control_tilt)
v_tilt = position[1] * 180 / np.pi
x.stream.close()
