def __init__(self, heater_pin, temp_cs, top_pin=None,booster_pin=None, fan_pin = None):
self.heater_pin = heater_pin
self.fan_pin= fan_pin
self.top_pin = top_pin
self.booster_pin = booster_pin
self.thermocouple = MAX31855(SPI1,temp_cs)
#self.thermocouple = Client('/dev/ttyUSB0')
self.pid = PID(PID_KP, PID_KI, PID_KD)
#pinMode(heater_pin, OUTPUT)
GPIO.setup(heater_pin, GPIO.OUT)
#if (fan_pin): pinMode(fan_pin, OUTPUT)
if (fan_pin): GPIO.setup(fan_pin, GPIO.OUT)
else: self.fan_state = "disabled"
self.heatOff()
self.fanOff()
addToCleanup(self.stop)
self.current_phase = ''
self.current_step = 0
self.realtime_data = []
self.error = ''
self.abort = False
self.solder_type = LEADED
def ride_distance(self,distance): self.__motor1.reset_count() self.__motor2.reset_count() pid1 = PID(1,1./10000.,1./50000.,0.01) pid2 = PID(2,1/5000.,1./2500,0.01) speed = pid1.new_value(distance,0.01) self.__motor1.on(speed) self.__motor2.on(speed) while (speed != 0): distance1 = self.__motor1.get_count()*self.__perimeter*self.__gear_ratio distance2 = self.__motor2.get_count()*self.__perimeter*self.__gear_ratio speed = pid1.new_value(distance - distance1,0.01) speed_diff = pid2.new_value(distance1-distance2,0.01) print 'a' + str(self.__motor1.get_count()) self.__motor1.update_speed(speed) self.__motor2.update_speed(speed + speed_diff) self.__motor1.off() self.__motor2.off()
class PIDTester (object):
def __init__(self):
self.buggy = Buggy()
self.PID = PID(self.buggy.pos, self.buggy.angle)
def change_angle(self, direction):
print self.buggy.angle
while self.PID.value != direction:
print self.buggy.angle, "old angle"
new_angle = self.PID.update(self.buggy.angle, direction)
print new_angle, "new_angle"
direction = if self.buggy.angle > new_angle:
"left"
else: "right"
self.buggy.turn(direction, abs(new_angle - self.buggy.angle))
#camera = cameraRobot.getCamera("eagleCamera")
#camera = Camera("eagleCamera")
#camera.enable(TIME_STEP)
#camera.getImage()
#image = camera.getImage()
#data = np.array(image.getdata(), np.uint8).reshape(image.size[1], image.size[0], 3)
yaw_setpoint = -1
pitch_Kp = 2
pitch_Ki = 0.1
pitch_Kd = 2
roll_Kp = 2
roll_Ki = 0.1
roll_Kd = 2
pitchPID = PID.PID(float(pitch_Kp),
float(pitch_Ki),
float(pitch_Kd),
setpoint=0.0)
rollPID = PID.PID(float(roll_Kp), float(roll_Ki), float(roll_Kd), setpoint=0.0)
throttlePID = PID.PID(float(params["throttle_Kp"]),
float(params["throttle_Ki"]),
float(params["throttle_Kd"]),
setpoint=0.5)
yawPID = PID.PID(float(params["yaw_Kp"]),
float(params["yaw_Ki"]),
float(params["yaw_Kd"]),
setpoint=float(yaw_setpoint))
def headTo(targetX, targetY, targetZ, maxTimeToGetThere):
startTime = timer()
print([targetX, targetY, targetZ])
def __init__(self):
self.vicon_cb_flag = False
self.state_cb_flag = False
self.vel_sp_cb_flag = False
# setting params
self.vicon_yaw_sp = rospy.get_param(
'/attitude_thrust_publisher/vicon_yaw_sp')
self.P = rospy.get_param(
'/attitude_thrust_publisher/velocity_controller_P')
self.I = 0
self.D = rospy.get_param(
'/attitude_thrust_publisher/velocity_controller_D')
# calculating PID
self.vicon_y_pid = PID.PID(self.P, self.I, self.D)
self.vicon_x_pid = PID.PID(self.P, self.I, self.D)
# X axis of the vicon system should alwasy be aligned with the front of
# the quad
# Rate init
# DECIDE ON PUBLISHING RATE
self.rate = rospy.Rate(20.0) # MUST be more then 2Hz
# PUBLISHERS
self.attitude_target_pub = rospy.Publisher(
"/px4_quad_controllers/rpy_setpoint", PoseStamped, queue_size=10)
# SUBSCRIBERS
vicon_sub = rospy.Subscriber("/intel_aero_quad/odom", Odometry,
self.vicon_subscriber_callback)
state_sub = rospy.Subscriber("/mavros/state", State,
self.state_subscriber_callback)
vel_sp_sub = rospy.Subscriber("/px4_quad_controllers/vel_setpoint",
TwistStamped,
self.vel_sp_subscriber_callback)
while not rospy.is_shutdown():
self.P = rospy.get_param(
'/attitude_thrust_publisher/velocity_controller_P')
self.D = rospy.get_param(
'/attitude_thrust_publisher/velocity_controller_D')
self.vicon_y_pid.setKp(self.P)
self.vicon_y_pid.setKd(self.D)
self.vicon_x_pid.setKp(self.P)
self.vicon_x_pid.setKd(self.D)
if (self.vicon_cb_flag and self.state_cb_flag):
# Update setpoint
if (self.vel_sp_cb_flag == False):
self.vel_y_sp = rospy.get_param(
'/attitude_thrust_publisher/vel_y_sp')
self.vel_x_sp = rospy.get_param(
'/attitude_thrust_publisher/vel_x_sp')
else:
self.vel_y_sp = self.vel_sp_y_from_pos_ctrl
self.vel_x_sp = self.vel_sp_x_from_pos_ctrl
self.vicon_y_pid.SetPoint = self.vel_y_sp
self.vicon_x_pid.SetPoint = self.vel_x_sp
self.vicon_y_pid.update(self.vicon_y_pos)
self.vicon_x_pid.update(self.vicon_x_pos)
vicon_y_output = self.vicon_y_pid.output
vicon_x_output = -self.vicon_x_pid.output
target_attitude = PoseStamped()
target_attitude.header.frame_id = "home"
target_attitude.header.stamp = rospy.Time.now()
target_attitude.pose.position.x = -vicon_y_output * \
math.cos(self.yaw_change) - vicon_x_output * math.sin(self.yaw_change) # roll -
target_attitude.pose.position.y = -vicon_y_output * \
math.sin(self.yaw_change) + vicon_x_output * math.cos(self.yaw_change) # pitch
self.attitude_target_pub.publish(target_attitude)
self.rate.sleep()
# Create networking for publisher
context = zmq.Context()
socket = context.socket(zmq.SUB)
socket.connect("tcp://127.0.0.1:5557")
socket.setsockopt_string(zmq.SUBSCRIBE, '')
# Create networking for PUSH to plotting script
socket_push = context.socket(zmq.PUB)
socket_push.bind("tcp://127.0.0.1:5558")
# Create PID controller
Kp = 1
Kd = 1
Ki = 1
pid = PID.PID(Kp, Ki, Kd, 0)
pid.setKp(6)
A = np.matrix('[2 -3; 1 3]')
B = np.matrix('[1;-1]')
C = np.matrix('[1 0]')
D = 0
A2 = np.matrix('[0 1; -2 -3]')
B2 = np.matrix('[0;-1]')
C2 = np.matrix('[1 0]')
D2 = 0
sys = control.ss(A, B, C, D)
tf = control.ss2tf(sys)
poles = control.pole(sys)
# GPIO.setup(11, GPIO.OUT)
# GPIO.setup(12, GPIO.OUT)
# GPIO.setup(7, GPIO.OUT)
# pwm=GPIO.PWM(7, 100)
# pwm.start(0)
# drive1 = 0
# drive2 = 0
# motorPwm = 0
# pwm.ChangeDutyCycle(100)
# GPIO.output(7, True)
#
# Controller
tiltController = PID.PID()
tiltController.SetKp(Kp)
tiltController.SetKi(Ki)
tiltController.SetKd(Kd)
# Time initialization
startTime = time.time()
prevUpdateTime = 0
# Warmup Period
# This allows some of the initial crazy samples to bleed out
print("Warming up ...")
for i in range(1,101):
gyroRaw = sense.get_gyroscope_raw()
accelRaw = sense.get_accelerometer_raw()
magRaw = sense.get_compass_raw()
class Oven(object):
def __init__(self, heater_pin, temp_cs, top_pin=None,booster_pin=None, fan_pin = None):
self.heater_pin = heater_pin
self.fan_pin= fan_pin
self.top_pin = top_pin
self.booster_pin = booster_pin
self.thermocouple = MAX31855(SPI1,temp_cs)
#self.thermocouple = Client('/dev/ttyUSB0')
self.pid = PID(PID_KP, PID_KI, PID_KD)
#pinMode(heater_pin, OUTPUT)
GPIO.setup(heater_pin, GPIO.OUT)
#if (fan_pin): pinMode(fan_pin, OUTPUT)
if (fan_pin): GPIO.setup(fan_pin, GPIO.OUT)
else: self.fan_state = "disabled"
self.heatOff()
self.fanOff()
addToCleanup(self.stop)
self.current_phase = ''
self.current_step = 0
self.realtime_data = []
self.error = ''
self.abort = False
self.solder_type = LEADED
def run(self, profile=None, solder_type=LEADED):
""" Runs a single reflow cycle using the given profile if provided, or the
default profile for the given solder type. """
self.abort = False
self.error = ''
if (not profile):
if (solder_type == LEADED): profile = DEFAULT_LEADED
elif (solder_type == LEADFREE): profile = DEFAULT_LEADFREE
self.solder_type = solder_type
reflow_map = self.generateMap(profile, solder_type)
# Count the number of data points in the reflow map:
n_points = 0
for i in PHASES:
n_points += len(reflow_map[i])
# Initialize the realtime data array:
self.realtime_data = []
self.pid.reset()
time_step_s = TIME_STEP_MS / 1000.0
current_total_step = 0
for phase in PHASES:
self.current_phase = phase
self.current_step = 0
for target in reflow_map[phase]:
if (self.abort):
self.stop()
self.error = "Manually aborted."
print "**Reflow aborted!"
return
# Just using a 'bang-bang' type control for initial testing, I'll
# switch to the PID soon:
#state = self.pid.calculateState(self.getTemp(), i)
temp = self.getTemp()
if (temp == None):
self.stop()
self.error = self.thermocouple.error
print self.error
return
# Record current temp:
self.realtime_data.append([current_total_step*time_step_s, temp])
# Bang-bang for now:
if (temp < target):
self.heatOn()
self.fanOff()
print "%s - temp: %f, target: %f - on" % (phase, self.getTemp(), target)
else:
print "%s - temp: %f, target: %f - off" % (phase, self.getTemp(), target)
self.heatOff()
self.fanOn()
delay(TIME_STEP_MS)
self.current_step += 1
current_total_step += 1
self.error = ''
self.current_phase = ''
self.current_step = 0
def stop(self):
""" Flags the current phase to stop if it is being run in a separate thrad. """
self.current_phase = ''
self.current_step = 0
self.heatOff()
self.fanOff()
def heatOn(self):
#digitalWrite(self.heater_pin, HEATER_ON)
GPIO.output(self.heater_pin, GPIO.HIGH)
self.heat_state = "on"
def heatOff(self):
#digitalWrite(self.heater_pin, HEATER_ON^1)
GPIO.output(self.heater_pin, GPIO.LOW)
self.heat_state = "off"
def fanOn(self):
if (self.fan_pin):
#digitalWrite(self.fan_pin, FAN_ON)
GPIO.output(self.fan_pin, GPIO.HIGH)
self.fan_state = "on"
def fanOff(self):
if (self.fan_pin):
#digitalWrite(self.fan_pin, FAN_ON^1)
GPIO.output(self.fan_pin, GPIO.LOW)
self.fan_state = "off"
def getTemp(self):
for i in range(10):
# readTempC() return None if error; try a few times to make
# sure error isn't just a hiccup before aborting current reflow.
SPI1.open()
temp = self.thermocouple.readTempC()
if temp:
if (temp > 100): print '\n%s\n' % temp
return temp
# There was an error reading the temperature, abort:
print "**Error reading thermocouple:\n %s" % \
self.thermocouple.error
self.heatOff()
return None
def generateMap(self, profile, solder_type=None):
""" Generates and returns reflow map given a profile config dict. """
if not solder_type: solder_type = self.solder_type
reflow_map = {}
start_temp = self.getTemp()
if (start_temp == None):
# Error getting temperature, assume room temperature:
start_temp = 25.0
for phase in range(len(PHASES)):
tmin = LIMITS[PHASES[phase]]['tmin'][solder_type]
tmax = LIMITS[PHASES[phase]]['tmax'][solder_type]
dmin = LIMITS[PHASES[phase]]['dmin'][solder_type]
dmax = LIMITS[PHASES[phase]]['dmax'][solder_type]
slopemin = LIMITS[PHASES[phase]]['slopemin'][solder_type]
slopemax = LIMITS[PHASES[phase]]['slopemax'][solder_type]
config = profile[PHASES[phase]]
target_temp = config.get('target')
if (not target_temp):
assert phase > 0, "**Profile must contain pre_soak target_temp, aborting!"
target_temp = start_temp
duration = config.get('duration')
slope = config.get('slope')
assert duration or slope,\
"**duration or slope required for every phase; missing from %s, aborting!"%\
(PHASES[phase])
if (not duration):
# Calculate duration from slope
duration = float(target_temp-start_temp)/slope
elif (not slope):
# Calculate slope from duration
slope = float(target_temp-start_temp)/duration
#print "%s duration=%f" % (PHASES[phase], duration)
#print "%s slope=%f" % (PHASES[phase], slope)
#print "%s start_temp=%f" % (PHASES[phase], start_temp)
#print "%s target_temp=%f\n" % (PHASES[phase], target_temp)
# Create map of the current phase:
f = lambda ms : slope * float(ms/1000.0) + start_temp
n_steps = int((duration*1000.0)/TIME_STEP_MS)
reflow_map[PHASES[phase]] = []
for x in range(n_steps): reflow_map[PHASES[phase]].append(f(x*TIME_STEP_MS))
start_temp = target_temp
return reflow_map
lambdaoverd_arc = config['Image_params']['lambdaoverd_arc']
# reference values
Itot_off = config['QACITS_params']['Itot_off']
DIx_ref = config['QACITS_params']['DIx_ref']
DIy_ref = config['QACITS_params']['DIy_ref']
# PID loop gains
Kp = config['PID']['Kp']
Ki = config['PID']['Ki']
Kd = config['PID']['Kd']
#ipdb.set_trace()
# PID loop
p = pid.PID(P=np.array([Kp, Kp]),
I=np.array([Ki, Ki]),
D=np.array([Kd, Kd]),
Deadband=.001)
p.setPoint(np.array[0., 0.])
c = 1
while True:
img = pharo.get_image()
img = pre.equalize_image(img, masterflat=flat, bkgd=bgd, badpix=bpix)
# Derive center of the image from the satellite spots
# if c == 1 :
# spotcenters = dm.get_satellite_centroids(img)
# else :
# spotcenters = fit_satellite_centers(img, spotcenters, window=20)
spotcenters = fit_satellite_centers(img, spotcenters, window=20)
# Universal Power Supply Controller
# USAID Middle East Water Security Initiative
#
# Developed by: Nathan Webster
# Primary Investigator: Nathan Johnson
#
# Version History (mm_dd_yyyy)
# 1.00 03_24_2018_NW
#
######################################################
# Import Libraries
import PID
import Parameters
from UPS_Error import *
DC_PID = PID.PID(Parameters.P, Parameters.I, Parameters.D)
DC_PID.setSampleTime(Parameters.PID_Time)
DC_PID.SetPoint = Parameters.Voltage_Setpoint
def PWM_PID(DC_Voltage, D_PID_OLD):
DC_PID.update(DC_Voltage)
D_update = DC_PID.output
#print("D_update = ",D_update)
D = D_PID_OLD - D_update
#print(D)
if D > .8:
D_out = .8
elif D < .4:
D_out = .4
elif D <= .8 and D >= .4:
def main(self,num_samples, set_voltage, Father):
RE_VAL = set()
# Initialize Relay
REL = relay.Relay_module()
REL.reset()
# detect file
if APP_PEN_DRIVE:
FILE_NAME = detect_file.File(folder)[0]
else:
FILE_NAME = "/home/pi/Desktop/Temperature_profile.txt"
# Load Profile
TP = readTempProfile(FILE_NAME)[0]
print(TP)
self.NEXTION_NUM_SAMPLES = num_samples
self.NEXTION_SET_VOLTAGE = set_voltage
# import oven and multimeter
if APP_OVEN_PRESENT:
OVEN = VT4002_SM.VT4002(VT4002_IP)
OVEN.startComm()
P = 5
I = 0
D = 0
pid = PID.PID(P, I, D)
pid.setSampleTime(1)
if APP_Sourcemeter_PRESENT:
Sourcemeter = Keithley_2602_Eth.SourceMeter(SM_IP, SM_PORT)
Sourcemeter.startComm()
# create file and title
CF = data_storage.create_file()
test_mode = "Auto"
equipment_Info = "VT-4002 + Sourcemeter 2602"
Driver_root = detect_file.File(folder)[1]
start_time = str(datetime.datetime.now())
filename = start_time.replace(" ", "_").replace(".", "-").replace(":", "-")
PATH = CF.folder(Driver_root, filename)
CF.header(PATH, filename, start_time, equipment_Info, test_mode, self.NEXTION_NUM_SAMPLES)
CF.content(PATH, filename,
("Time(s)\tSetTemp.(oC)\tActual Temp.(oC)\tHumidity(%)\tTemp.Sensor(oC)\tSample number\tCurrent(A)\tResistence(ohm)\r\n"))
t_start = time.time()
for step in TP:
print(step)
# setting the oven
step_time = step[0] * 60 # step_time in seconds
step_temp = float(format(float(step[1]) / 0.94, ".2f"))
t1 = datetime.datetime.now()
t2 = datetime.datetime.now() + datetime.timedelta(seconds=step_time)
while( t1 < t2 ):
# Relay switching
REL.reset()
for i in range(self.NEXTION_NUM_SAMPLES):
if APP_OVEN_PRESENT:
# run oven)
print("01 - Reading data from Oven...")
temp = OVEN.read_temp()
temp_set = temp[0]
temp_real = temp[1]
else:
temp_set = format(1.00, "0.2f")
temp_real = format(1.00, "0.2f")
pid.SetPoint = step_temp
pid.setKp(float(P))
pid.setKi(float(I))
pid.setKd(float(D))
print("02 - Reading data from Humidity Sensor...")
if APP_BME_280_PRESENT:
try:
temperature, pressure, humidity = bme280.readBME280All()
# Medicine
if ((humidity == None) or (temperature == None)):
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
print("02 - Reading data from Humidity Sensor (NONE! - ERROR)...")
elif ((type(humidity) == str) or (type(temperature) == str)):
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
print("02 - Reading data from Humidity Sensor (INVALID STRING! - ERROR)...")
except:
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
print("02 - Reading data from Humidity Sensor (INVALID STRING! - ERROR)...")
else:
print("02 - Reading data from Humidity Sensor (DISABLED)...")
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
HUMI_sensor = format(humidity, "0.2f")
TEMP_sensor = format(temperature, "0.2f")
print("02 - Reading data from Humidity Sensor: Temp(oC): ", TEMP_sensor)
print("02 - Reading data from Humidity Sensor: Humi(%): ", HUMI_sensor)
print("Sensor Temperature : ", str(TEMP_sensor))
pid.update(float(TEMP_sensor))
target_temperature = pid.output
if target_temperature > 180:
target_temperature = 180
elif target_temperature < -40:
target_temperature = -40
else:
target_temperature = target_temperature
OVEN.set_temp(target_temperature)
print("PID set Temperature : ", str(target_temperature))
print("Chamber real Temperature : ", temp_real)
# run time
t_step = time.time()
t_run = format(t_step - t_start, "0.2f")
REL.RelaySelect(i)
sleep(RELAY_HOLDING_TIME)
if APP_Sourcemeter_PRESENT:
fcurrent = Sourcemeter.Measure("a", self.NEXTION_SET_VOLTAGE)
print("Reading Sourcemeter:", i)
else:
fcurrent = (i*float(t_run))
I_Value = format(fcurrent, ".3E")
R_Value = format(self.NEXTION_SET_VOLTAGE / float(I_Value), ".3E")
print("Resistance:", R_Value)
print("")
REL.RelayDeSelect(i)
# Persistency
print("06 - Saving data...")
result1 = []
result2 = []
result1.append([
str(t_run), str(step[1]), str(temp_real), str(HUMI_sensor), str(TEMP_sensor), str(i),
str(I_Value),
str(R_Value)])
CF.content(PATH, filename, result1)
# create file for each sample
result2.append([
str(t_run), str(step[1]), str(temp_real), str(HUMI_sensor), str(TEMP_sensor), str(i),
str(I_Value),
str(R_Value)])
CF.content(PATH, "Sample" + str(i), result2)
Father.updateMultimeter([temp_set, temp_real, TEMP_sensor, HUMI_sensor, t_run, R_Value, i])
DE = str(DIS.read())
# print (DE)
RE_VAL.add(DE)
# print (RE_VAL)
if "['e\\x0e\\x13\\x01\\xff\\xff\\xff']" in RE_VAL:
print("Exiting")
OVEN.close()
RE_VAL.clear()
DIS.write("page Device Select\xff\xff\xff")
return
elif "['e\\x0e\\x14\\x01\\xff\\xff\\xff']" in RE_VAL:
# DIS.write("rest\xff\xff\xff")
RE_VAL.clear()
DIS.write("page restart\xff\xff\xff")
os.system("sudo reboot")
print("07 - Updating Display...")
t1 = datetime.datetime.now()
if APP_OVEN_PRESENT:
OVEN.close()
def run_building_sim_range(building_simulation, district_heating_ini, time_ratio, P, I, D, upLim, lowLim, sample_time, total_sampling,\
solar_radiance, water_capacity, water_densidty):
pid = PID.PID(P, I, D)
pid.upLim = upLim
pid.lowLim = lowLim
pid.setSampleTime(sample_time)
pid.windup_guard = windup_guard
# Initialize indoor temperature
feedback = district_heating_ini.average_indoor_temperature
feedback_list = []
time_list = []
output_list = []
inflow_temp_list = []
outside_temperature_list = []
print("start testing....")
for i in range(1, total_sampling):
time_list.append(i)
feedback_list.append(feedback)
district_heating_ini.outside_temperature = outside_temperature[i-1]
pid.update_range(feedback)
output = pid.output
output_list.append(output)
# Update manipulated_inflow_temperature based on PID output
manipulated_inflow_temperature = district_heating_ini.district_inflow_temperature + output
# manipulated_inflow_temperature has upper and lower limits
if manipulated_inflow_temperature > district_heating_ini.inflow_temp_max:
district_heating_ini.district_inflow_temperature = district_heating_ini.inflow_temp_max
elif manipulated_inflow_temperature < district_heating_ini.inflow_temp_min:
district_heating_ini.district_inflow_temperature = district_heating_ini.inflow_temp_min
else:
district_heating_ini.district_inflow_temperature = manipulated_inflow_temperature
# Simulate indoor temperature for next time point based on manipulated_inflow_temperature
feedback = building_simulation.computeTemperature(time_ratio, solar_radiance[i-1], water_capacity, water_densidty, district_heating_ini)
time.sleep(0.02)
inflow_temp_list.append(district_heating_ini.district_inflow_temperature)
outside_temperature_list.append(district_heating_ini.outside_temperature)
print(feedback_list)
print(output_list)
print(time_list)
print(inflow_temp_list)
print(outside_temperature_list)
# plot results
plt.figure(1)
plt.subplot(411)
plt.plot(time_list, feedback_list, 'b--')
plt.hlines(upLim, 1, len(time_list), 'red')
plt.hlines(lowLim, 1, len(time_list), 'red')
plt.ylabel('$^\circ$C').set_rotation(0)
plt.title('indoor temperature')
plt.subplot(412)
plt.plot(time_list, output_list, 'b--')
plt.ylabel('$^\circ$C').set_rotation(0)
plt.title('PID output')
plt.subplot(413)
plt.plot(time_list, inflow_temp_list, 'b--')
plt.ylabel('$^\circ$C').set_rotation(0)
plt.title('Adjusted inflow temperature')
plt.subplot(414)
plt.plot(time_list, outside_temperature_list, 'b--')
plt.ylabel('$^\circ$C').set_rotation(0)
plt.title('Outside temperature')
plt.subplots_adjust(hspace=0.5)
plt.show()
def __init__(self):
self.vicon_cb_flag = False
self.state_cb_flag = False
self.state_pid_reset_flag = False
self.pid_reset_flag = False
self.pos_sp_cb_flag = False
self.update_pos_sp_flag = False
self.P = rospy.get_param('/attitude_thrust_publisher/height_hover_P')
self.I = rospy.get_param('/attitude_thrust_publisher/height_hover_I')
self.D = rospy.get_param('/attitude_thrust_publisher/height_hover_D')
self.height_pid = PID.PID(self.P, self.I, self.D)
self.timer_flag = True
self.time_threshold = rospy.get_param(
'/attitude_thrust_publisher/timer_threshold')
# Rate init
# DECIDE ON PUBLISHING RATE
self.rate = rospy.Rate(100.0) # MUST be more then 2Hz
self.height_target_pub = rospy.Publisher(
"/px4_quad_controllers/thrust_setpoint",
PoseStamped,
queue_size=10)
# ADD SUBSCRIBER FOR VICON DATA
vicon_sub = rospy.Subscriber("/intel_aero_quad/odom", Odometry,
self.vicon_sub_callback)
state_sub = rospy.Subscriber("/mavros/state", State,
self.state_subscriber_callback)
rospy.Subscriber("/evdodge/positionSetpoint", PoseStamped,
self.positionSetpoint_callback)
while not rospy.is_shutdown():
if (self.vicon_cb_flag and self.state_cb_flag):
# Update PID
self.P = rospy.get_param(
'/attitude_thrust_publisher/height_hover_P')
self.I = rospy.get_param(
'/attitude_thrust_publisher/height_hover_I')
self.D = rospy.get_param(
'/attitude_thrust_publisher/height_hover_D')
self.height_pid.setKp(self.P)
self.height_pid.setKi(self.I)
self.height_pid.setKd(self.D)
# Update setpoint
if (not self.pos_sp_cb_flag):
self.height_sp = rospy.get_param(
'/attitude_thrust_publisher/height_sp')
else:
self.height_sp = self.pos_sp_z
# rospy.loginfo(self.update_pos_sp_flag)
if self.update_pos_sp_flag:
if self.pos_update_z != 0:
print 'crossed'
if self.timer_flag:
start_time = time.time()
self.timer_flag = False
timer_count = time.time() - start_time
if timer_count < self.time_threshold:
self.height_pid.SetPoint = self.height_sp + self.pos_update_z
else:
self.height_pid.SetPoint = self.height_sp
else:
self.height_pid.SetPoint = self.height_sp
rospy.loginfo('height sp' + str(self.height_pid.SetPoint))
else:
self.height_pid.SetPoint = self.height_sp
self.height_pid.update(self.vicon_height)
# For this to work, we have to align x,y of quad and vicon
self.hover_thrust = rospy.get_param(
'/attitude_thrust_publisher/hover_thrust')
thrust_output = self.height_pid.output + self.hover_thrust
target_thrust = PoseStamped()
target_thrust.header.frame_id = "home"
target_thrust.header.stamp = rospy.Time.now()
self.min_thrust = rospy.get_param(
'/attitude_thrust_publisher/min_thrust')
self.max_thrust = rospy.get_param(
'/attitude_thrust_publisher/max_thrust')
# Thrust threshold
if (thrust_output <= self.max_thrust
and thrust_output >= self.min_thrust):
target_thrust.pose.position.x = thrust_output
elif (thrust_output > self.max_thrust):
target_thrust.pose.position.x = self.max_thrust
elif (thrust_output < self.min_thrust):
target_thrust.pose.position.x = self.min_thrust
else:
print("HEIGHT CONTROLLER ERROR!")
target_thrust.pose.position.x = self.min_thrust
self.height_target_pub.publish(target_thrust)
self.rate.sleep()
"target": None,
"start": False,
"data_fresh": False,
}
)
controller = BathController(config.get("Connection", "Port"), config.get("Connection", "Baud"))
time.sleep(3)
print "\rLift off!"
get_temp()
# Start main window thread
window_thread = Process(target=window_main, args=(sys.argv, shared_memory))
window_thread.daemon = True
window_thread.start()
# Initialise PID object
pid = PID.control(shared_memory["p"] / 10000.0, shared_memory["i"] / 10000.0, shared_memory["d"] / 10000.0)
# Whilst the UI is open
while window_thread.is_alive():
# Kick over
get_temp()
# get_temp()
if shared_memory["start"] and shared_memory["data_fresh"]:
cycleStart = time.time()
dist = None
target_reached = False
while shared_memory["start"] and window_thread.is_alive():
cycleStart = time.time()
To = shared_memory["target"]
Ta = shared_memory["env_temp"]
Tb = shared_memory["bath_temp"]
m = shared_memory["mass"]
'target':None,
'start':False,
'data_fresh':False
})
controller = BathController(config.get('Connection', 'Port'),
config.get('Connection', 'Baud'))
time.sleep(3)
print "\rLift off!"
get_temp()
# Start main window thread
window_thread = Process(target = window_main, args = (sys.argv, shared_memory))
window_thread.daemon = True
window_thread.start()
# Initialise PID object
pid = PID.control(shared_memory['p'], shared_memory['i'], shared_memory['d'])
# Whilst the UI is open
while window_thread.is_alive():
# Kick over
get_temp()
#get_temp()
if shared_memory['start'] and shared_memory['target'] is not None:
To = shared_memory['target']
Ta = shared_memory['env_temp']
m = shared_memory['mass']
h = shared_memory['heatCapacity']
e = shared_memory['emissivity']
a = shared_memory['area']
w = resistance_to_watts(shared_memory['resistance'], shared_memory['voltage'])
set_point = temperature_to_joules(To, Ta, m, h)
def run():
minimum_speed = 70.0
maximum_speed = 95.0
ratio_queue = [0]
layout_queue = ["normal_straight"]
last_ratio = 0
direction_list = ["left", "left", "right"]
x_wheel_left = 0
y_wheel_left = 287
x_wheel_right = 480
y_wheel_right = 287
last_ratio = 0.0
column_factor = 0.02 # <0.5
row_factor = 0.02 # <0.5
des = 0
pd1 = PD(50.0, 20.0, 0.0)
pid2 = PID(1.0, 0.0, 0.0, 0.0)
while True:
start = time.time()
try:
rat, layout_queue, ratio_queue, direction_list = ratio.get_ratio(
"/dev/shm/cam.jpg",
x_wheel_left,
y_wheel_left,
x_wheel_right,
y_wheel_right,
last_ratio,
column_factor,
row_factor,
ratio_queue,
layout_queue,
direction_list,
)
except:
print "ERROR"
rat = last_ratio
last_ratio = rat
if rat > 0:
rat = abs(rat)
global_speed = pd1.value(math.log(1.0 / rat), 0.1)
global_speed = min(max(minimum_speed, global_speed), maximum_speed)
new_ratio = 1 - pid2.value(rat, 0.1)
if new_ratio < 0.2:
data = (global_speed, global_speed * -1)
else:
data = (global_speed, global_speed * new_ratio * 1.5)
elif rat == 0:
data = (maximum_speed, maximum_speed)
else:
rat = abs(rat)
global_speed = pd1.value(math.log(1.0 / (rat)), 0.1)
global_speed = min(max(minimum_speed, global_speed), maximum_speed)
new_ratio = 1 - pid2.value(rat, 0.1)
if new_ratio < 0.2:
data = (global_speed * -1, global_speed)
else:
data = (global_speed * new_ratio * 1.5, global_speed)
dsckt.send(data)
end = time.time()
print end - start
if end - start < 0.1:
time.sleep(0.1 - (end - start))
y1_record = [] y2_record = [] y1_0 = 0.383 y2_0 = 0.4 # parameters for PID controller P = 0.0 I_1 = 0.000011 D_1 = 0.00001 I_2 = 0.000038 D_2 = 0.000038 pid1 = PID.PID(P, I_1, D_1) pid2 = PID.PID(P, I_2, D_2) pid1.SetPoint = f1_d pid2.SetPoint = f2_d pid1.setSampleTime(0.1) pid2.setSampleTime(0.1) # discard the first few noisy readings for i in range(20): discard1 = lift1_status.InValue['force'] discard2 = lift2_status.InValue['force'] discard3 = arm1_status.InValue['force'] discard4 = arm2_status.InValue['force'] time.sleep(0.05) # the motor sampling frequency is 25 Hz
def step(self, action): # 焊接过程走一步的函数，即根据强化学习的动作值得出下一步焊道的形状
done = False
self.counter += 1
# obtain error and delta error
self.H_error = self.H_target - self.H_prediction # 误差值
self.H_del_error = self.H_error - self.H_error_last # 误差值的差分
self.H_error_last = self.H_error
self.W_error = self.W_target - self.W_prediction # 误差值
self.W_del_error = self.W_error - self.W_error_last # 误差值的差分
self.W_error_last = self.W_error
# pid control the robot speed
# 底层为PID控制器，强化学习的作用为调节PID控制器的参数1
# pid1 = PID.PID(self.Kp + learning_rate * action[0]*0.1, self.Ki + learning_rate * action[1]*0.1, self.Kd + learning_rate * action[2]*0.1 )
# pid2 = PID.PID(self.Kp2 + learning_rate * action[3]*0.1, self.Ki2 + learning_rate * action[4]*0.1, self.Kd2 + learning_rate * action[5]*0.1)
pid1 = PID.PID(self.Kp, self.Ki, self.Kd)
pid2 = PID.PID(self.Kp2, self.Ki2, self.Kd2)
action_np = np.zeros((6, 1), dtype="float32")
action_np[0, 0] = action[0]
action_np[1, 0] = action[1]
action_np[2, 0] = action[2]
action_np[3, 0] = action[3]
action_np[4, 0] = action[4]
action_np[5, 0] = action[5]
self.delta_A = pid1.update(self.H_error, self.H_del_error)
self.delta_B = pid2.update(self.W_error, self.W_del_error)
# 28组数据反解
self.delta_Rs = -12.1674 * self.delta_A + 3.0336 * self.delta_B
self.delta_Ws = -26.0291 * self.delta_A + 2.1745 * self.delta_B
self.Rs += self.delta_Rs
self.Ws += self.delta_Ws
self.Rs = np.clip(self.Rs, 3, 13)
self.Ws = np.clip(self.Ws, self.Rs, 2 * self.Rs)
print(self.Rs, self.Ws)
self.H_actual = height_lstm.welding_pred(
self.Rs_list[-height_lstm.TIMESTEPS:],
self.Ws_list[-height_lstm.TIMESTEPS:])
self.W_actual = width_lstm.welding_pred(
self.Rs_list[-width_lstm.TIMESTEPS:],
self.Ws_list[-width_lstm.TIMESTEPS:])
for num in range(0, 60):
self.Rs_list.append(self.Rs[0]) ## wrong ???
self.Ws_list.append(self.Ws[0])
# print("original:",np.shape(self.Rs_list))
# 将新的焊接参数序列输入焊接过程模型，得到新的焊道形状预测值
self.H_prediction = height_lstm.welding_pred(
self.Rs_list[-height_lstm.TIMESTEPS:],
self.Ws_list[-height_lstm.TIMESTEPS:])
self.W_prediction = width_lstm.welding_pred(
self.Rs_list[-width_lstm.TIMESTEPS:],
self.Ws_list[-width_lstm.TIMESTEPS:])
# print(self.Rs_list[-1 : ], self.Ws_list[-1 : ], self.H_prediction, self.target)
for num in range(0, 59):
self.Rs_list.pop() ## wrong ???
self.Ws_list.pop()
# print("after:", np.shape(self.Rs_list))
# reward
# if abs(self.H_target - self.H_prediction) < 0.002 and abs(self.W_target - self.W_prediction) < 0.005:
# self.on_goal += 1
# r = 1
# if self.on_goal > 80:
# done = True
# else:
# r = 1 / (1 + np.exp((abs(self.H_target - self.H_prediction) + abs(self.W_target - self.W_prediction)))) - 0.5
# # r = - (abs(self.H_target - self.H_prediction) + abs(self.W_target - self.W_prediction))
# # self.on_goal = 0
if abs(self.H_target - self.H_prediction) < 0.002 and abs(
self.W_target - self.W_prediction) < 0.005:
self.on_goal += 1
r = 1
if self.on_goal > 80:
done = True
else:
r = 1 / (1 + np.exp(
abs(self.H_target - self.H_prediction) +
abs(self.W_target - self.W_prediction))) - 0.5
self.on_goal = 0
# if self.flag>60:
# done = True
# r = -100
# state
s = np.hstack(
((self.H_prediction - 2), self.H_error, self.H_del_error,
((self.W_prediction - 5) / 3), self.W_error, self.W_del_error,
(self.Rs - 8) / 5, ((self.Ws - 14.5) / 11.5)))
# s = np.hstack(((self.H_prediction - 2), self.H_target, ((self.W_prediction - 5) / 3),
# self.W_target))
#print(s,r)
return s, r, done
import base64
import picamera
from picamera.array import PiRGBArray
import argparse
import imutils
from collections import deque
import psutil
import os
import servo
import PID
import LED
import datetime
from rpi_ws281x import *
import move
pid = PID.PID()
pid.SetKp(0.5)
pid.SetKd(0)
pid.SetKi(0)
Y_lock = 0
X_lock = 0
tor = 17
FindColorMode = 0
WatchDogMode = 0
UltraData = 3
LED = LED.LED()
class FPV:
def __init__(self):
self.frame_num = 0
import cv2
import numpy as np
import PID
import RobotSerial
import time
from scipy.interpolate import spline
import matplotlib.pyplot as plt
count = 0
vid = cv2.VideoCapture(1)
vid.set(cv2.CAP_PROP_EXPOSURE, -8)
robot = RobotSerial.RobotSerial()
pidAngle = PID.PID(.7, 0, 0.005)
pidAngle.SetPoint = 320.0
pidAngle.setSampleTime(0.01)
pidDist = PID.PID(.75, 0, 0.005)
pidDist.SetPoint = 90.0
pidDist.setSampleTime(0.01)
time.sleep(.5)
while (True):
ret, frame = vid.read()
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower_green = np.array([30, 100, 100])
upper_green = np.array([90, 255, 255])
mask = cv2.inRange(hsv, lower_green, upper_green)
UART.setup("UART4")
Left_Motor_Pin = "P8_19"
Right_Motor_Pin = "P9_14"
Left_Motor_Direction = "P9_26"
Right_Motor_Direction = "P9_25"
Start_Detection = "P8_7"
ser = serial.Serial(port = "/dev/ttyO4", baudrate = 9600)
GPIO.setup(Left_Motor_Direction, GPIO.OUT)
GPIO.setup(Right_Motor_Direction, GPIO.OUT)
GPIO.setup(Start_Detection,GPIO.IN)
PWM.start(Left_Motor_Pin, 0.0, 20000, 1) #Motor 1 (Left)
PWM.start(Right_Motor_Pin, 0.0, 20000, 1) #Motor 2 (Right)
p = PID(0.001,0.0,0.0) # set up PID with KP, KI, KD
p.setPoint(6500) #setpoint to middle of device.
black_line = 200
GPIO.output(Left_Motor_Direction, GPIO.HIGH)
GPIO.output(Right_Motor_Direction, GPIO.HIGH)
var = 1
base_speed = 15
eventCount = 0
#time.sleep(5) #wait for calibration of line follower
#1 = dont go (LED on)
#0 = go (LED off)
def fuzzy_and(typ, x, y): # two types if typ == 0: return min(x, y) else: return x * y def fuzzy_or(typ, x, y): if typ == 0: return max(x, y) else: return x + y - x * y if __name__ == "__main__": pid = PID.PID(1, 1, 0.5, 10) pid.point = 1 pid.sample_time = 0.01 pid.windup = 5 feedback = 0 feedback_list = [] time_list = [] setpoint_list = [] fuzzy_P = [0, 0, 0, 0, 0, 0, 0] fuzzy_I = [0, 0, 0, 0, 0, 0, 0] fuzzy_D = [0, 0, 0, 0, 0, 0, 0] L = 100
if LOG_CYCLE_TIME:
plotCycleTime(t_hist, "plot", FRAMERATE, fname="cycle_time")
if LOG_SCAN_LINE:
np.save('analysis/scan_hist.npy', np.array(scan_hist))
if LOG_LINE_POS:
plotPos(pos_hist, "plot", fname="line_pos")
atexit.register(exit_handler)
input("Press Enter to continue ...")
# Measure the time needed to process 300 images to estimate the FPS
ti = t = time.time()
pid = PID.PID(Kp_straight, Ki_straight, Kd_straight)
pid.SetPoint = CAMERA_CENTER
#pid.setSampleTime(1/camera.framerate)
if USE_STATE:
pid.setState(PID_STATE)
# start car
motor.duty_cycle = MOTOR_BRAKE + SPEED
# Initialize variables
line_pos = CAMERA_CENTER
first_frame = True
state = 0 # 0: straight, 1: turn
# Logging
if LOG_FRAME:
import PID
euler_pidx = PID.PID()
euler_pidx.clear
euler_pidy = PID.PID()
euler_pidy.clear
euler_pidz = PID.PID()
euler_pidz.clear
class ctrl:
def __init__(self, rx0=0.0, ry0=0.0, rz0=0.0):
self.rollx = rx0
self.rolly = ry0
self.rollz = rz0
self.clear()
def clear(self):
self.dP = [1, 1, 1]
self.motorP = [60, 60, 60, 60]
def motor(self, rx, ry, rz):
self.euler = [rx, ry, rz]
euler_pidx.update(rx)
d_rx = euler_pidx.output
euler_pidy.update(ry)
d_ry = euler_pidy.output
euler_pidy.update(rz)
def main(self, num_samples, set_temperature, setV, setF, Father):
RE_VAL = set()
# Initialize Relay
REL = relay.Relay_module()
REL.reset()
# set PID parameters
P = 5
I = 0
D = 0
pid = PID.PID(P, I, D)
pid.setSampleTime(1)
if APP_NEXTION_PRESENT:
self.NEXTION_NUM_SAMPLES = num_samples
self.NEXTION_SET_TEMP = float(
format(float(set_temperature) / 0.84, ".2f"))
else:
self.NEXTION_NUM_SAMPLES = 8
self.NEXTION_SET_TEMP = 25
# import oven and multimeter
if APP_OVEN_PRESENT:
OVEN = TTC4006_Tira.TTC4006(TTC_SERIAL_PORT)
OVEN.TTC_ON()
OVEN.TTC_ENABLE_TEMP()
if APP_DMM_196_PRESENT:
LCR = LCR_Eth.LCR_Meter(LCR_IP, LCR_PORT)
LCR.startComm()
# create file and title
CF = data_storage.create_file()
test_mode = "Manual"
equipment_Info = "VT-4002 + LCR-4192A"
Driver_root = detect_file.File(folder)[1]
start_time = str(datetime.datetime.now())
filename = start_time.replace(" ", "_").replace(".",
"-").replace(":", "-")
PATH = CF.folder(Driver_root, filename)
CF.header(PATH, filename, start_time, equipment_Info, test_mode,
self.NEXTION_NUM_SAMPLES)
CF.content(
PATH, filename,
"Time(s)\tSetTemp.(oC)\tActual Temp.(oC)\tHumidity(%)\tTemp.Sensor(oC)\tSample number\tR\tX\tCP\tRP\r\n"
)
t_start = time.time()
while True:
for i in range(self.NEXTION_NUM_SAMPLES):
if APP_OVEN_PRESENT:
# run oven)
print("01 - Reading data from Oven...")
temp_real = OVEN.TTC_Read_PV_Temp()
temp_set = OVEN.TTC_Read_SP_Temp()
else:
temp_set = format(1.00, "0.2f")
temp_real = format(1.00, "0.2f")
pid.SetPoint = self.NEXTION_SET_TEMP
pid.setKp(float(P))
pid.setKi(float(I))
pid.setKd(float(D))
# read temperature sensor
# Humidity Sensor
# If is not OK => apply non valid temp and humidity
print("02 - Reading data from Humidity Sensor...")
if APP_BME_280_PRESENT:
try:
temperature, pressure, humidity = bme280.readBME280All(
)
# Medicine
if ((humidity == None) or (temperature == None)):
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
print(
"02 - Reading data from Humidity Sensor (NONE! - ERROR)..."
)
elif ((type(humidity) == str)
or (type(temperature) == str)):
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
print(
"02 - Reading data from Humidity Sensor (INVALID STRING! - ERROR)..."
)
except:
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
print(
"02 - Reading data from Humidity Sensor (INVALID STRING! - ERROR)..."
)
else:
print(
"02 - Reading data from Humidity Sensor (DISABLED)...")
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
HUMI_sensor = format(humidity, "0.2f")
TEMP_sensor = format(temperature, "0.2f")
print("02 - Reading data from Humidity Sensor: Temp(oC): ",
TEMP_sensor)
print("02 - Reading data from Humidity Sensor: Humi(%): ",
HUMI_sensor)
print("Sensor Temperature : ", str(TEMP_sensor))
pid.update(TEMP_sensor)
target_temperature = pid.output
if target_temperature > 130:
target_temperature = 130
elif target_temperature < -40:
target_temperature = -40
else:
target_temperature = target_temperature
print("PID set Temperature : ", str(target_temperature))
print("Chamber real Temperature : ", temp_real)
OVEN.TTC_Set_Temp(target_temperature)
# run time
t_step = time.time()
t_run = format(t_step - t_start, "0.2f")
# relay selection
print("03 - Swtich Relay: %d" % i)
REL.RelaySelect(i)
sleep(RELAY_HOLDING_TIME)
# run multimeter
print("04- Multimeter DMM196 Reading...")
if APP_DMM_196_PRESENT:
FinalD = LCR.measure(setV, setF)
print("LCR Reading: ", FinalD)
CP = str(FinalD[0])
RP = str(FinalD[1])
R_value = str(FinalD[2])
X_value = str(FinalD[3])
else:
CP = 0
RP = 0
R_value = 0
X_value = 0
REL.RelayDeSelect(i)
# Persistency
print("06 - Saving data...")
print("")
result1 = []
result2 = []
result1.append([
str(t_run),
str(temp_real),
str(HUMI_sensor),
str(TEMP_sensor),
str(i),
str(R_value),
str(X_value),
str(CP),
str(RP)
])
CF.content(PATH, filename, result1)
# create file for each sample
result2.append([
str(t_run),
str(temp_real),
str(HUMI_sensor),
str(TEMP_sensor),
str(i),
str(R_value),
str(X_value),
str(CP),
str(RP)
])
CF.content(PATH, 'Sample' + str(i), result2)
Father.Update([
temp_set, temp_real, TEMP_sensor, humidity, t_run, R_value,
X_value, i
])
DE = str(DIS.read())
# print (DE)
RE_VAL.add(DE)
# print (RE_VAL)
if "['e\\x0f\\x1b\\x01\\xff\\xff\\xff']" in RE_VAL:
print("Exiting")
RE_VAL.clear()
OVEN.TTC_OFF()
DIS.write("page Device Select\xff\xff\xff")
return
elif "['e\\x0f\\x1c\\x01\\xff\\xff\\xff']" in RE_VAL:
# DIS.write("rest\xff\xff\xff")
RE_VAL.clear()
DIS.write("page restart\xff\xff\xff")
os.system("sudo reboot")
print("07 - Updating Display...")
def pid_loop(dummy, state):
import sys
from time import sleep, time
from math import isnan
import Adafruit_GPIO.SPI as SPI
import Adafruit_MAX31855.MAX31855 as MAX31855
import PID as PID
import config as conf
def c_to_f(c):
return c * 9.0 / 5.0 + 32.0
sensor = MAX31855.MAX31855(spi=SPI.SpiDev(conf.spi_port, conf.spi_dev))
pid = PID.PID(conf.Pc, conf.Ic, conf.Dc)
pid.SetPoint = state['settemp']
pid.setSampleTime(conf.sample_time * 5)
nanct = 0
i = 0
j = 0
pidhist = [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]
avgpid = 0.
temphist = [0., 0., 0., 0., 0.]
avgtemp = 0.
lastsettemp = state['settemp']
lasttime = time()
sleeptime = 0
iscold = True
iswarm = False
lastcold = 0
lastwarm = 0
try:
while True: # Loops 10x/second
tempc = sensor.readTempC()
if isnan(tempc):
nanct += 1
if nanct > 100000:
sys.exit
continue
else:
nanct = 0
tempf = c_to_f(tempc)
temphist[i % 5] = tempf
avgtemp = sum(temphist) / len(temphist)
if avgtemp < 100:
lastcold = i
if avgtemp > 200:
lastwarm = i
if iscold and (i - lastcold) * conf.sample_time > 60 * 15:
pid = PID.PID(conf.Pw, conf.Iw, conf.Dw)
pid.SetPoint = state['settemp']
pid.setSampleTime(conf.sample_time * 5)
iscold = False
if iswarm and (i - lastwarm) * conf.sample_time > 60 * 15:
pid = PID.PID(conf.Pc, conf.Ic, conf.Dc)
pid.SetPoint = state['settemp']
pid.setSampleTime(conf.sample_time * 5)
iscold = True
if state['settemp'] != lastsettemp:
pid.SetPoint = state['settemp']
lastsettemp = state['settemp']
if i % 10 == 0:
pid.update(avgtemp)
pidout = pid.output
pidhist[i / 10 % 10] = pidout
avgpid = sum(pidhist) / len(pidhist)
state['i'] = i
state['tempf'] = round(tempf, 2)
state['avgtemp'] = round(avgtemp, 2)
state['pidval'] = round(pidout, 2)
state['avgpid'] = round(avgpid, 2)
state['pterm'] = round(pid.PTerm, 2)
if iscold:
state['iterm'] = round(pid.ITerm * conf.Ic, 2)
state['dterm'] = round(pid.DTerm * conf.Dc, 2)
else:
state['iterm'] = round(pid.ITerm * conf.Iw, 2)
state['dterm'] = round(pid.DTerm * conf.Dw, 2)
state['iscold'] = iscold
print time(), state
sleeptime = lasttime + conf.sample_time - time()
if sleeptime < 0:
sleeptime = 0
sleep(sleeptime)
i += 1
lasttime = time()
finally:
pid.clear
#!/usr/bin/env python
#@uthor : $umanth_Nethi
import rospy
import numpy as np
import PID
import time
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped
rospy.init_node('drone', anonymous=True)
global P, I, D, pid_x, pid_y
P = 0.004
I = 0.000005
D = 0.0005
pid_x = PID.PID(P, I, D)
pid_x.SetPoint = 0.0
pid_x.setSampleTime(0.01)
pid_y = PID.PID(P, I, D)
pid_y.SetPoint = 0.0
pid_y.setSampleTime(0.01)
def callback(data):
assign(data)
rospy.Subscriber("cv_bounding_box", PoseStamped, callback)
current_pos = PoseStamped()
def current_pos_callback(position):
def test_pid(P = 0.2, I = 0.0, D= 0.0, L=100):
"""Self-test PID class
.. note::
...
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1/i))
if i>9:
pid.SetPoint = 1
time.sleep(0.02)
---
"""
pid = PID.PID(P, I, D)
pid.SetPoint=0.0
pid.setSampleTime(0.01)
END = L
feedback = 0
feedback_list = []
time_list = []
setpoint_list = []
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output)
if i==9:
pid.SetPoint = 1
if i>20:
pid.SetPoint += 0.1
time.sleep(0.02)
feedback_list.append(feedback)
setpoint_list.append(pid.SetPoint)
time_list.append(i)
""" this is a multiline comment
time_sm = np.array(time_list)
time_smooth = np.linspace(time_sm.min(), time_sm.max(), 300)
feedback_smooth = spline(time_list, feedback_list, time_smooth)
plt.plot(time_smooth, feedback_smooth)
plt.plot(time_list, setpoint_list)
plt.xlim((0, L))
plt.ylim((min(feedback_list)-0.5, max(feedback_list)+0.5))
plt.xlabel('time (s)')
plt.ylabel('PID (PV)')
plt.title('TEST PID')
plt.ylim((1-0.5, 1+0.5))
plt.grid(True)
plt.show()
"""
for i in range(1,END-1):
print("%f, %f" % (feedback_list[i], setpoint_list[i]) )
def __init__(self):
self.vicon_cb_flag = False
self.state_cb_flag = False
self.vicon_yaw_sp = rospy.get_param(
'/attitude_thrust_publisher/vicon_yaw_sp')
self.pos_sp_cb_flag = False
# get the info for the PID and for the set point
# PID Data
self.dx_P = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_x_P')
self.dx_I = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_x_I')
self.dx_D = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_x_D')
self.dy_P = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_y_P')
self.dy_I = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_y_I')
self.dy_D = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_y_D')
self.vicon_dx_pid = PID.PID(self.dx_P, self.dx_I, self.dx_D)
self.vicon_dy_pid = PID.PID(self.dy_P, self.dy_I, self.dy_D)
# Rate of looping
self.rate = rospy.Rate(20.0)
# Information
# Publisher for updating attitude for the speed
# vicon_sum for current speed data
self.attitude_target_speed_pub = rospy.Publisher(
"/px4_quad_controllers/rpy_setpoint", PoseStamped, queue_size=10)
vicon_sub = rospy.Subscriber(
"/intel_aero_quad/odom",
Odometry,
self.vicon_subscriber_callback)
while not rospy.is_shutdown():
if (self.vicon_cb_flag): # If connected and data read in
# Update params if it was changed
# SetPoint Data
self.dx_sp = rospy.get_param(
'/attitude_thrust_publisher/dx_sp')
self.dy_sp = rospy.get_param(
'/attitude_thrust_publisher/dy_sp')
# PID Data
self.dx_P = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_x_P')
self.dx_I = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_x_I')
self.dx_D = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_x_D')
self.dy_P = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_y_P')
self.dy_I = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_y_I')
self.dy_D = rospy.get_param(
'/attitude_thrust_publisher/speed_controller_y_D')
# Update the PID with the speed data from vicon
self.vicon_dx_pid.update(self.vicon_dx)
self.vicon_dy_pid.update(self.vicon_dy)
# Change the PID coefficients if they changed
self.vicon_dx_pid.setKp(self.dx_P)
self.vicon_dx_pid.setKi(self.dx_I)
self.vicon_dx_pid.setKd(self.dx_D)
self.vicon_dy_pid.setKp(self.dy_P)
self.vicon_dy_pid.setKi(self.dy_I)
self.vicon_dy_pid.setKd(self.dy_D)
self.vicon_dy_pid.SetPoint = self.dy_sp
self.vicon_dx_pid.SetPoint = self.dx_sp
vicon_y_output = self.vicon_dy_pid.output
vicon_x_output = -self.vicon_dx_pid.output
target_attitude_speed = PoseStamped()
target_attitude_speed.header.frame_id = "home"
target_attitude_speed.header.stamp = rospy.Time.now()
target_attitude_speed.pose.position.x = vicon_y_output
target_attitude_speed.pose.position.y = vicon_x_output
self.attitude_target_speed_pub.publish(target_attitude_speed)
self.rate.sleep()
stdin=PIPE,
stderr=PIPE)
#light & camera: params = light mode, time on, time off, interval
camera_process = Popen(['python', 'cameraElement.py', '10'],
stdout=PIPE,
stdin=PIPE,
stderr=PIPE)
#create controllers:
#heater: PID Library on temperature
P_temp = 75
I_temp = 0
D_temp = 1
pid_temp = PID.PID(P_temp, I_temp, D_temp)
pid_temp.SetPoint = targetT
pid_temp.setSampleTime(1)
#humidifier: PID library on humidity
P_hum = .5
I_hum = 0
D_hum = 1
pid_hum = PID.PID(P_hum, I_hum, D_hum)
pid_hum.SetPoint = targetH
pid_hum.setSampleTime(1)
#fan: custom proportional and derivative gain on both temperature and humidity
last_temp = 0
last_hum = 0
import math
import PID
parser = argparse.ArgumentParser()
parser.add_argument('--prc_motor', type=int, default=0)
parser.add_argument('--tgt_yaw', type=int, default=-44)
parser.add_argument('--time', type=int, default=1)
args = parser.parse_args()
#print(args)
#print(args.prc)
#sys.exit()
ADJUST_EVERY = 0.05
lc = lcm.LCM()
pid = PID.PID(P=10, I=5.0, D=5.0)
pid.SetPoint = float(args.tgt_yaw) / 180.0 * math.pi
pid.setSampleTime(ADJUST_EVERY)
time_start = time.time()
last = 0
last_adjust = time.time()
last_yaw = 0
def stop():
msg = control_t()
msg.timestamp = time.time()
msg.motor = 0
msg.servo = 0
def test_pid(P, I, D, L=50):
"""Self-test PID class
.. note::
...
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1/i))
if i>9:
pid.SetPoint = 1
time.sleep(0.02)
---
"""
pid = myPID.PID(P, I, D)
pid.SetPoint = 0.0
pid.setSampleTime(0.01)
END = L
feedback = 0
feedback_list = []
time_list = []
setpoint_list = []
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1 / i))
if i > 9:
pid.SetPoint = 1
if i > 15:
pid.SetPoint = 1.25
if i > 25:
pid.SetPoint = 1
time.sleep(0.02)
feedback_list.append(feedback)
setpoint_list.append(pid.SetPoint)
time_list.append(i)
time_sm = np.array(time_list)
time_smooth = np.linspace(time_sm.min(), time_sm.max(), 300)
# feedback_smooth = spline(time_list, feedback_list, time_smooth)
# Using make_interp_spline to create BSpline
helper_x3 = make_interp_spline(time_list, feedback_list)
feedback_smooth = helper_x3(time_smooth)
plt.plot(time_smooth, feedback_smooth)
plt.plot(time_list, setpoint_list)
plt.xlim((0, L))
plt.ylim((min(feedback_list) - 0.5, max(feedback_list) + 0.5))
plt.xlabel('time (s)')
plt.ylabel('PID (PV)')
plt.title('TEST PID')
plt.ylim((1 - 0.5, 1 + 0.5))
plt.grid(True)
plt.show()
def initMain(self):
# Layout settings
QToolTip.setFont(QFont('SansSerif', 10))
self.statusBar().showMessage('Ready')
self.setToolTip('This is a <b>Mesh regulator</b> widget')
self.menu()
self.setGeometry(300, 300, 460, 320)
wid = QWidget(self)
self.setCentralWidget(wid)
self.windowLayout = QHBoxLayout()
wid.setLayout(self.windowLayout)
self.display(self.setval)
self.buttons(self.setval)
self.center()
self.setWindowTitle('GUI')
self.setWindowIcon(QIcon('web.png'))
# PID skjeten
self.P = 2
self.I = 0.01
self.D = 0
self.i = 0
# Use pid library to calculate output
self.pid = PID.PID(self.P, self.I, self.D) # Start PID funksjonen
self.pid.clear()
# Config PID
self.pid.setPoint = self.setval
self.pid.setSampleTime(0.01)
self.pid.setOutputLim(0.01, 100)
self.pid.setWindup(100)
self.WindowSize = 5000
# PID Timer
self.timer = QTimer()
self.timer.setTimerType(Qt.PreciseTimer)
self.timer.timeout.connect(self.on_timer)
self.timer.start(100)
# Process plot
# Init data
self._interval = int(60 * 1000)
self._bufsize = int(60)
self.databuffer = collections.deque([0.0] * self._bufsize,
self._bufsize)
self.temp_list = np.zeros(self._bufsize, dtype=np.float)
self.setpoint_list = [] # Not being used
self.time_list = np.linspace(-60, 0.0, self._bufsize)
self.startime = time.time()
# Pyqtgraph
self.procPlt = qtplt.PlotWidget()
self.procPlt.setEnabled(True)
self.procPlt.setGeometry(QRect(20, 40, 100, 100))
self.windowLayout.addWidget(self.procPlt)
self.procPlt.resize(230, 150)
self.trend = self.procPlt.plot(self.time_list,
self.temp_list,
pen=(255, 0, 0))
# Plot Timer
self.pltTimer = QTimer()
self.pltTimer.timeout.connect(self.updateplot)
self.pltTimer.start(1000)
self.show()
if __name__ == "__main__":
#mtrn = serial.Serial(port='/dev/cu.usbserial-A5047ITL', baudrate=9600,timeout=0)
plotter = Plotter(500, -200, 200)
#sensorimotor = sensor.Sensorimotor()
#sensorimotor.start()
#sensorimotor.cleanbuffer(mtrn)
import PID
import random
random.seed()
P = 1.2
I = 1
D = 0.001
pid = PID.PID(P, I, D)
pid.SetPoint = 45
pid.setSampleTime(0.001)
feedback = 45
output = 0
a = 40
b = 1
while True:
feedback = a + random.uniform(-1, 1)
pid.update(feedback)
output = pid.output
if (True):
plotter.plotdata([output, feedback, 0])
y = 2
tickgreen = 1
twist = Twist()
twist2 = Twist()
twist3 = Twist()
joycontrol = 0
speed = 0
#feedback = 50.0
outputpid = 0.0
mode = 1
stage = 0
mission = 0
kp = 0.01
ki = 0.00
kd = 0.00
pid = PID()
run = 0
brake = 0
emer = 1
class ControlLane():
def __init__(self):
self.sub_lane = rospy.Subscriber('/angle',
Float64,
self.cbFollowLane,
queue_size=10)
self.sub_light = rospy.Subscriber('/light',
Int8,
self.get_light,
queue_size=1)
def followWall(self, numberDatapointsFromMiddle, numberDatapointsFromSide):
"""
Enables the robot to follow the wall.
The distance from the robot side to the wall is measured and compared with the target distance.
A PID controller ensures that this distance can be adhered to with relatively small fluctuation.
If a wall is detected in front of the robot, it will rotate for 90° to the specified direction.
:param numberDatapointsFromMiddle: Number of data points to use from the laser sensor middle.
:param numberDatapointsFromSide: Number of data points to use at the laser sensor side (left/right).
:return: void
"""
# Init PID
P = 0.6
I = 0
D = 1.4
pid = PID.PID(P, I, D)
pid.SetPoint = self.WALLDISTANCE
pid.setSampleTime(0.0)
while (True):
# Circle detection
distanceMargin = 0.6
if (self.checkForCircle(distanceMargin)):
self.repositionRobot()
return
# Calculate the avg distance from the robot side to the wall.
datapointsInGroup = 90
equalsizedDatapointGroups = self.getEqualsizedDatapointGroups(
datapointsInGroup)
if (self.followWallDirection == "left"):
# Use sensor data from the right side of the robot to follow the wall to the left
targetGroupIdx = 0
followWallDirectionAdjustment = 1
elif (self.followWallDirection == "right"):
# Use sensor data from the left side of the robot to follow the wall to the right.
# Also change rotation direction.
targetGroupIdx = len(equalsizedDatapointGroups) - 1
followWallDirectionAdjustment = -1
else:
rospy.logerr("Incorrect followWallDirection parameter.")
return
self.vel.linear.x = self.ROBOTMOVESPEED
# PID control cycle
# Update PID with the minimal distance from the robot side (measured process value).
minDistWallfollowSide = min(
equalsizedDatapointGroups[targetGroupIdx])
pid.update(minDistWallfollowSide)
# Applying the PID output (control variable) as robot rotation.
self.vel.angular.z = pid.output * followWallDirectionAdjustment
# Check if wall is in front of robot
minMiddleDatapoints = self.getMinMiddleDatapoints(
numberDatapointsFromMiddle)
if (minMiddleDatapoints <= self.WALLDISTANCE):
# Turn the robot 90° if wall is in front
self.vel.linear.x = 0
self.velPub.publish(self.vel)
self.alignToWall(numberDatapointsFromSide)
self.velPub.publish(self.vel)
self.rate.sleep()
def main(self, num_samples, set_temperature, start_freq, end_freq, set_voltage, Father):
RE_VAL = set()
# Initialize Relay
REL = relay.Relay_module()
REL.reset()
# set PID parameters
P = 5
I = 0
D = 0
pid = PID.PID(P, I, D)
pid.setSampleTime(1)
self.NEXTION_NUM_SAMPLES = num_samples
self.NEXTION_SET_TEMP = float(format(float(set_temperature) / 0.84, ".2f"))
self.NEXTION_START_FREQ = start_freq
self.NEXTION_END_FREQ = end_freq
self.NEXTION_SET_VOLTAGE = set_voltage
# import oven and multimeter
if APP_OVEN_PRESENT:
OVEN = TTC4006_Tira.TTC4006(TTC_SERIAL_PORT)
OVEN.TTC_ON()
OVEN.TTC_ENABLE_TEMP()
if APP_IMP_PRESENT:
IMP = Impedanz_4192A_Eth.Impedance_Analyser(IMP_IP, IMP_PORT)
IMP.startComm()
# create file and title
CF = data_storage.create_file()
test_mode = "Auto"
equipment_Info = "TTC-4006 + IMP-4192A"
Driver_root = detect_file.File(folder)[1]
start_time = str(datetime.datetime.now())
filename = start_time.replace(" ", "_").replace(".", "-").replace(":", "-")
CF.folder(Driver_root, filename)
t_start = time.time()
t1 = datetime.datetime.now()
t2 = datetime.datetime.now() + datetime.timedelta(seconds=20 * 60)
while True:
if APP_OVEN_PRESENT:
# run oven)
print("01 - Reading data from Oven...")
temp_real = OVEN.TTC_Read_PV_Temp()
temp_set = OVEN.TTC_Read_SP_Temp()
else:
temp_set = format(1.00, "0.2f")
temp_real = format(1.00, "0.2f")
pid.SetPoint = self.NEXTION_SET_TEMP
pid.setKp(float(P))
pid.setKi(float(I))
pid.setKd(float(D))
# read temperature sensor
# Humidity Sensor
# If is not OK => apply non valid temp and humidity
print("02 - Reading data from Humidity Sensor...")
if APP_BME_280_PRESENT:
try:
temperature, pressure, humidity = bme280.readBME280All()
# Medicine
if ((humidity == None) or (temperature == None)):
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
print("02 - Reading data from Humidity Sensor (NONE! - ERROR)...")
elif ((type(humidity) == str) or (type(temperature) == str)):
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
print("02 - Reading data from Humidity Sensor (INVALID STRING! - ERROR)...")
except:
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
print("02 - Reading data from Humidity Sensor (INVALID STRING! - ERROR)...")
else:
print("02 - Reading data from Humidity Sensor (DISABLED)...")
humidity = BME_280_INVALID_HUMI
temperature = BME_280_INVALID_TEMP
HUMI_sensor = format(humidity, "0.2f")
TEMP_sensor = format(temperature, "0.2f")
print('02 - Reading data from Humidity Sensor: Temp(oC): ', TEMP_sensor)
print('02 - Reading data from Humidity Sensor: Humi(%): ', HUMI_sensor)
print("Sensor Temperature : ", str(TEMP_sensor))
pid.update(float(TEMP_sensor))
target_temperature = pid.output
if target_temperature > 130:
target_temperature = 130
elif target_temperature < -40:
target_temperature = -40
else:
target_temperature = target_temperature
print("PID set Temperature : ", str(target_temperature))
print("Chamber real Temperature : ", temp_real)
OVEN.TTC_Set_Temp(target_temperature)
t_step = time.time()
while (t1 > t2):
for i in range(self.NEXTION_NUM_SAMPLES):
# run time
t_run = format(t_step - t_start, "0.2f")
# relay selection
print("03 - Swtich Relay: %d" % i)
REL.RelaySelect(i)
sleep(RELAY_HOLDING_TIME)
# create folder for each sample
current_time = str(datetime.datetime.now())
self.time_str = current_time.replace(" ", "_").replace(".", "-").replace(":", "-")
Father.updateIMPSweep([temp_set, temp_real, TEMP_sensor, HUMI_sensor, t_run, "Measureing", " ", i])
DE = str(DIS.read())
# print (DE)
RE_VAL.add(DE)
# print (RE_VAL)
if "['e\\x0f\\x1b\\x01\\xff\\xff\\xff']" in RE_VAL:
print("Exiting")
OVEN.TTC_OFF()
RE_VAL.clear()
DIS.write("page Device Select\xff\xff\xff")
return
elif "['e\\x0f\\x1c\\x01\\xff\\xff\\xff']" in RE_VAL:
# DIS.write("rest\xff\xff\xff")
RE_VAL.clear()
DIS.write("page restart\xff\xff\xff")
os.system("sudo reboot")
print("07 - Updating Display...")
# creat file
sample_time = str(datetime.datetime.now()).replace(" ", "_").replace(".", "-").replace(":", "-")
name = 'Sample' + str(i)
locals()['v' + str(i)] = i
PA = CF.sample_folder(name)
CF.header_imp(PA, self.time_str, self.time_str, equipment_Info, test_mode,
self.NEXTION_START_FREQ,
self.NEXTION_END_FREQ, self.NEXTION_SET_VOLTAGE)
# run multimeter
print("04- Multimeter DMM196 Reading...")
if APP_IMP_PRESENT:
data = IMP.sweep_measure(self.NEXTION_START_FREQ, self.NEXTION_END_FREQ,
self.NEXTION_SET_VOLTAGE)
CF.content(PA, self.time_str, data)
# relay reset
print("06 - Swtich Relay Unselection: %d" % i)
REL.RelayDeSelect(i)
else:
if APP_NEXTION_PRESENT:
# run time
t_run = format(t_step - t_start, "0.2f")
Father.updateIMPSweep([temp_set, temp_real, TEMP_sensor, HUMI_sensor, t_run, "Waiting", " ", 0])
DE = str(DIS.read())
# print (DE)
RE_VAL.add(DE)
# print (RE_VAL)
if "['e\\x0f\\x1b\\x01\\xff\\xff\\xff']" in RE_VAL:
print("Exiting")
OVEN.TTC_OFF()
RE_VAL.clear()
DIS.write("page Device Select\xff\xff\xff")
return
elif "['e\\x0f\\x1c\\x01\\xff\\xff\\xff']" in RE_VAL:
# DIS.write("rest\xff\xff\xff")
RE_VAL.clear()
DIS.write("page restart\xff\xff\xff")
os.system("sudo reboot")
print("07 - Updating Display...")
t1 = datetime.datetime.now()
def __init__(self):
self.buggy = Buggy()
self.PID = PID(self.buggy.pos, self.buggy.angle)
'target':None,
'start':False,
'data_fresh':False
})
controller = BathController(config.get('Connection', 'Port'),
config.get('Connection', 'Baud'))
time.sleep(3)
print "\rLift off!"
get_temp()
# Start main window thread
window_thread = Process(target = window_main, args = (sys.argv, shared_memory))
window_thread.daemon = True
window_thread.start()
# Initialise PID object
pid = PID.control(shared_memory['p']/10000.0, shared_memory['i']/10000.0, shared_memory['d']/10000.0)
# Whilst the UI is open
while window_thread.is_alive():
# Kick over
get_temp()
#get_temp()
if shared_memory['start'] and shared_memory['data_fresh']:
cycleStart = time.time()
dist = None
target_reached = False
while shared_memory['start'] and window_thread.is_alive():
cycleStart = time.time()
To = shared_memory['target']
Ta = shared_memory['env_temp']
Tb = shared_memory['bath_temp']
m = shared_memory['mass']
def InitUI(self):
self.MainInterface = Interface.Interface(self)
self.MainModel = Robot_Model.Robot_Model(self)
self.OpenCV = OpenCV.OpenCV(self)
self.FeatureDict = {}
self.CurrentFeature = None
self.XPID = PID.PID(0.4, 0.015, 0.15) #PID controller for pan
self.XPID.setPoint(self.OpenCV.CamCentreX)
self.YPID = PID.PID(0.4, 0.015, 0.15) #PID controller for tilt
self.YPID.setPoint(self.OpenCV.CamCentreY)
self.AddFeature = wx.Button(self, label="Add Feature")
self.AddFeature.Bind(wx.EVT_LEFT_DOWN, self.OnAddFeature)
self.RemoveFeature = wx.Button(self, label="Remove Feature")
self.RemoveFeature.Bind(wx.EVT_LEFT_DOWN, self.OnRemoveFeature)
self.FeatName = wx.TextCtrl (self)
self.Save = wx.Button(self, label="Save")
self.Save.Bind(wx.EVT_LEFT_DOWN, self.SaveFeatures)
self.Load = wx.Button(self, label="Load")
self.Load.Bind(wx.EVT_LEFT_DOWN, self.LoadFeatures)
self.FeatureCtrl = wx.ListCtrl(self, size=(-1,100), style=wx.LC_REPORT |wx.BORDER_SUNKEN)
self.FeatureCtrl.InsertColumn(0, 'Name')
self.FeatureCtrl.InsertColumn(1, 'Type')
self.FeatureCtrl.Bind(wx.EVT_LIST_ITEM_SELECTED, self.FeatureChanged)
self.XLabel = wx.StaticText(self, label="X:")
self.XValue = wx.TextCtrl(self, size=(80, -1))
self.YLabel = wx.StaticText(self, label="Y:")
self.YValue = wx.TextCtrl(self, size=(80, -1))
self.ZLabel = wx.StaticText(self, label="Z:")
self.ZValue = wx.TextCtrl(self, size=(80, -1))
self.PanLabel = wx.StaticText(self, label="Pan:")
self.PanValue = wx.TextCtrl(self, size=(80, -1))
self.TiltLabel = wx.StaticText(self, label="Tilt:")
self.TiltValue = wx.TextCtrl(self, size=(80, -1))
Options = ['None','Detect','Track']
self.MatchChoice = wx.Choice(self, choices = Options)
self.MatchChoice.SetSelection(0)
self.hsizer = wx.BoxSizer(wx.HORIZONTAL)
self.hsizer2 = wx.BoxSizer(wx.HORIZONTAL)
self.hsizer3 = wx.BoxSizer(wx.HORIZONTAL)
self.hsizer4 = wx.BoxSizer(wx.HORIZONTAL)
self.hsizer5 = wx.BoxSizer(wx.HORIZONTAL)
self.hsizer6 = wx.BoxSizer(wx.HORIZONTAL)
self.vsizer = wx.BoxSizer(wx.VERTICAL)
self.hsizer.Add(self.MainInterface, wx.ALL, border = 5)
self.hsizer.Add(self.OpenCV, wx.ALL, border = 5)
self.hsizer.Add(self.MainModel, wx.ALL, border = 5)
self.hsizer2.Add(self.AddFeature)
self.hsizer2.Add(self.RemoveFeature)
self.hsizer2.Add(self.FeatName)
self.hsizer3.Add(self.Save)
self.hsizer3.Add(self.Load)
self.hsizer4.Add(self.XLabel)
self.hsizer4.Add(self.XValue)
self.hsizer4.Add(self.YLabel)
self.hsizer4.Add(self.YValue)
self.hsizer4.Add(self.ZLabel)
self.hsizer4.Add(self.ZValue)
self.hsizer5.Add(self.FeatureCtrl)
self.hsizer5.Add(self.MatchChoice)
self.hsizer6.Add(self.PanLabel)
self.hsizer6.Add(self.PanValue)
self.hsizer6.Add(self.TiltLabel)
self.hsizer6.Add(self.TiltValue)
self.vsizer.Add(self.hsizer)
self.vsizer.Add(self.hsizer2)
self.vsizer.Add(self.hsizer3)
self.vsizer.Add(self.hsizer5)
self.vsizer.Add(self.hsizer4)
self.vsizer.Add(self.hsizer6)
self.SetSizer(self.vsizer)
self.vsizer.SetSizeHints(self)
self.SetTitle('Big Face Robotics - Robot Head Controller')
self.Centre()
self.timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.OnTimer)
self.timer.Start(1000/20) # timer interval
import PID import pylab from pylab import * s=0 v=0 Kp=-1.0/100 Kd=-20.0/100 Ki=0 Kf=0.1 target = 100 dt=0.1 samples=2000 yLabel = "Position [m], Velocity [m/s]" xLabel = "Time [s]" PID.solve(yLabel, xLabel, s, v, Kp, Kd, Ki, Kf, target, dt, samples) pylab.ylim(0, 120) show()
#!/usr/bin/python
from PID import *
import matplotlib.pyplot as plt
import numpy as np
pid_controller = PID(10.0, 0.0, 1.0, 1.0)
set_position = 1.0
actual_position = []
actual_velocity = []
force = []
clock = []
sample_num = 500
dt = 0.02
pid_controller.SetPoint = set_position
# Intentionally sleep for 2 seconds to test whether we handle the initial time
# correctly in I and D term.
time.sleep(2.0)
for i in range(sample_num):
t = time.time()
position = 0.0 if i == 0 else actual_position[i - 1]
pid_controller.update(position)
f = pid_controller.output
velocity = 0.0 if i == 0 else actual_velocity[i - 1]
velocity += f * dt
position += velocity * dt
# Update.
