360) # something might be wrong here
else:
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
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
Ivalue = 0.0
orginal = 0
orginalX = 0
count = 0
while True:
if (count == 1 / 0.02):
count = 0
f = open("setting.txt", "r")
setting = f.read().split()
startPoint = float(setting[0])
KP = float(setting[1])
KI = float(setting[2])
KD = float(setting[3])
pid.setpoint = startPoint
pid.Kp = KP
pid.Ki = KI
pid.Kd = KD
# Check if AltIMU is ready
if altIMU.IMURead():
# Get a new set of data from IMU and pressure sensor
data = altIMU.getIMUData()
# Extract fused roll, pitch and yaw from the data (values are in radians)
(fusionRollX, fusionPitchY, fusionYawZ) = data['fusionPose']
# Calculate PID term
fusionRollX = fusionRollX * 180 / pi
print(fusionRollX)
if orginal == 1:
# error = fusionRollX -orginalX
# Pvalue = KP * errors
# Ivalue += KI * error
