def take_off_us(set_velocity_serv,
navigate_serv,
z=1,
speed=0.3,
tolerance=0.08):
z_now = 0
x_now = 0
y_now = 0
def callback(data):
global z_now
z_now = data.range
# print(data.range)
# rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
rospy.Subscriber("/ultrasound", Range, callback)
def callback_2(data):
global x_now, y_now
if data.x == -1:
x_now = 160
y_now = 120
else:
x_now = data.x
y_now = data.y
# print(data)
# rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
rospy.Subscriber("/point_vision", Point, callback_2)
pid_x = PID(0.6, 0, 0.0, setpoint=0)
pid_y = PID(0.6, 0, 0.0, setpoint=0)
pid_z = PID(0.6, 0, 0.0, setpoint=0)
pid_x.output_limits = (-0.2, 0.2)
pid_y.output_limits = (-0.2, 0.2)
pid_z.output_limits = (-0.2, 0.2)
print('arming')
# arming_serv(True)
navigate_serv(z=0, yaw=0, speed=0.2, frame_id='fcu_horiz', auto_arm=True)
rospy.sleep(1)
while True:
global z_now, x_now, y_now
con_x = pid_x((160 - x_now) / 160)
con_y = pid_y((y_now - 120) / 120)
# con_z = pid_z(z_now-z)
print(con_x, con_y)
set_velocity_serv(vx=((x_now - 160) / 160) * 0.2,
vy=((120 - y_now) / 120) * 0.2,
vz=speed,
frame_id='fcu_horiz')
# print(z_now)
if abs((z_now - z)) < tolerance:
break
rospy.sleep(0.3)
set_velocity_serv(vx=0.0, vy=0.0, vz=0.0, frame_id='fcu_horiz')
set_velocity_serv(vx=0.0, vy=0.0, vz=0.0, frame_id='fcu_horiz')
def run(self):
dt = 0.05
kp = 0.8
ki = 2.5
kd = 0.01
pid_left = PID(kp, ki, kd, setpoint=0)
pid_right = PID(kp, ki, kd, setpoint=0)
pid_left.sample_time = dt
pid_right.sample_time = dt
while threadRun:
last_leftDistance = leftDistance
last_rightDistance = rightDistance
time.sleep(dt)
if leftDistance == 0:
delta_leftDistance = 0
else:
delta_leftDistance = leftDistance - last_leftDistance
if rightDistance == 0:
delta_rightDistance = 0
else:
delta_rightDistance = rightDistance - last_rightDistance
cur_leftSpeed = delta_leftDistance / (23 * dt)
cur_rightSpeed = delta_rightDistance / (23 * dt)
left_err = abs(cur_leftSpeed) - abs(leftSpeed)
right_err = abs(cur_rightSpeed) - abs(rightSpeed)
pid_left.output_limits = (-abs(leftSpeed), 100 - abs(leftSpeed))
pid_right.output_limits = (-abs(rightSpeed), 100 - abs(rightSpeed))
left_fix = pid_left(left_err)
right_fix = pid_right(right_err)
if leftSpeed == 0:
leftR.stop()
leftF.stop()
elif leftSpeed > 0:
leftR.stop()
leftF.start(leftSpeed + left_fix)
else:
leftF.stop()
leftR.start(-leftSpeed + left_fix)
if rightSpeed == 0:
rightR.stop()
rightF.stop()
elif rightSpeed > 0:
rightR.stop()
rightF.start(rightSpeed + right_fix)
else:
rightF.stop()
rightR.start(-rightSpeed + right_fix)
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 timer_callback(event):
global started, started1, pub, encoderValue, PWM_Output, desired_Position, current_wheel_distance, current_angle
if (started1):
if (started):
previouswheeldistance = current_wheel_distance
pid = PID(100, 0.5, 1, setpoint=desired_Position, auto_mode=True)
pid.output_limits = (-255, 255)
pid.sample_time = 0.001
PWM_Output = pid(previouswheeldistance)
current_wheel_distance = (encoderValue * 2 * pi *
r) / (PPR * gearRatio * decodeNumber)
#previous_angle=current_angle
#pid = PID(0.022,0.01,2,setpoint=desired_Position)
#pid.output_limits = (-255, 255)
#pid.sample_time = 0.001
#PWM_Output = pid(previous_angle)
#if( 0 < PWM_Output <= 13):
# PWM_Output = PWM_Output + 11.5
#elif (-13 <= PWM_Output < 0):
# PWM_Output = PWM_Output - 11.5
#
#current_angle = encoderValue/24
pub.publish(PWM_Output)
print "Publishing PWM Values", PWM_Output
print "Current Wheel distance", current_wheel_distance
def controlHumid(mean_Humid):
if (mean_Humid > 0 and obj_Control.force_Flushing_On == False
and obj_Control.flushing_On == False):
try:
datetime_Now = datetime.strptime(
datetime.now().strftime("%m/%d/%Y, %H:%M:%S"),
"%m/%d/%Y, %H:%M:%S"
) #Fait comme ca pour avoir la meme forme que les autres datetimes. Sinon des erreurs de precisions arrivent.
datetime_On_Humid = datetime.strptime(obj_Control.humid_On_Time,
"%m/%d/%Y, %H:%M:%S")
datetime_Off_Humid = datetime.strptime(obj_Control.humid_Off_Time,
"%m/%d/%Y, %H:%M:%S")
if (datetime_Now > datetime_Off_Humid
and datetime_Now > datetime_On_Humid):
obj_Control.humid_On_Time = datetime_Now.strftime(
"%m/%d/%Y, %H:%M:%S")
obj_Control.humid_Off_Time = (
datetime_Now +
timedelta(seconds=obj_Control.PERIOD_HUMIDITY)
).strftime("%m/%d/%Y, %H:%M:%S")
elif (
datetime_Now >= datetime_Off_Humid
): #Lorsque tempsoff est depassé, allume et set le temps max On
try:
pid_Humid = PID(5,
4,
1,
setpoint=obj_Control.setpoint_Humid)
pid_Humid.output_limits = (
0, obj_Control.PERIOD_HUMIDITY
) #Limites sont 0 a 60, puisque l<humidificateur prends ~5 secondes avant de commencer a humidifier. donc minimum On time = 5 secondes, maximum 55 secondes genre
humid_Control = pid_Humid(mean_Humid)
except:
print("Erreur Humidity Control")
if (obj_Control.humidity_On == False and humid_Control >= 6):
obj_Control.humidity_On = True
obj_Control.humid_On_Time = (
datetime_Off_Humid +
timedelta(seconds=(int(humid_Control)))
).strftime("%m/%d/%Y, %H:%M:%S")
obj_Control.humid_Off_Time = (
datetime_Off_Humid +
timedelta(seconds=obj_Control.PERIOD_HUMIDITY)
).strftime("%m/%d/%Y, %H:%M:%S")
obj_Control.toggle(obj_Control.PIN_HUMID)
print(
"Humidity On - Time : {} -> Humidity Shutoff : {} - New Cycle : {}"
.format(datetime_Now.strftime("%m/%d/%Y, %H:%M:%S"),
obj_Control.humid_On_Time,
obj_Control.humid_Off_Time))
elif (datetime_Now >= datetime_On_Humid):
if (obj_Control.humidity_On == True):
print("Shutting off Humidity. - Time : {}".format(
datetime_Now.strftime("%m/%d/%Y, %H:%M:%S")))
obj_Control.humidity_On = False
obj_Control.toggle(obj_Control.PIN_HUMID)
except Exception as err:
print(str(err))
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 main():
last_time = time.time()
j = pyvjoy.VJoyDevice(1)
pid = PID(0.1, 0, 0, setpoint=0)
pid.output_limits = (-3,3)
direction_mean = deque()
cx_mean=deque()
cy_mean=deque()
while True:
if(len(direction_mean)>3):
direction_mean.popleft()
# cx_mean.popleft()
# cy_mean.popleft()
# screen = np.array(ImageGrab.grab(bbox=(0,40,800,600)))
screen = grab_screen(region =(0,40,800,600))
last_time = time.time()
hsv = processed_img(screen)
new_screen,cx,cy = road(hsv)
virage,turnx,turny=turns(hsv)
#new_screen,cx,cy = road(hsv)
#cv2.imshow('window2',hsv)
#cx_mean.append(cx)
#cy_mean.append(cy)
#cx=st.mean(cx_mean)
#cy=st.mean(cy_mean)
turn = (turnx-400)/(turny-400)
if (abs(turn)> 5 ):
print("turn ahead")
direction = (cx-400)/(cy-800)
cv2.line(new_screen,(400,600),(int(cx),int(cy)),(170,80,255),5)
direction_mean.append(direction)
direction=st.mean(direction_mean)
#direction=pid(direction)
print(direction)
drive(direction,j)
cv2.imshow('window', new_screen)
cv2.imshow('virage',virage)
#cv2.imshow('window',cv2.cvtColor(screen, cv2.COLOR_BGR2RGB))
if cv2.waitKey(25) & 0xFF == ord('e'):
cv2.destroyAllWindows()
break
def PIDcorona(y0,nat,mort,beta,gamma,dSM,dM,dZ,dHI,dHD,sm,m,z,h,mh,T0,tN,simtime,ICU,n_samples,g,K,Ki,Kd):
pid = PID(K, Ki, Kd, setpoint=1900)
# define controller
pid.sample_time = 1 # update every day
pid.output_limits = (0.02, 0.244)
# calculate new beta
beta = pid(y0[5])
# run model for one timestep
simtime = 1
tN = 2
y=runSimulation(y0,nat,mort,beta,gamma,dSM,dM,dZ,dHI,dHD,sm,m,z,h,mh,T0,tN,simtime,ICU,n_samples,g)
#print(y0[5],y[5])
return(beta,y)
def orientate_robot(self):
# PID controller:
orientation_pid = PID(self.kp, self.ki, self.kd, self.target)
orientation_pid.output_limits = (
-self.max_angular_speed, self.max_angular_speed
) # setting the limitis for the angular speeds that will be output by the PID control
if (
abs(self.magreader.mag_data.x) > self.tol
): # If the x-reading of the IMU magnetometer is above a certain error, we apply the PID control on the angular_speed
# Given that IMU data can be a bit noisy, we create a running mean of the last 5 readings to base our PID control on the average of these readings - this smoothens the motion
self.orientation_estimation_list.append(
self.magreader.mag_data.y
) # Negative to ensure that we turn in the correct direction - left if we're pointing East, and right if we are pointing West
orientation_running_mean = float(
sum(self.orientation_estimation_list)) / float(
len(self.orientation_estimation_list))
angular_speed = orientation_pid(orientation_running_mean)
# We will sometimes get calculated angular speeds that are so low that the wheels actually don't turn on the terrain and we never reach the North orientation, we just get close to it. Hence, we set a minimum angular speed - this would also have to be tuned for the robot and the terrain.
if angular_speed < self.min_angular_speed:
angular_speed = self.min_angular_speed
# Here we have to make an adjustment, because mag_data.y is minimum at the South and maximum at the North, increasing equally clockwise and anticlockwise, hence angular_speed is always going to be positive. To move anticlockwise when the robot is pointing towards the West (ie when mag_data.x < 0), we have to make the angular_speed negative
if self.magreader.mag_data.x < 0: # ie if the front of the robot is angled towards the West
angular_speed = -angular_speed
self.vel_data.angular.z = angular_speed
self._vel_pub.publish(
self.vel_data
) # publish the calculated angular_speed to the '/cmd_vel' topic
elif (abs(self.magreader.mag_data.x) < self.tol) and (
self.magreader.mag_data.y > 0
): # If the robot is pointing towards the North then we stop its rotation
self.vel_data.angular.z = 0
self._vel_pub.publish(self.vel_data)
# We've left a case untouched - if (abs(self.magreader.mag_data.x) < self.tol) and (self.magreader.mag_data.y < 0) - this means that the robot is practically fully aligned with the South direction. If this happens, by skipping it we neither bring the robot to a stop, nor do we change the angular_speed, hence the robot continues moving at the
# angular_speed at which it entered this "window". This is a form of hysteresis, added here to prevent rapid switching of the angular_speed when the robot goes through the state of looking South - if not it could start jittering in place.
# However, we need to consider the case when the robot is looking South and it isn't moving. This could happen at startup, and if it happens we have to tell the robot to start moving:
else:
if self.vel_data.angular.z == 0:
self.vel_data.angular.z = self.max_angular_speed
self._vel_pub.publish(self.vel_data)
print(
'Robot angular speed: ' + str(self.vel_data.angular.z)
) # We print the angular_speed of the robot for debugging purposes
self.r.sleep() # wait so that robot motion is smoother
def controlTemp(mean_Temp):
if (mean_Temp > 0):
datetime_Now = datetime.strptime(
datetime.now().strftime("%m/%d/%Y, %H:%M:%S"), "%m/%d/%Y, %H:%M:%S"
) #Fait comme ca pour avoir la meme forme que les autres datetimes. Sinon des erreurs de precisions arrivent.
datetime_On_Heat = datetime.strptime(obj_Control.heat_On_Time,
"%m/%d/%Y, %H:%M:%S")
datetime_Off_Heat = datetime.strptime(obj_Control.heat_Off_Time,
"%m/%d/%Y, %H:%M:%S")
if (datetime_Now > datetime_Off_Heat
and datetime_Now > datetime_On_Heat):
obj_Control.heat_On_Time = datetime_Now.strftime(
"%m/%d/%Y, %H:%M:%S")
obj_Control.heat_Off_Time = (
datetime_Now + timedelta(seconds=obj_Control.PERIOD_HEAT)
).strftime("%m/%d/%Y, %H:%M:%S")
elif (datetime_Now >= datetime_Off_Heat
): #Lorsque tempsoff est depassé, allume et set le temps max On
try:
pid_Heating = PID(10, 5, 3, setpoint=obj_Control.setpoint_Temp)
pid_Heating.output_limits = (
0, obj_Control.PERIOD_HEAT
) #Fait un output de 0 a 5 qui sera changé en secondes
temp_Control = pid_Heating(mean_Temp)
except:
print("Erreur Heating Control")
if (obj_Control.heating_On == False):
obj_Control.heating_On = True
obj_Control.heat_On_Time = (
datetime_Off_Heat +
timedelta(seconds=(int(temp_Control)),
milliseconds=(temp_Control * 1000) %
1000)).strftime("%m/%d/%Y, %H:%M:%S")
obj_Control.heat_Off_Time = (
datetime_Off_Heat +
timedelta(seconds=obj_Control.PERIOD_HEAT)
).strftime("%m/%d/%Y, %H:%M:%S")
obj_Control.toggle(obj_Control.PIN_CHAUFFAGE)
#print("Heating On - Time : {} -> Heat Shutoff : {} - New Cycle : {}".format(datetime_Now.strftime("%m/%d/%Y, %H:%M:%S"),obj_Control.heat_On_Time, obj_Control.heat_Off_Time))
elif (datetime_Now >= datetime_On_Heat):
if (obj_Control.heating_On == True):
#print("Shutting off Heat. - Time : {}".format(datetime_Now.strftime("%m/%d/%Y, %H:%M:%S")))
obj_Control.heating_On = False
obj_Control.toggle(obj_Control.PIN_CHAUFFAGE)
def pid_testing(max_time=60,
p=0.01,
i=0.01,
d=0.001,
setpoint=40,
plotting=True):
time, temp_rb, volt_out = [], [], []
pid = PID(p, i, d, setpoint=setpoint)
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()
while True:
current_value = temp2.value
output = pid(current_value)
time.append(ttime.time() - time_start)
temp_rb.append(current_value)
volt_out.append(output)
RE(bps.mv(volt_override, output))
if ttime.time() - time_start > 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))
if plotting:
fig, ax = plt.subplots(1)
ax2 = ax.twinx()
ax.plot(time, temp_rb, 'r.-')
ax2.plot(time, volt_out, 'k.-')
ax.hlines(setpoint, time[0], time[-1], colors='g')
return np.array(time[::5]), np.array(temp_rb[::5]), np.array(volt_out[::5])
def setupEvironment(configData):
global dutyCycle
global diffTemp
global lastRPMAccessTime
global sleepTime
global pidBuffer
global maxTempExceeded
global useBuffer
global checkFrequency
output("setup new evironment")
GPIO.cleanup()
dutyCycle=0
output("setup pid-controller")
pid = PID(configData['general']['pidAggressiveness'], 0.01, 0.1, setpoint=configData['general']['destTemp'])
pid.output_limits = (0, configData['pwm']['maxDutyCycle'])
output("setup gpio")
#pwn
GPIO.setmode(GPIO.BCM)
GPIO.setup(configData['pwm']['pin'], GPIO.OUT)
myPWM = GPIO.PWM(configData['pwm']['pin'], configData['pwm']['frequency'])
myPWM.start(dutyCycle)
# interrupt detection
lastRPMAccessTime = datetime.datetime.now()
GPIO.setmode(GPIO.BCM)
GPIO.setup(configData['speedometer']['pin'], GPIO.IN)
GPIO.remove_event_detect(configData['speedometer']['pin'])
GPIO.add_event_detect(configData['speedometer']['pin'], GPIO.FALLING, callback=countInterrupt)
GPIO.setwarnings(False)
diffTemp = configData['general']['maxTemp']-configData['general']['destTemp']
sleepTime = configData['general']['sleepTime']
checkFrequency = configData['general']['checkFrequency']
pidBuffer = list()
maxTempExceeded = False
useBuffer = True
return myPWM, pid
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))
sp = np.ones(101) * T_ss sp[5:] = 340.0 u_bias = u_ss # create PID controller # op = op_bias + Kc * e + Ki * ei + Kd * ed # with ei = error integral # with ed = error derivative Kc = 1.0 # Controller gain Ki = 1.0 # Controller integral parameter Kd = 0.0 # Controller derivative parameter pid = PID(Kc,Ki,Kd) # lower and upper controller output limits oplo = 250.0 ophi = 350.0 pid.output_limits = (oplo-u_bias,ophi-u_bias) # PID sample 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):
def update(self, boiler_power, dt):
if boiler_power > 0:
# boiler can only produce heat, not cold
self.water_temp += 1 * boiler_power * dt
# some heat dissipation
self.water_temp -= 0.02 * dt
return self.water_temp
if __name__ == '__main__':
boiler = WaterBoiler()
water_temp = boiler.water_temp
pid = PID(5, 0.01, 0.1, setpoint=water_temp)
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]
GPIO.setup(motor1, GPIO.OUT)
GPIO.setup(motor2, GPIO.OUT)
GPIO.setup(motorSpeed, GPIO.OUT)
GPIO.output(motor1, GPIO.LOW)
GPIO.output(motor2, GPIO.LOW)
speedControl=GPIO.PWM(motorSpeed,100)
speedControl.start(currentSpeed)
#Anemo setup
adc = Adafruit_ADS1x15.ADS1015()
GAIN = 2
#PID setup
pid = PID(1, 0.1, 0.05, setpoint=440)
pid.sample_time = 1
pid.output_limits = (30, 90)
#Switch Setup
switch1 = 17
GPIO.setup(switch1, GPIO.IN,GPIO.PUD_UP)
#exit
def exitProgram():
GPIO.cleanup()
print("EXIT")
atexit.register(exitProgram)
def startMotor():
clear = lambda: os.system('clear')
setpoint = 0.59
left_stop = 7.1
right_stop = 7.08
left_backwards_max = 6.5
right_backwards_max = 8.5
left_forwards_max = 8.5
right_forwards_max = 6.5
#pid = PID (4,1.5,0.3, setpoint)
pid = PID (10,1.5,2, setpoint)
pid1 = PID(10,1.5,2, setpoint)
pid.output_limits = (6.5, 8.5)
pid1.output_limits = (6.5,8.5)
IO.setwarnings(False)
IO.setmode(IO.BCM)
IO.setup(13,IO.OUT)
IO.setup(19,IO.OUT)
right=IO.PWM(19,50)
left=IO.PWM(13,50)
right.start(0)
left.start(0)
#forward()
rospy.init_node('base_movement')
sub = rospy.Subscriber('/imu0',Imu,callback)
rospy.spin()
def main():
# Class define
state = RobotState()
parser = config.config()
robot = serialCom()
image_thread = vision.imageCapRS2()
# Socket Conn
conn = websocket.create_connection('ws://192.168.2.66:9090/')
ws = Referee(conn)
# Params
distances = []
teamColor = "magenta"
basketxs = []
finaldistance = 0
throwing = False
counter = 0
temp_dist_centerX = 0
middle_px = parser.get('Cam', 'Width') / 2
# PID
toBallSpeed = PID(0.015, 0.00001, 0, setpoint=460) # P=0.4 oli varem
toBallSpeed.output_limits = (15, 75) # Motor limits
toBallSpeed.sample_time = 0.01
basketCenterSpeed = PID(0.06, 0.00001, 0.003,
setpoint=310) # 0.1, 0.000001, 0.001
basketCenterSpeed.output_limits = (-5, 5) # Motor limits
basketCenterSpeed.sample_time = 0.01
while True:
if ws.go: # True
teamColor = ws.basketColor
frame = image_thread.getFrame()
if state.current is STATE.EMPTYING:
if robot.recive.ir == 1:
robot.setIr(1)
robot.startThrow(100)
robot.setServo(100)
else:
if state.timer_ms_passed() > 0.5:
robot.setIr(0)
state.change_state(STATE.WAITING)
elif state.current is STATE.WAITING:
# waiting
robot.setIr(0)
robot.setServo(0)
robot.stopThrow()
state.change_state(STATE.FINDING_BALL)
#
elif state.current is STATE.FINDING_BALL:
imageProcessing.detectLine(frame)
ballCnts = imageProcessing.getContours(frame)
if len(ballCnts) > 0:
ball_x, ball_y = imageProcessing.detectObj(frame, ballCnts)
if ball_y > 415 and 310 < ball_x < 330: # 30 - 60 parser.get('Cam', 'Height') - 65
print('found ball')
state.change_state(STATE.PICKING_UP_BALL)
else:
speed = max(50 - int(ball_y / 5), 20)
robot.omniMovement(speed, middle_px, ball_x, ball_y)
else:
robot.left(10)
#
elif state.current is STATE.PICKING_UP_BALL:
# picking up ball
robot.forward(10)
if state.timer_seconds_passed() > 1:
robot.stopMoving()
robot.setIr(0)
robot.setServo(50)
if state.timer_seconds_passed() > 8:
robot.setServo(-100) # eject
state.change_state(
STATE.FINDING_BALL) # ball disappeared, restart
elif robot.recive.ir == 1:
state.change_state(STATE.FINDING_BASKET)
pass
#
elif state.current is STATE.FINDING_BASKET:
# finding basket
# to add if basket too close go back a little
basketCnts = imageProcessing.getBasketContours(
frame, teamColor)
if len(basketCnts) > 0:
target_x, target_y, w, h = imageProcessing.detectObj(
frame, basketCnts, False)
basketCenteX, basketY = target_x + w / 2, target_y + h
distance = imageProcessing.calc_distance(w)
if len(distances) > 5:
distances.pop(0)
distances.append(distance)
finaldistance = sum(distances) / len(distances)
else:
distances.append(distance)
dist_from_centerX = 320
if target_x > -1:
dist_from_centerX = middle_px - basketCenteX
basketxs.append(dist_from_centerX)
print("dist_from_centerX", dist_from_centerX)
if 0 < dist_from_centerX < 30:
if (int(dist_from_centerX) == int(temp_dist_centerX)
) and (dist_from_centerX != 320):
counter += 1
else:
temp_dist_centerX = dist_from_centerX
counter = 0
else:
print(-basketCenterSpeed(basketCenteX))
robot.rotate(-basketCenterSpeed(basketCenteX))
if finaldistance > 130:
speed = max(50 - int(basketY / 5), 20)
robot.omniMovement(speed, middle_px, basketCenteX,
basketY)
elif 0 < finaldistance < 60:
robot.reverse(10)
else:
if counter > 3 or state.timer_seconds_passed() > 15:
robot.stopMoving()
state.change_state(STATE.DRIVING_TO_BASKET)
counter = 0
else:
robot.left(12)
#
elif state.current is STATE.DRIVING_TO_BASKET:
# driving to basket
if state.timer_ms_passed() > 0.5:
state.current = STATE.STARTING_THROWER
#
elif state.current is STATE.STARTING_THROWER:
# starting thrower
# 0.180854 ja 14.205 oli varem
throwSpeed = -0.000161064 * finaldistance**2 + 0.181854 * finaldistance + 15.205
robot.startThrow(throwSpeed)
if state.timer_ms_passed() > 0.5:
state.change_state(STATE.THROWING_BALL)
#
elif state.current is STATE.THROWING_BALL:
# throwing ball
robot.setIr(1)
if state.timer_seconds_passed() + state.timer_ms_passed(
) > 1.5:
robot.setIr(0)
robot.setServo(0)
robot.startThrow(0)
state.change_state(STATE.FINDING_BALL)
#
else:
raise Exception(
f'State not implemented in main(): {state.current}')
pass
cv2.putText(frame, str(state.current), (10, 30),
cv2.FONT_HERSHEY_DUPLEX, 1, cv2.COLOR_YUV420sp2GRAY)
cv2.imshow('Processed', frame)
k = cv2.waitKey(1) & 0xFF
if k == ord("q"):
break
else:
robot.stopMoving()
state.change_state(STATE.EMPTYING)
image_thread.setStopped(False)
robot.stopThrow()
robot.setStopped(False)
cv2.destroyAllWindows()
rospy.Subscriber("/ref_pan_angle", Float32, angle_cb)
rospy.Subscriber("/ref_tilt_angle", Float32, tilt_angle_cb)
pub = rospy.Publisher("/angle_ref", Float32, queue_size=1)
ku = 1100
tu = 5.2
[kp, ki, kd] = tune_gain(ku, tu, "noovershoot")
pid_pan = PID(kp, ki - 30, kd - 200)
# pid_tilt = PID(100, 0, 0, setpoint=0)
v_pan = position[0] * 180 / np.pi
# v_tilt = position[1]*180/np.pi
pid_pan.output_limits = (-11448, 11448)
# 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
__maximum_w = 0
__minimum_v = 0
__maximum_v = 0
__handle_dis = 0
__handle_ang = 0
Kp_v = 1.5
Ki_v = 0.0
Kd_v = 0.1
Cp_v = 0
Kp_w = 0.25
Ki_w = 0.0
Kd_w = 0.1
Cp_w = 0
pid_v = PID(Kp_v, Ki_v, Kd_v, setpoint=Cp_v)
pid_v.output_limits = (-1*__maximum_v, __maximum_v)
pid_v.auto_mode = True
pid_w = PID(Kp_w, Ki_w, Kd_w, setpoint=Cp_w)
pid_w.output_limits = (-1*__maximum_w, __maximum_w)
pid_w.auto_mode = True
def TuningVelocityContorller(self, p, i, d, cp = Cp_v):
self.pid_v.setpoint = cp
self.pid_v.tunings = (p, i, d)
def TuningAngularVelocityContorller(self, p, i, d, cp = Cp_w):
self.pid_w.setpoint = cp
self.pid_w.tunings = (p, i, d)
def ChangeVelocityRange(self, m, M):
self.__minimum_v = m
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
cf2_psi = 0 # Get current yaw-angle of cf
cf2_bat = cf2.vbat # Get current battery level of cf
cf2_blevel = cf2.blevel
cf2_pwm = cf2.m1_pwm
cf2_temp = cf2.temp
cf2_phi = 0
cf2_theta = 0
marker_psi = 0 # Set marker angle to zero initially
pid = PID(0, 0, 0, setpoint=marker_psi) # Initialize PID
pid_x = PID(0, 0, 0, setpoint=0) # init PID for roll
# Define pid parameters
pid.tunings = (Kp_psi, Kd_psi, Ki_psi)
pid_x.tunings = (Kp_x, Kd_x, Ki_x)
pid_x.sample_time = 0.05 # update pid every 50 ms
pid.output_limits = (-60, 60)
pid_x.output_limits = (-20, 20)
pid.proportional_on_measurment = False
pid_x.proportional_on_measurment = False
if cf2_bat >= low_battery:
cf2.takeoff() # CF takes off from ground
print("Taking off ! Battery level: " + str(cf2_bat))
time.sleep(1.5) # Let the CF hover for 1.5s
t_init = time.time()
endTime = t_init + 30000 # Let the CF do its thing for some time
# Add values to array for logging/plotting
setpoint = np.append(setpoint, marker_psi) # Commanded yaw
yaw_cf = np.append(yaw_cf, cf2.psi) # Actual yaw of CF
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
#v = getCPUtemperature().update(0)
myPWM.ChangeDutyCycle(output)
print(output)
time.sleep(1)
#except KeyboardInterrupt: # trap a CTRL+C keyboard interrupt
#GPIO.cleanup() # resets all GPIO ports used by this program
tf_listener = tf.TransformListener()
rospy.Subscriber('odom', Odometry, Position)
time.sleep(0.5)
cmd_vel = rospy.Publisher('cmd_vel', geometry_msgs.msg.Twist,
queue_size=5) # TODO: topic namespace
rate = rospy.Rate(60.0)
p = getPose()
print('pos', p)
goal_a = 0
goal_x = 0.8
goal_y = 0
goal_da = 0
goal_distance = 0.8
pid = PID(1, 0.1, 0, setpoint=-1.8)
pid.output_limits = (0, 1.1)
while (abs(p[0] - goal_x) > 0.02):
global p
p = getPose()
speed = pid(p[0])
move_cmd = geometry_msgs.msg.Twist()
move_cmd.linear.x = speed * 10
move_cmd.angular.z = 0
print(p)
print(move_cmd)
cmd_vel.publish(move_cmd)
rate.sleep()
move_cmd = geometry_msgs.msg.Twist()
move_cmd.linear.x = 0
move_cmd.angular.z = 0
#Newport
port = 'USB0::0x1FDE::0x0007::59401447::0::INSTR'
#Ahlborn
serial = 'COM3'
Newport = Newport_ILX(port)
Ahlborn = Physics_1000(serial)
Newport.open()
Newport.init_ITE_mode()
#Settings for PID
pid = PID(-5, -0.02, 0, setpoint=23.0)
#max and min output values of the PID
pid.output_limits = (-2.5, 2.5)
#Delay betweeen calculations in s
pid.sample_time = 1
tmp = Ahlborn.read_tmp()
if __name__ == '__main__':
try:
while True:
#refreshed reading of the tmp
tmp = Ahlborn.read_tmp()
p, i, d = pid.components
Ahlborn.save_tmp_to_txt(TimeStamp=True)
# compute new ouput from the PID according to the systems current temp value
control = pid(tmp)
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 _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))
if (robot_heading_to_target - robot_heading
) > math.radians(180): # something might be wrong here
robot_heading_to_target -= math.radians(
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(
