def set_velocity(vehicle, x_velocity=0, y_velocity=0, duration=0):
'''Sets the velocity of the vehicle RELATIVE TO HEADING'''
x_pid = PID(P, I, D, setpoint=x_velocity * 133)
x_pid.sample_time = 0.1
y_pid = PID(P, I, D, setpoint=y_velocity * 133)
y_pid.sample_time = 0.1
for _ in range(0, duration * 10):
# compute new ouput from the PID according to the systems current value
xcontrol = x_pid(X_VELOCITY)
ycontrol = y_pid(Y_VELOCITY)
set_attitude(vehicle,
pitch_angle=-1 * xcontrol,
roll_angle=ycontrol,
duration=.1)
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 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 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 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 set_position(self):
'''Uses PID and position guesses to set the quad's position'''
assert self.target_position is not None
#set attitude to 0
self.set_attitude(yaw_angle=0, duration=5)
while True:
print("Targeting", self.target_position)
targ = []
targ = self.target_position
xpid = PID(self.configs.pos.P,
self.configs.pos.I,
self.configs.pos.D,
setpoint=targ[0])
xpid.sample_time = 0.33
ypid = PID(self.configs.pos.P,
self.configs.pos.I,
self.configs.pos.D,
setpoint=targ[1])
ypid.sample_time = 0.33
till_stop = 15
while True:
if targ != self.target_position:
#if dest changes rest PID algs
break
if till_stop == 0:
self.reached_dest = True
#Get control from each PID
x_control = xpid(self.position_guess[0])
y_control = ypid(self.position_guess[1])
remaining_x = targ[0] - self.position_guess[0]
remaining_y = targ[1] - self.position_guess[1]
remaining_dst_sq = (remaining_x**2 + remaining_y**2)**.5
if remaining_dst_sq < 20:
till_stop -= 1
else:
till_stop = 15
#Correct for tilts higher than total_tilt
total_control_sq = (y_control**2 + x_control**2)**.5
if total_control_sq > self.total_tilt:
x_control = self.total_tilt * remaining_x / remaining_dst_sq
y_control = self.total_tilt * remaining_y / remaining_dst_sq
self.set_attitude(roll_angle=x_control, \
pitch_angle=y_control, \
duration=.33, \
yaw_angle=0)
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 AutoTunning(kp, ki, kd, Input, mitad):
#Auto-Tunning
pid = PID(kp, ki, kd, setpoint=mitad)
#pid.output_limits = (0, vel + 15)
pid.sample_time = 0.01
#computa nuevo pid
control = pid(Input)
kp, ki, kd = pid.components
print(kp, ki, kd)
return kp, ki, kd
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 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 pid_process(output, p, i, d, objCoord, centerCoord):
## # signal trap to handle keyboard interrupt
## signal.signal(signal.SIGINT, signal_handler)
# create a PID and initialize it
pid = PID(p.value, i.value, d.value, setpoint=centerCoord.value)
pid.sample_time = 2.0
pid.auto_mode = True
## p.initialize()
# loop indefinitely
while True:
output.value = pid(objCoord.value)
def run_pid(cont_name,interval=1):
starttime=time.time()
time.sleep(interval - ((time.time() - starttime) % (interval) ) )
this_pid=PID(2,0.05,0.0001,setpoint=mu,output_limits=(-50,200))
this_pid.sample_time=pid_interval
logger.info("started PID for {}".format(cont_name) )
while not exited:
try:
# state = returnMetric(cont_name=cont_name,metric_name='delay')
state =os.popen("cat {}.delays | tail -1".format( os.path.join(PROJECT_HOME,DIRECTORY,cont_name)) ).read().strip()
# logger.info('Delay for {} = {}'.format(cont_name,state) )
if not len(state)>0:
continue
state= float(state)
except:
continue
try:
cont_delays[cont_name].append(state);
cont_delays[cont_name].pop(0)
# AVERAGING DISABLED TO MAKE PID MORE REACTIVE
# state= np.mean(cont_delays[cont_name])
state = (state-mu)/sdev
pid_op = this_pid(this_pid.setpoint-state)
sigm = sigmoid(pid_op)
# window of last five PID outputs:
action[cont_name].append(sigm)
action[cont_name].pop(0)
# check if current container is suffering
if sum(action[cont_name])>=490:
others_fine = resolve(cont_name)
if others_fine:
challenges[cont_name]=0
if challenges[cont_name]==3:
# DECIDE BEST HOST -> MIGRATE
#requests.get('http://10.5.20.124:1234/GM/Migrate_apache/{}'.format(cont_name),proxies={'http':None}).text
logger.info("Migrate called for {}".format(cont_name))
except:
logger.exception("PID Error : {}".format(cont_name) )
time.sleep(interval - ((time.time() - starttime) % interval))
def AutoLand():
#ultrasonic.distance() returns distance in cm. 11cm is height from ground. 12 should be good then to drop
LandPID = PID(-0.5, 0.1, 0.05, setpoint=12)
LandPID.sample_time = 0.01
pid.output_limits = (300, 325) # output value will be between 0 and 10
#Current distance away
distance = ultrasonic.distance()
while distance > 12:
control = LandPID(distance)
#Change throttle - control needs to be between 240-420
talktopi(ZERO_VALUE, ZERO_VALUE, control, ZERO_VALUE)
#update distance
distance = ultrasonic.distance()
#Turn off motors
talktopi(ZERO_VALUE, ZERO_VALUE, MIN_VALUE, ZERO_VALUE)
time.sleep(1)
def main():
global quit, out, control, err
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out = cv2.VideoWriter('.\CV_DS\debug_1.avi', fourcc, fps, frame_size)
keyboard.on_press_key("1", toggle_quit, suppress=True)
pid = PID(1, 0.1, 0.05, setpoint=1)
pid.sample_time = 1
print("Press 1 to start")
while quit:
sleep(1)
print("Press 1 to quit")
while not quit:
frame = np.array(ImageGrab.grab())
err = get_err(frame)
control = pid(err)
update(control, err)
cv2.destroyAllWindows()
keyboard.unhook_all()
out.release()
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 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))
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
pid_pan.setpoint = ref
control_pan = pid_pan(v_pan)
# print("Control: ", control_pan)
def marker_hover(vehicle, marker_tracker, rs=None):
pid_x = PID(1, 0, 0, setpoint=0)
pid_y = PID(1, 0, 0, setpoint=0)
pid_z = PID(1, 0, 0, setpoint=-0.5)
pid_x.sample_time = 0.01
pid_y.sample_time = 0.01
pid_z.sample_time = 0.01
# Hover until manual override
pose_queue = np.zeros((RUNNING_AVG_LENGTH, 3), dtype=float)
count = 0
if args["debug"] > 0:
vehicle_pose_file = file_utils.open_file(VEHICLE_POSE_FILE)
start_time = time.time()
print("Tracking marker...")
while True:
# while vehicle.mode == VehicleMode("GUIDED"):
# Track marker
marker_tracker.track_marker(
alt=vehicle.location.global_relative_frame.alt)
if marker_tracker.is_marker_found():
marker_pose = marker_tracker.get_pose()
# Transform pose to UAV body frame; proportional gain
marker_pose_body_ref = marker_ref_to_body_ref(marker_pose)
# Pid response, input is UAV rel. to marker
control_pose = np.array([[
pid_x(-marker_pose_body_ref[0]),
pid_y(-marker_pose_body_ref[1]),
pid_z(-marker_pose_body_ref[2])
]])
# Keep a queue of recent marker poses
pose_queue[count % len(pose_queue)] = control_pose
count += 1
if count < RUNNING_AVG_LENGTH - 1:
pose_avg = np.sum(pose_queue, axis=0) / count
else:
pose_avg = np.average(pose_queue, axis=0)
# Send position command to the vehicle
dronekit_utils.goto_position_target_body_offset_ned(
vehicle,
forward=pose_avg[0],
right=pose_avg[1],
down=-HOVER_ALTITUDE - pose_avg[2])
if args["debug"] > 0 and rs is not None:
data = rs.read()
rs_pose = tf.quaternion_matrix([
data.rotation.w, data.rotation.x, data.rotation.y,
data.rotation.z
])
rs_pose[0][3] = data.translation.x
rs_pose[1][3] = data.translation.y
rs_pose[2][3] = data.translation.z
vehicle_pose = realsense_localization.rs_to_body(rs_pose)
vehicle_trans = np.array(
tf.translation_from_matrix(vehicle_pose))
vehicle_pose_file.write("{} {} {} {} 0 0 0 0\n".format(
time.time() - start_time, vehicle_trans[0],
vehicle_trans[1], vehicle_trans[2]))
if args["debug"] > 2:
print("Nav command: {} {} {}".format(pose_avg[0], pose_avg[1],
pose_avg[2]))
# Maintain frequency of sending commands
marker_tracker.wait()
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
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(
max_forward_heading
): # don't drive forward until error in heading is less than max_forward_heading
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()
def goto(desPos, maxSpeed):
global leftSpeed
global rightSpeed
global straight
mem = straight
straight = False
global speed
global arucoUpdate
arucoUpdate = True
#print(curPos)
#print(desPos)
pathList = path(curPos, desPos, initMap)
if pathList == []:
return False
print("Path: ", pathList)
for p in pathList:
ac = path2actions(curPos, p)
#direction = math.atan2(p[1] - curPos[1], p[0] - curPos[0])
# turnAngle = - errAngle(direction,curPos[2])*180/np.pi
# print("Turn 1: ", turnAngle)
#print(direction*180/np.pi)
#print(curPos[2]*180/np.pi)
direction = ac[0]
turn2Dir(direction)
#distance = math.sqrt((desPos[1] - curPos[1])*(desPos[1] - curPos[1]) + (desPos[0] - curPos[0])*(desPos[0] - curPos[0]))
distance = ac[1]
dt = 0.1
pid_straight = PID(1.2, 0.5, 0.04, setpoint=0)
pid_straight.sample_time = dt
speed = maxSpeed
if distance > 0:
forward()
else:
backward()
curDistance = abs(distance)
while 1:
ds = curDistance
curDistance = dis2Pos(curPos, p)
ds = ds - curDistance
if curDistance > 200:
direction = math.atan2(p[1] - curPos[1], p[0] - curPos[0])
if curDistance < 400:
speed = 30
#print(curDistance)
#print(speed)
# Keep direction by angle
if straight == False and leftSpeed != 0 and rightSpeed != 0:
if distance > 0:
err = -errAngle(direction, curPos[2]) * 180 / np.pi
else:
err = errAngle(direction, curPos[2]) * 2
pid_straight.output_limits = (-speed / 3, speed / 3)
fix = pid_straight(err)
#print(err,fix)
#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
print(curDistance, ds)
if curDistance < 30 or (ds < 0 and curDistance < 300):
stop()
ss = 50
if distance > 0:
leftR.start(ss)
leftF.stop()
rightF.stop()
rightR.start(ss)
else:
leftF.start(ss)
leftR.stop()
rightR.stop()
rightF.start(ss)
time.sleep(0.4)
leftF.stop()
leftR.stop()
rightF.stop()
rightR.stop()
print("Last distance: ", dis2Pos(curPos, p))
if curDistance > 30:
aa = path2actions(curPos, p)
turn2Dir(aa[0])
move(aa[1], 20)
#goto(p,30)
break
time.sleep(dt)
# Last turn
turn2Dir(desPos[2], True)
print("After go: ", curPos)
straight = mem
return True
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):
# PID control
pid.setpoint=sp[i]
Ydist = 0.17
Xdist = 0.25
height = 0.17
#body frame to foot frame vector
bodytoFeet0 = np.matrix([[Xdist / 2, -Ydist / 2, -height],
[Xdist / 2, Ydist / 2, -height],
[-Xdist / 2, -Ydist / 2, -height],
[-Xdist / 2, Ydist / 2, -height]])
orientation = np.array([0., 0., 0.])
deviation = np.array([0., 0., 0.])
lastTime = 0.
pidX = PID(-0.0005, 0.0, 0.000001, setpoint=0.)
pidY = PID(0.0005, 0., 0., setpoint=0.)
pidX.sample_time = 0.02 # update every 0.01 seconds
pidY.sample_time = 0.02
T = 0.5 #period of time (in seconds) of every step
offset = np.array([
0.5, 0., 0., 0.5
]) #defines the offset between each foot step in this order (FR,FL,BR,BL)
while (True):
startTime = time.time()
pos, orn, L, angle, Lrot, T = pybulletDebug.cam_and_robotstates(boxId)
L, angle, Lrot, T = joystick.read()
#calculates the feet coord for gait, defining length of the step and direction (0º -> forward; 180º -> backward)
bodytoFeet, s = trot.loop(L, angle, Lrot, T, offset, bodytoFeet0)
mode = 1180
tilt = 1500
#PID
P_pitch = 1.48
I_pitch = 28.0
D_pitch = 0.05
P_roll = 0.1
I_roll = 30.0
D_roll = 0.1
pid_pitch = PID(P_pitch, I_pitch, D_pitch, setpoint=0.0)
pid_roll = PID(P_roll, I_roll, D_roll, setpoint=0.0)
pid_pitch.sample_time = 0.01
pid_roll.sample_time = pid_pitch.sample_time
pid_pitch.auto_mode = True
pid_roll.auto_mode = True
pid_pitch.output_limits = (-100.0, 100.0)
pid_roll.output_limits = (-100.0, 100.0)
##############################################################################
#Thread functions
def ReadSerial():
global mode, tilt
data = []
while 1:
GPIO.setmode(GPIO.BCM)
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)
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
cf2 = CF(scf, available)
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
def searchBook(distance, maxSpeed, angle, ID=10):
global leftSpeed
global rightSpeed
global straight
global speed
global arucoUpdate
arucoUpdate = True
ar = MyAruco()
print("ar ok")
time.sleep(1)
dt = 0.1
pid_straight = PID(4.5, 1.5, 0.4, setpoint=0)
pid_straight.sample_time = dt
mem = straight
straight = False
speed = maxSpeed
if distance > 0:
forward()
else:
backward()
while 1:
curDistance = max(abs(leftDistance), abs(rightDistance)) * PULSE_LENGTH
#speed = dis2Speed(curDistance,abs(distance),maxSpeed)
#print(speed)
# Keep direction by angle
if straight == False and leftSpeed != 0 and rightSpeed != 0:
err = -errAngle(angle, curPos[2]) * 180 / np.pi
#print(err)
pid_straight.output_limits = (-speed / 3, speed / 3)
fix = pid_straight(err)
#print(err,fix)
#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
# Search book
found, dis = ar.detectAndGetTargetDist(ID, 1)
if found:
speed = 10
if dis > 0 and leftSpeed < 0:
leftSpeed = speed
rightSpeed = speed
if dis < 0 and leftSpeed > 0:
leftSpeed = -speed
rightSpeed = -speed
print(dis)
if curDistance >= abs(distance) or (found and abs(dis) < 5):
stop()
ss = 100
if distance > 0:
leftR.start(ss)
leftF.stop()
rightF.stop()
rightR.start(ss)
else:
leftF.start(ss)
leftR.stop()
rightR.stop()
rightF.start(ss)
time.sleep(0.4)
leftF.stop()
leftR.stop()
rightF.stop()
rightR.stop()
if found:
print("Found and grab the book")
turn2Dir(desPos[2])
found, dis = ar.detectAndGetTargetDist(ID, 1)
print("Last D2Book: ", dis)
while abs(dis) > 5:
move(dis, 20)
time.sleep(0.5)
found, dis = ar.detectAndGetTargetDist(ID, 1)
print("Last D2Book: ", dis)
arucoUpdate = True
print("Last D2Book: ", dis)
getBook()
else:
print("Cannot find the book")
break
if not found:
time.sleep(dt)
del ar
straight = mem
