def __init__(self, log=None):
#Setup Logging
if log != None:
self.log = log
else:
self.log = logging.getLogger()
#Check for vital pieces
try:
self.log.info("Checking for 1-Wire Thermometer")
self.ambient_therm = W1ThermSensor()
#self.meat_therm = W1ThermSensor()
except w1thermsensor.NoThermFound:
self.log.error("Could not find any thermometers")
#Set simulator flag (No fan, no temp sensor)
self.simulator = False
self.status_lock = threading.Lock()
#Check for connection to internet
noInternet = True
if noInternet == True:
self.local_status = True
#Set Alg. Variables
self.temp_loop_time = 2.0
self.p_gain = 6
self.i_gain = 0.02
self.d_gain = 0.0
self.pid = PID(self.p_gain, self.i_gain, self.d_gain)
self.fan = PWMFanController(self.log)
#Setup Default Values
self.enable_logging = False
self.enable_pid = False
self.status = {}
self.target_ambient_temp = 235
self.max_duty_cycle = 100
self.min_duty_cycle = 25
self.max_ambient_temp = 265
self.min_ambient_temp = 205
self.run_id = None
self.pid.SetPoint = self.target_ambient_temp
#Setup External Values
self.value_lock = threading.Lock()
#Setup Status Thread
self.status_thread = None
self.status_sleep_time = 1
#Setup PID Controll Thread
self.pid_thread = None
self.pid_sleep_time = 1
self.log.info("Initialization complete")
def __init__(self):
# initilize controllers
self.position_controllers = []
self.ts = rospy.get_param('ts')
args = rospy.get_param("/position_controllers")
self.joint_01_controller = PID(args["joint_01"], self.ts)
self.joint_02_controller = PID(args["joint_02"], self.ts)
self.joint_03_controller = PID(args["joint_03"], self.ts)
self.prismatic_controller = PID(args["prismatic"], self.ts)
self.position_controllers.append(self.joint_01_controller)
self.position_controllers.append(self.joint_02_controller)
self.position_controllers.append(self.joint_03_controller)
self.position_controllers.append(self.prismatic_controller)
self.reset_controller = True
self.pause_sim = True
self.set_points = [0, 0, 0, 0]
# service for applying joint efforts to gazebo
self.torque_srv = rospy.ServiceProxy("gazebo/apply_joint_effort",
ApplyJointEffort)
# create a subscriber to subscribe the set_points from the envirnment
rospy.Subscriber('/position_controllers/command', Float64MultiArray,
self.joint_targets_callback)
# subscribe the joint states
rospy.Subscriber('/joint_states', JointState,
self.joints_state_callback)
# param for reset the controllers
rospy.Subscriber('/reset_controller', Bool,
self.reset_controller_callback)
# param for stop publishing efforts
rospy.Subscriber('/pause_sim', Bool, self.pause_sim_callback)
self.joints_name = ['joint_01', 'joint_02', 'joint_03', 'prismatic']
def __init__(self, distance):
"""
Inicialização dos drivers, parâmetros do controle PID e decolagem do drone.
"""
self.distance = distance
self.pid_foward = PID(distance, 0.01, 0.0001, 0.01, 500, -500, 0.7,
-0.7)
self.pid_yaw = PID(0, 0.33, 0.0, 0.33, 500, -500, 100, -100)
self.pid_angle = PID(0.0, 0.01, 0.0, 0.01, 500, -500, 0.3, -0.3)
self.pid_height = PID(0.0, 0.002, 0.0002, 0.002, 500, -500, 0.3, -0.2)
cflib.crtp.init_drivers(enable_debug_driver=False)
self.factory = CachedCfFactory(rw_cache='./cache')
self.URI4 = 'radio://0/40/2M/E7E7E7E7E4'
self.cf = Crazyflie(rw_cache='./cache')
self.sync = SyncCrazyflie(self.URI4, cf=self.cf)
self.sync.open_link()
self.mc = MotionCommander(self.sync)
self.cont = 0
self.notFoundCount = 0
self.calculator = DistanceCalculator()
self.cap = cv2.VideoCapture(1)
self.mc.take_off()
time.sleep(1)
def __init__(self,pid = None):
self.closed = False
self.pid = PID(1.2,1,0.001)
if pid is not None:
self.pid = pid
#how many inputs are in the graph at a time
self.x_interval = 30
self.output = 0
self.x = []
self.y = []
self.start = 0
self.line = None
self.root = Tk.Tk()
self.root.title("Brought to you by Your Mom and co.")
fig = plt.Figure()
#bunch of labels & their position on the grid. play around to figure it out
self.error_label = Tk.Label(self.root,text= "Your Mom")
self.error_label.grid(column = 5, row = 0)
p_label = Tk.Label(self.root,text= "P Constant").grid(column = 0, row = 0)
i_label = Tk.Label(self.root,text= "I Constant").grid(column = 1, row = 0)
d_label = Tk.Label(self.root,text= "D Constant").grid(column = 2, row = 0)
pid_label = Tk.Label(self.root,text= "PID Setpoint").grid(column = 3, row = 0)
#we only care about the text in the box. All other elements work on their own with Tkinter
self.p_constant = Tk.Text(self.root,height = 2, width = 10, bg = "light yellow")
self.p_constant.grid(column = 0, row = 1)
self.i_constant = Tk.Text(self.root,height = 2, width = 10, bg = "light yellow")
self.i_constant.grid(column = 1, row = 1)
self.d_constant = Tk.Text(self.root,height = 2, width = 10, bg = "light yellow")
self.d_constant.grid(column = 2, row = 1)
self.sp = Tk.Text(self.root,height = 2, width = 10, bg = "light yellow")
self.sp.grid(column = 3, row = 1)
changePID = Tk.Button(self.root,text = "Change PID value", command = self.change_PID).grid(column = 1, row = 2)
self.canvas = FigureCanvasTkAgg(fig,master = self.root)
self.canvas.get_tk_widget().grid(column = 4, row = 3)
#create a plot and put it into matplot's figure. 111 divides into 1 row & 1 column of plot
#refers to the online doc. But yeah, we only need 1 plot.
ax = fig.add_subplot(111)
#set x and y axis. This can be put into variable in future sprint.
ax.set_ylim([-180,180])
ax.set_xlim([0,self.x_interval])
#matplot automatically draw the line from a list of x and y values. line need to be redraw again and again
self.line, = ax.plot(self.x, self.y)
def on_closing():
self.closed = True
self.root.destroy()
self.root.protocol("WM_DELETE_WINDOW", on_closing)
def run(self):
global prediction
# establish connection with TORCS server
C = snakeoil.Client()
# load PID controller and set vehicle speed
pid = PID(1.0, 0.1, 0.1)
pid.setPoint(15.5)
try:
while True:
C.get_servers_input()
R = C.R.d
S = C.S.d
R['steer'] = prediction
R['accel'] = pid.update(S['speedX'])
R['accel'] = np.clip(R['accel'], 0, 0.1)
snakeoil.drive_example(C)
C.respond_to_server()
C.shutdown()
except KeyboardInterrupt:
print('\nShutdown requested. Exiting...')
def main():
global pid
global pwm_A1
global pwm_A2
global pwm_B1
global pwm_B2
# setup GPIO pins and motors
FREQ = 100
A1_PIN = 13
A2_PIN = 19
B1_PIN = 12
B2_PIN = 18
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(A1_PIN, GPIO.OUT)
GPIO.setup(A2_PIN, GPIO.OUT)
GPIO.setup(B1_PIN, GPIO.OUT)
GPIO.setup(B2_PIN, GPIO.OUT)
pwm_A1 = GPIO.PWM(A1_PIN, FREQ)
pwm_A2 = GPIO.PWM(A2_PIN, FREQ)
pwm_B1 = GPIO.PWM(B1_PIN, FREQ)
pwm_B2 = GPIO.PWM(B2_PIN, FREQ)
pwm_A1.start(0)
pwm_A2.start(0)
pwm_B1.start(0)
pwm_B2.start(0)
# initialize PID controller
pid = PID(0, 100, K_P, K_I, K_D)
# initialize ROS node and listen for commands
rospy.init_node("motor_ctrl")
rospy.Subscriber("motor_cmd", MotorCmd, handle_motor_cmd)
rospy.spin()
def __init__(self):
self.ST = True # software testing
self.CODE = 'Team1' if self.ST else 'Team614'
self.NAME = 'Stardust'
self.pid = PID(0.05, 0.0001, 1.5)
self.counter = 0 # counter for reducing the number of processed image
# x = ay + b
self.left_a, self.left_b = [], []
self.right_a, self.right_b = [], []
self.subscriber = rospy.Subscriber("/{}_image/compressed".format(
self.CODE),
CompressedImage,
self.callback,
queue_size=1)
self.steer_angle = rospy.Publisher('{}_steerAngle'.format(self.CODE),
Float32,
queue_size=1)
self.speed_pub = rospy.Publisher('{}_speed'.format(self.CODE),
Float32,
queue_size=1)
policy = TD3.TD3(state_dim, action_dim, max_action)
replay_buffer = utils.ReplayBuffer(args.replay_size)
replay_buffer2 = utils.ReplayBuffer2(args.replay_size2)
total_steps = 0
episode_num = 0
master = mavutil.mavlink_connection('udp:0.0.0.0:14550')
master.wait_heartbeat()
# file names
reward_file = 'reward_blue.txt'
data_file = 'data.txt'
save_data(data_file, 'Depth', 'Roll', 'Pitch', ['F1', 'F2', 'F3', 'F4'])
state_exp = np.array([2.0, 0.000, 0.000])
depth_hold = PID(65, 0.9, 0.01)
pitch_hold = PID(10, 0.9, 0.01)
roll_hold = PID(10, 0.9, 0.01)
arm()
# change_mode('STABILIZE')
for episode_idx in range(args.max_episode):
arm()
episode_reward = 0
counter = 0
# dep_init = np.random.uniform(0.5, 4.0)
# r_init = np.random.uniform(-0.1, 0.1)
# p_init = np.random.uniform(-0.1, 0.1)
# pid_for_init(dep_init, p_init, r_init)
# arm()
# print('1')
# change_mode('ALT_HOLD')
from pid_controller import PID
# NOTE, anything involving "pid_simulator" has been commented out because it is not 100% functional yet
# If the simulator never ends up working, we can just remove those lines of code
#from pid_simulator import PID_simulator
# TODO: Tune P, I, D constants
#Global Variables
G_KP = 0.5
G_KI = 0.0
G_KD = 0.0
G_WINDUP = 360.0
pub = rospy.Publisher('server/send_drive_vec', Drive_Vector, queue_size=10)
controller = PID(G_KP, G_KI, G_KD)
# simulator = PID_simulator()
# simulator = PID_simulator(controller)
"""
Callback function executed every time a new Orientation_Vector is received on orient_vector
"""
def run_pid(data):
global G_KP, G_KD, G_WINDUP
# global simulator
global pub
angle_error = PID_error(data)
# offset = simulator.update_feedback(angle_error) Commented out because mttkinter from pid_simulator cannot be pip install on the nuc
def __init__(self, pid_x, pid_y, pid_z, pid_theta, bounding_box=True):
'''
@param: pid_x is a tuple such that (kp, ki, kd, integrator, derivator,
set_point)
@param: pid_y is a tuple such that (kp, ki, kd, integrator, derivator,
set_point)
@param: pid_z is a tuple such that (kp, ki, kd, integrator, derivator,
set_point)
@param: pid_theta is a tuple such that (kp, ki, kd, integrator,
derivator, set_point)
@param: bounding_box is a boolean that will initially turn the bounding
box on (True) or off (False). Default is True
'''
self.pid_x = PID()
self.pid_y = PID()
self.pid_z = PID()
self.pid_theta = PID()
# Set gains, integrators, derivators, and set points
self.pid_x.setKp(pid_x[0])
self.pid_x.setKi(pid_x[1])
self.pid_x.setKd(pid_x[2])
self.pid_x.setIntegrator(pid_x[3])
self.pid_x.setDerivator(pid_x[4])
self.pid_x.setPoint(pid_x[5])
self.pid_y.setKp(pid_y[0])
self.pid_y.setKi(pid_y[1])
self.pid_y.setKd(pid_y[2])
self.pid_y.setIntegrator(pid_y[3])
self.pid_y.setDerivator(pid_y[4])
self.pid_y.setPoint(pid_y[5])
self.pid_z.setKp(pid_z[0])
self.pid_z.setKi(pid_z[1])
self.pid_z.setKd(pid_z[2])
self.pid_z.setIntegrator(pid_z[3])
self.pid_z.setDerivator(pid_z[4])
self.pid_z.setPoint(pid_z[5])
self.pid_theta.setKp(pid_theta[0])
self.pid_theta.setKi(pid_theta[1])
self.pid_theta.setKd(pid_theta[2])
self.pid_theta.setIntegrator(pid_theta[3])
self.pid_theta.setDerivator(pid_theta[4])
self.pid_theta.setPoint(pid_theta[5])
# Should we use the bounding box?
self.use_bounding_box = bounding_box
def update(self, values):
'''
This updates each controller and returns the updated values as a
tuple
@param: values is a tuple with the current value for each degree of
freedom
'''
x_update = self.pid_x.update(values[0])
y_update = self.pid_y.update(values[0])
z_update = self.pid_z.update(values[0])
theta_update = self.pid_theta.update(values[0])
return (x_update, y_update, z_update, theta_update)
nhf_array[time_step] = nhf
error_array[
time_step] = surface_temperature_setpoint - surface_temperature
timeChange = t - last_time
if timeChange == 0:
timeChange = 1e-6
# call the PID only once every time lag
if time_lag_counter < number_timereadings[alpha_number]:
time_lag_counter += 1
else:
# call PID
new_q, new_error, error_sum = PID(
surface_temperature, surface_temperature_setpoint,
last_error, last_surface_temperature, error_sum,
timeChange, kp[alpha_number], ki[alpha_number],
kd[alpha_number])
# update parameters
q_arrays[alpha_number][time_step + 1:time_step +
maximum_limit_tgrid] = new_q
# ---- PID debugging
# print("\n")
# print("PID called")
# print(f"Scenario: {scenario}")
# print(f"alpha: {alphas[alpha_number]}")
# print(f"Time step: {time_step+1}/{len(t_grids[alpha_number])}")
# print(f"Time: {t}")
# print(f"Set surface temperature: {surface_temperature_setpoint}")
import urllib.request as urllib from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm from cflib.crazyflie.log import LogConfig cflib.crtp.init_drivers(enable_debug_driver=False) factory = CachedCfFactory(rw_cache='./cache') URI4 = 'radio://0/40/2M/E7E7E7E7E4' cf = Crazyflie(rw_cache='./cache') sync = SyncCrazyflie(URI4, cf=cf) sync.open_link() pid_foward = PID(40, 0.01, 0.0001, 0.01, 500, -500, 0.7, -0.7) pid_yaw = PID(0, 0.33, 0.0, 0.33, 500, -500, 100, -100) pid_angle = PID(0.0, 0.01, 0.0, 0.01, 500, -500, 0.3, -0.3) pid_height = PID(0.0, 0.002, 0.0002, 0.002, 500, -500, 0.3, -0.2) cont = 0 notFoundCount = 0 velocity_x = 0 velocity_y = 0 velocity_z = 0 horizontal_distance = 0 alpha = 0 calculator = DistanceCalculator() cap = cv2.VideoCapture(1) mc = MotionCommander(sync) mc.take_off() time.sleep(1)
surface_temperature = T[0]
nhf_array[time_step] = nhf
error_array[time_step] = nhf_setpoint - nhf
timeChange = t - last_time
if timeChange == 0:
timeChange = 1e-6
# call the PID only once every time lag
if time_lag_counter < number_timereadings[alpha_number]:
time_lag_counter += 1
else:
# call PID
new_q, new_error, error_sum = PID(nhf, nhf_setpoint,
last_error, last_nhf,
error_sum, timeChange,
kp[alpha_number],
ki[alpha_number],
kd[alpha_number])
# update parameters
q_arrays[alpha_number][time_step + 1:time_step +
maximum_limit_tgrid] = new_q
last_nhf = nhf
last_error = new_error
last_time = t
# ---- PID debugging
# print("\n")
# print("PID called")
# print(f"Scenario: {scenario}")
# print(f"alpha: {alphas[alpha_number]}")
