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,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 __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 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 __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)
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 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()
class BBQController(object):
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 start(self):
if self.status_thread:
self.log.warn("Status thread already started; skipping")
else:
self.status_enable = True
self.status_thread = threading.Thread(target=self.status_logger)
self.status_thread.daemon = True
self.status_thread.start()
if self.pid_thread:
self.log.warn(
"Temperature and PID thread already started; skipping")
else:
self.pid_enable = True
self.pid_thread = threading.Thread(target=self.run_pid)
self.pid_thread.daemon = True
self.pid_thread.start()
def stop(self):
if not self.status_thread:
self.log.warn("Status thread already stopped; skipping")
else:
self.status_enable = False
self.status_thread.join(2)
self.status_thread = None
if not self.pid_thread:
self.log.warn("Temp and PID thread already stopped; skipping")
else:
self.pid_enable = False
self.pid_thread.join(2)
self.pid_thread = None
def set_target_ambient_temp(self, val):
with self.value_lock.acquire():
self.target_ambient_temp = val
self.pid.SetPoint = val
def convertPIDOutput(self, x):
if x < 0:
return 0
elif x < self.min_duty_cycle:
return 0
elif x > self.max_duty_cycle:
return self.max_duty_cycle
else:
return int(round(x))
def log_data(self):
self.log.info(
"Current Temp: %f F Target Temp: %f Tach Speed: %f" %
(self.status["ambient_temp"], self.status["target_ambient_temp"],
self.status["fan_speed"]))
def run_pid(self):
while self.pid_enable:
try:
self.status_lock.acquire()
self.pid.update(self.status["ambient_temp"])
self.fan.setDutyCycle(self.convertPIDOutput(self.pid.output))
finally:
self.status_lock.release()
time.sleep(self.pid_sleep_time)
def update_status(self):
try:
self.status_lock.acquire()
self.status = {
"timestamp":
time.time(),
"ambient_temp":
self.ambient_therm.get_temperature(W1ThermSensor.DEGREES_F),
"meat_temp":
0.0,
"fan_duty_cycle":
self.fan.getDutyCycle(),
"max_ambient_temp":
self.max_ambient_temp,
"min_ambient_temp":
self.min_ambient_temp,
"target_ambient_temp":
self.target_ambient_temp,
"fan_speed":
self.get_tach()
}
finally:
self.status_lock.release()
def status_logger(self):
while self.status_enable:
self.update_status()
try:
self.status_lock.acquire()
if self.enable_logging:
self.log_data()
finally:
self.status_lock.release()
time.sleep(self.status_sleep_time)
def get_tach(self):
#TODO Implement Later
return 0
def get_ambient_temperature(self):
return self.ambient_therm.get_temperature(W1ThermSensor.DEGREES_F)
class PID_simulator:
#PID simulator with tkinter & matplotlib. Code might have been taken here and there from stackoverflow & github
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)
#threading bad =(
#set an animation with delay of 500 mili.
#ani = FuncAnimation(fig,
# self.func_animate,interval=0, blit=False)
#Tk.mainloop()
#callback function for the button. Triggered when button pressed.
#Every element is changed when button pressed. Bunch of try except in case of dumb/blank input
def change_PID(self):
pconst = self.p_constant.get("1.0",'end-1c')
try:
p_c = float(pconst)
self.pid.setKp(p_c)
except:
pass
iconst = self.i_constant.get("1.0",'end-1c')
try:
i_c = float(iconst)
self.pid.setKi = i_c
except:
pass
dconst = self.d_constant.get("1.0",'end-1c')
try:
d_c = float(dconst)
pid.setKd = d_c
except:
pass
setpoint = self.sp.get("1.0",'end-1c')
try:
s_p = float(setpoint)
self.pid.SetPoint = s_p
except:
pass
#callback function for the FuncAnimation. I have no idea what i does (interval maybe)
def func_animate(self,feedback):
if(len(self.y)<self.x_interval):
self.x.append(self.start)
self.start += 1
self.output = self.pid.update(feedback)
#uncomment this line when start getting feedback from actual env
#self.feedback += self.output
self.y.append(feedback)
else:
self.x = []
self.y = []
self.start = 0
self.x.append(0)
self.y.append(feedback)
if not self.closed:
#self.error_label.config(text = '%.3g'%(feedback-self.pid.SetPoint))
self.line.set_data(self.x, self.y)
self.canvas.draw()
self.root.update()
#update feedback here
def get_output(self):
return self.output
def update_feedback(self,feedback):
self.func_animate(feedback)
return self.output
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)
class Lane_Detect:
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)
def get_hist(self, img):
hist = np.sum(img, axis=0)
return hist
def binary_HSV(self, img):
minThreshold = (0, 0, 180)
maxThreshold = (179, 30, 255)
hsv_img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
out = cv2.inRange(hsv_img, minThreshold, maxThreshold)
return out
def shadow_HSV(self, img):
minShadowTh = (30, 43, 36)
maxShadowTh = (120, 81, 171)
minLaneInShadow = (90, 43, 97)
maxLaneInShadow = (120, 80, 171)
imgHSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
shadowMaskTh = cv2.inRange(imgHSV, minShadowTh, maxShadowTh)
shadow = cv2.bitwise_and(img, img, mask=shadowMaskTh)
shadowHSV = cv2.cvtColor(shadow, cv2.COLOR_BGR2HSV)
out = cv2.inRange(shadowHSV, minLaneInShadow, maxLaneInShadow)
return out
def get_bird_view(self, img, shrink_ratio, dsize=(480, 320)):
height, width = img.shape[:2]
SKYLINE = int(height * 0.55)
roi = img.copy()
cv2.rectangle(roi, (0, 0), (width, SKYLINE), 0, -1)
dst_width, dst_height = dsize
src_pts = np.float32([[0, SKYLINE], [width, SKYLINE], [0, height],
[width, height]])
dst_pts = np.float32([[0, 0], [dst_width, 0],
[dst_width * shrink_ratio, dst_height],
[dst_width * (1 - shrink_ratio), dst_height]])
M = cv2.getPerspectiveTransform(src_pts, dst_pts)
dst = cv2.warpPerspective(roi, M, dsize)
return dst
def inv_bird_view(self, img, stretch_ratio, dsize=(320, 240)):
height, width = img.shape[:2]
dst_width, dst_height = dsize
SKYLINE = int(dst_height * 0.55)
src_pts = np.float32([[0, 0], [width, 0],
[width * stretch_ratio, height],
[width * (1 - stretch_ratio), height]])
dst_pts = np.float32([[0, SKYLINE], [dst_width, SKYLINE],
[0, dst_height], [dst_width, dst_height]])
M = cv2.getPerspectiveTransform(src_pts, dst_pts)
dst = cv2.warpPerspective(img, M, dsize)
return dst
def sliding_window(self,
img,
nwindows=16,
margin=20,
minpix=1,
draw_windows=True,
left_color=(0, 0, 255),
right_color=(0, 255, 0),
thickness=1):
# global left_a, left_b, left_c, right_a, right_b, right_c
left_fit_ = np.empty(2)
right_fit_ = np.empty(2)
# I haven't understood this line of code
out_img = np.dstack((img, img, img)) * 255
s_hist = self.get_hist(img)
# find peaks of left and right halves
midpoint = int(s_hist.shape[0] / 2)
leftx_base = np.argmax(s_hist[:midpoint])
rightx_base = np.argmax(s_hist[midpoint:]) + midpoint
# set the height of windows
window_height = np.int(img.shape[0] / nwindows)
# identify the x and y positions of all nonzero pixels in the image
nonzero = img.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# current positions to be updated for each window
leftx_current = leftx_base
rightx_current = rightx_base
# create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# step through the windows one by one
for window in range(nwindows):
# identify window boundaries in x and y (and right and left)
win_y_low = img.shape[0] - (window + 1) * window_height
win_y_high = img.shape[0] - window * window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
# draw the windows on the visualization image
if draw_windows == True:
cv2.rectangle(out_img, (win_xleft_low, win_y_low),
(win_xleft_high, win_y_high), left_color,
thickness)
cv2.rectangle(out_img, (win_xright_low, win_y_low),
(win_xright_high, win_y_high), right_color,
thickness)
# identify the nonzero pixels in x and y within the window
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high)
& (nonzerox >= win_xleft_low) &
(nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) &
(nonzeroy < win_y_high) &
(nonzerox >= win_xright_low) &
(nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# if you found > minpix pixels, recenter next window on their mean position
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# concatenate the arrays of indices ???
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
# extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
if len(leftx) == 0 or len(lefty) == 0:
left_fit = np.array([0, 0])
else:
left_fit = np.polyfit(lefty, leftx, 1)
if len(rightx) == 0 or len(righty) == 0:
right_fit = np.array([0, 0])
else:
right_fit = np.polyfit(righty, rightx, 1)
self.left_a.append(left_fit[0])
self.left_b.append(left_fit[1])
self.right_a.append(right_fit[0])
self.right_b.append(right_fit[1])
left_fit_[0] = np.mean(self.left_a[-10:]) # ???
left_fit_[1] = np.mean(self.left_b[-10:]) # ???
right_fit_[0] = np.mean(self.right_a[-10:])
right_fit_[1] = np.mean(self.right_b[-10:])
# generate x and y values for plotting (x = ay + b => y = (x - b) / a)
ploty = np.linspace(0, img.shape[0] - 1, img.shape[0])
left_fitx = left_fit_[0] * ploty + left_fit_[1]
right_fitx = right_fit_[0] * ploty + right_fit_[1]
return out_img, (left_fitx, right_fitx), (left_fit_, right_fit_), ploty
def draw_lane(self,
src_img,
plot_dsize,
leftx_pts,
rightx_pts,
ploty,
color=(255, 0, 0)):
stretch_ratio = 0.3
color_image = np.zeros((plot_dsize[0], plot_dsize[1], 3))
left = np.array(
[np.flipud(np.transpose(np.vstack([leftx_pts, ploty])))])
right = np.array([np.transpose(np.vstack([rightx_pts, ploty]))])
points = np.hstack((left, right))
cv2.fillPoly(color_image, np.int_(points), color)
inv = self.inv_bird_view(color_image, stretch_ratio)
retval = cv2.addWeighted(src_img.astype(np.uint8), 1,
inv.astype(np.uint8), 1, 1)
return retval
def get_desired_line(self, leftx, rightx, ys):
xs = np.mean(np.vstack((leftx, rightx)), axis=0)
line = np.polyfit(ys, xs, 1)
return line, xs, ys
def radian_to_degree(self, x):
return (x / math.pi) * 180
def pipeline(self, img):
ratio = 0.3 # shrink the bottom by 30%
bird_view = self.get_bird_view(img, ratio)
none_shadow_mask = self.binary_HSV(bird_view)
shadow_mask = self.shadow_HSV(bird_view)
lane_mask = cv2.bitwise_or(none_shadow_mask, shadow_mask)
s_window, (left_fitx, right_fitx), (
left_fit_, right_fit_), ploty = self.sliding_window(lane_mask)
d_line, d_xs, d_ys = self.get_desired_line(left_fitx, right_fitx,
ploty)
lines_plot_img = np.zeros_like(s_window, dtype=np.uint8)
cv2.line(lines_plot_img, (240, 0), (240, 320),
color=(0, 0, 255),
thickness=1)
cv2.line(lines_plot_img, (int(d_xs[0]), 0),
(int(d_xs[len(d_xs) - 1]), 320),
color=(0, 255, 0),
thickness=1)
draw_lane = self.draw_lane(img, bird_view.shape[:2], left_fitx,
right_fitx, ploty)
cv2.imshow('frame', img)
cv2.imshow('sliding windows', s_window)
cv2.imshow('lines', lines_plot_img)
cv2.imshow('draw_lane', draw_lane)
cte = math.atan(d_line[0])
self.pid.update_error(cte)
# steer_delta = self.pid.total_error()
steer_delta = self.radian_to_degree(self.pid.total_error())
print('steer_delta:', steer_delta)
self.steer_angle.publish(steer_delta)
self.speed_pub.publish(50)
cv2.waitKey(2)
def callback(self, ros_data):
self.counter += 1
if self.counter % 2 == 0: # reduce half of images for processing
self.counter = 0
return
# decode image
np_arr = np.fromstring(ros_data.data, np.uint8)
img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0
self.pipeline(img)
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)
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):
# Stores the most recent navdata
self.recentNavdata = None
# Subscribe to the /ardrone/navdata topic, so we get updated whenever new information arrives from the drone
self.subscribeNavdata = rospy.Subscriber("ardrone/navdata", Navdata, self.__ReceiveNavdata)
# Allow us to publish to the steering topic
self.publishSteering = rospy.Publisher("ardrone/steering_commands", matrix33)
# Store the constants
self.constants = LandingConstants()
# The flag which we set via ctrl + c, when we want to stop the current loop
self.stopTask = False
# Attach the keyboard listener
signal.signal(signal.SIGINT, self.__SignalHandler)
# Create a timer, which is used to wait for the drone to execute the command, until the command to stop is sent
self.r = rospy.Rate(self.constants.COMMAND_PUBLISH_RATE)
# Create controllers, which weigh the steering commands according to different variables
self.findTagController = PID(1.0, 0.3, 0.5)
self.findTagController.setPoint(0.0)
# self.centerFrontController = PID(0.8, 0.01, 1.5)
# The one below worked pretty good
# self.centerFrontController = PID(0.8, 0.01, 0.5)
self.centerFrontController = PID(2.0, 0.0, 0.0)
self.centerFrontController.setPoint(0)
self.approachFrontController = PID(1.5, 2.0, 2.0)
self.approachFrontController.setPoint(0)
# self.centerBottomXController = PID(0.8, 0.5, 3.0)
# self.centerBottomXController = PID(0.8, 0.5, 1.0)
self.centerBottomXController = PID(0.8, 0.2, 0.6)
self.centerBottomXController.setPoint(0)
# self.centerBottomYController = PID(0.8, 0.5, 3.0)
# self.centerBottomYController = PID(0.8, 0.5, 1.0)
self.centerBottomYController = PID(0.8, 0.2, 0.6)
self.centerBottomYController.setPoint(0)
self.alignBottomController = PID()
self.alignBottomController.setPoint(0)
# Stores if we centered the tag once already (In that case, we can assume we are on a direct path
# towards the tag and can now move laterally instead of turning in one place)
self.wasTagCentered = False
# A flag to stop the loop for when we are done
self.success = False
# Some counters to evaluate how we are doing
self.searchCounter = 0
self.rotateCounter = 0
self.approachCounter = 0
self.centerCounter = 0
self.bottomCounter = 0
class LandingController(object):
def __init__(self):
# Stores the most recent navdata
self.recentNavdata = None
# Subscribe to the /ardrone/navdata topic, so we get updated whenever new information arrives from the drone
self.subscribeNavdata = rospy.Subscriber("ardrone/navdata", Navdata, self.__ReceiveNavdata)
# Allow us to publish to the steering topic
self.publishSteering = rospy.Publisher("ardrone/steering_commands", matrix33)
# Store the constants
self.constants = LandingConstants()
# The flag which we set via ctrl + c, when we want to stop the current loop
self.stopTask = False
# Attach the keyboard listener
signal.signal(signal.SIGINT, self.__SignalHandler)
# Create a timer, which is used to wait for the drone to execute the command, until the command to stop is sent
self.r = rospy.Rate(self.constants.COMMAND_PUBLISH_RATE)
# Create controllers, which weigh the steering commands according to different variables
self.findTagController = PID(1.0, 0.3, 0.5)
self.findTagController.setPoint(0.0)
# self.centerFrontController = PID(0.8, 0.01, 1.5)
# The one below worked pretty good
# self.centerFrontController = PID(0.8, 0.01, 0.5)
self.centerFrontController = PID(2.0, 0.0, 0.0)
self.centerFrontController.setPoint(0)
self.approachFrontController = PID(1.5, 2.0, 2.0)
self.approachFrontController.setPoint(0)
# self.centerBottomXController = PID(0.8, 0.5, 3.0)
# self.centerBottomXController = PID(0.8, 0.5, 1.0)
self.centerBottomXController = PID(0.8, 0.2, 0.6)
self.centerBottomXController.setPoint(0)
# self.centerBottomYController = PID(0.8, 0.5, 3.0)
# self.centerBottomYController = PID(0.8, 0.5, 1.0)
self.centerBottomYController = PID(0.8, 0.2, 0.6)
self.centerBottomYController.setPoint(0)
self.alignBottomController = PID()
self.alignBottomController.setPoint(0)
# Stores if we centered the tag once already (In that case, we can assume we are on a direct path
# towards the tag and can now move laterally instead of turning in one place)
self.wasTagCentered = False
# A flag to stop the loop for when we are done
self.success = False
# Some counters to evaluate how we are doing
self.searchCounter = 0
self.rotateCounter = 0
self.approachCounter = 0
self.centerCounter = 0
self.bottomCounter = 0
def __SignalHandler(self, frame, placeholderArgument):
"""
Sets the stopTask flag, so our loops stop when interrupted via keyboard
"""
self.stopTask = True
def __ReceiveNavdata(self, navdata):
"""
Stores the incoming navdata
"""
self.recentNavdata = navdata
def BringMeHome(self):
"""
Core function to land the drone.
It will turn the drone till it finds a 3-colored tag witht the front camera, approach this tag until
it sees a A4-tag on the bottom and will land the drone on this tag
"""
# This is the big loop for all the functions. It resets all flags and periodically asks for instructions to be sent to the drone.
if self.recentNavdata == None:
print "No navdata available, exiting"
return
# Reset flags
self.stopTask = False
self.wasTagCentered = False
self.success = False
# Reset the controllers
self.findTagController.setPoint(0)
self.centerFrontController.setPoint(0)
self.approachFrontController.setPoint(0)
self.centerBottomXController.setPoint(0)
self.centerBottomYController.setPoint(0)
self.alignBottomController.setPoint(0)
# Some counters to evaluate how we are doing
self.searchCounter = 0
self.rotateCounter = 0
self.approachCounter = 0
self.centerCounter = 0
self.bottomCounter = 0
# Get the drone up to a good height:
workingNavdata = self.recentNavdata
while workingNavdata.altd < 1450:
if self.stopTask:
# In case of keyboard interruption
break
workingNavdata = self.recentNavdata
self.ExecuteCommand(matrix33(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0))
print "Gaining Altitude"
# Now we are at a proper height and we can start finding & approaching the tag
while not self.stopTask and not self.success:
# Create a copy of the current navdata, so it doesn't get refreshed while we work on it
workingNavdata = self.recentNavdata
# Receive the necessary actions
steeringCommand = self.TellMeWhatToDo(workingNavdata)
# Send them to the drone
self.ExecuteCommand(steeringCommand)
# In case we got stopped via the keyboard, we want to make sure that our last command is to stop the drone from
# doing anything
steeringCommand = self.constants.STOP_MOVING
self.ExecuteCommand(steeringCommand)
print "Steps searching: %i" % self.searchCounter
print "Steps rotating: %i" % self.rotateCounter
print "Steps approaching: %i" % self.approachCounter
print "Steps centering: %i" % self.centerCounter
print "Steps centering bottom tag %i " % self.bottomCounter
def ExecuteCommand(self, steeringCommand):
"""
Takes a matrix33, publishes it to the drone, and waits for a moment
"""
# Publish the command
self.publishSteering.publish(steeringCommand)
# Let the drone actually do that action for a moment
self.r.sleep()
# Stop the movement
# self.publishSteering.publish(self.constants.STOP_MOVING)
# Wait a moment for the drone to calm down
# self.r.sleep()
def TellMeWhatToDo(self, navdata):
"""
Gets the current navdata and returns a matrix33 with appropriate steering commands
Automatically decides which commands to send, based on the available tags
"""
# We got 3 cases: No tag visible, front tag visble, bottom tag visible
# Check where the tags are in the arrays in the navdata (You can check which information the navdata contains from the commandline with "rosmsg show ardrone/Navdata")
bottomTagIndex = self.GetBottomTagIndex(navdata)
frontTagIndex = self.GetFrontTagIndex(navdata)
if navdata.tags_count == 0:
# No tag visible
# We want the drone to turn in one place
print "Searching"
self.searchCounter += 1
steeringCommand = matrix33(0.0, 0.0, 0.0, self.constants.FIND_TAG_TURN_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0)
return steeringCommand
# return self.constants.STOP_MOVING
elif bottomTagIndex > -1:
# Sweet, the bottom tag is visible!
print "Centering bottom tag"
self.bottomCounter += 1
# We need to check if we are turned the right way above the tag
currentAngle = navdata.tags_orientation[bottomTagIndex]
# Actually this step doesn't seem to be necessary so we skip it
# It should have led to a bigger precision, but mostly the drone is oriented the right way
return self.LandingPreparations(navdata)
# if currentAngle > 170.0 and currentAngle < 190.0:
# # Ok, so we are turned correctly, we need to center the bottom tag now. If it is centered
# # the command to land will be returned
# return self.LandingPreparations(navdata)
# else:
# # We need to turn the drone on the spot
# return self.AlignBottomTag(navdata)
else:
# Only the front tag is visble
# Now we got 2 cases:
# - We are still in the spot we started and we are trying to center it (Meaning we are turning on the spot)
# - We are already on our way towards that tag
# The x position is represented in values between 0 - 1000 from left to right
x = navdata.tags_xc[frontTagIndex]
if x > 450 and x < 550:
# It is pretty centered, set the flag and start approaching
self.wasTagCentered = True
print "Approaching"
self.approachCounter += 1
return self.TellMeApproach(navdata)
else:
# Check if we already centered it once
if self.wasTagCentered:
# We did? Ok, then lets move laterally
print "Centering laterally"
self.centerCounter += 1
return self.TellMeCenter(navdata)
else:
# The tag hasn't been centered yet, turn on the spot
print "Turning"
self.rotateCounter += 1
return self.TellMeFindTag(navdata)
def TellMeFindTag(self, navdata):
"""
Returns a matrix33 with appropriate commands to center the front tag in the field of view,
where those commands will turn the drone on the spot
"""
tagIndex = self.GetFrontTagIndex(navdata)
# We feed the controller with values between -1 and 1, and we receive factors, with which we multiply the speed at which the
# drone is turned
controller_input = (navdata.tags_xc[tagIndex] - 500.0) / 500.0
controller_output = self.findTagController.update(controller_input)
# Sometimes the controller might want to do some drastic actions, which we want to avoid
controller_output = self.findTagController.avoid_drastic_corrections(controller_output)
return matrix33(
0.0, 0.0, 0.0, controller_output * self.constants.FIND_TAG_TURN_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0
)
def TellMeCenter(self, navdata):
"""
Returns a matrix33 with appropriate commands to center the front tag in the field of view,
where those commands will move the drone laterally
"""
tagIndex = self.GetFrontTagIndex(navdata)
controller_input = (navdata.tags_xc[tagIndex] - 500.0) / 500.0
controller_output = self.centerFrontController.update(controller_input)
controller_output = self.centerFrontController.avoid_drastic_corrections(controller_output)
# Thing is, the corrections done by the controller will have a pretty huge impact once we are
# close to the tag, therefore we reduce those corrections depending on the distance from the tag
# reducingFactor = self.constants.reduceFactor * navdata.tags_distance[tagIndex] / self.constants.reduceDistance
reducingFactor = navdata.tags_distance[tagIndex] / self.constants.reduceDistance
if reducingFactor > 1:
reducingFactor = 1
sidewardsMovement = reducingFactor * controller_output * self.constants.TAG_CENTER_VELOCITY
return matrix33(0.0, sidewardsMovement, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
def TellMeApproach(self, navdata):
"""
Returns a matrix33 whith commands to approach the tag up to a certain distance
"""
tagIndex = self.GetFrontTagIndex(navdata)
# We want the drone to stop at a certain point in front of the tag
# Also, we want it to go full speed up to controllerDistance away from that point, followed by a
# distance, where the controller handles the speed (The controller isn't given the actual distance but a float value representing how
# far away we are. It then returns a factor, with which we multiply our TAG_APPROACH_VELOCITY)
controller_input = (
navdata.tags_distance[tagIndex] - self.constants.desiredDistance
) / self.constants.controllerDistance
controller_output = self.approachFrontController.update(controller_input)
# The output value needs to be inverted, avoid drastic controller outputs
controller_output = -self.approachFrontController.avoid_drastic_corrections(controller_output)
return matrix33(
controller_output * self.constants.TAG_APPROACH_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
)
def LandingPreparations(self, navdata):
"""
Returns the appropriate commands to center the bottom tag. Once it is centered, it returns the command to land
"""
tagIndex = self.GetBottomTagIndex(navdata)
tagXPosition = navdata.tags_xc[tagIndex]
tagYPosition = navdata.tags_yc[tagIndex]
controller_input_y = (tagYPosition - 500.0) / 500.0
controller_input_x = (tagXPosition - 500.0) / 500.0
if not (tagYPosition > 400 and tagYPosition < 600) or not (tagXPosition > 400 and tagXPosition < 600):
# We aren't centered, now we determine in which direction we are more off, so we can deal with that one first
if (controller_input_y * controller_input_y) > (controller_input_x * controller_input_x):
controller_output_y = self.centerBottomYController.update(controller_input_y)
# controller_output_y = self.centerBottomYController.avoid_drastic_corrections(controller_output_y)
print "Y"
print controller_input_y
print controller_output_y
return matrix33(
controller_output_y * self.constants.CENTER_BOTTOM_Y_VELOCITY,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
)
else:
controller_output_x = self.centerBottomXController.update(controller_input_x)
# controller_output_x = self.centerBottomXController.avoid_drastic_corrections(controller_output_x)
print "X"
print controller_input_x
print controller_output_x
return matrix33(
0.0,
controller_output_x * self.constants.CENTER_BOTTOM_X_VELOCITY,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
)
else:
print "READY"
# return self.constants.STOP_MOVING
# Tag is centered in both directions, we are done, we can land!
self.success = True
return matrix33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0)
def AlignBottomTag(self, navdata):
"""
Returns the command to orient the drone in a 180° angle over the bottom tag
"""
tagIndex = self.GetBottomTagIndex(navdata)
controller_input = navdata.tags_orientation[tagIndex] - 180.0 / 360.0
controller_output = self.alignBottomController.update(controller_input)
controller_output = self.alignBottomController.avoid_drastic_corrections(controller_output)
return matrix33(
0.0, 0.0, 0.0, controller_output * self.constants.ALIGN_BOTTOM_TAG_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0
)
def GetBottomTagIndex(self, navdata):
"""
returns the index in the given navdata of the bottom tag
returns -1 if none is found
"""
for tagtype in navdata.tags_type:
if tagtype == self.constants.bottomTagType:
return navdata.tags_type.index(tagtype)
return -1
def GetFrontTagIndex(self, navdata):
"""
returns the index in the current navdata of the front tag
returns -1 if none is found
"""
for tagtype in navdata.tags_type:
if tagtype == self.constants.frontTagType:
return navdata.tags_type.index(tagtype)
return -1
def BringMeCenterRotate(self):
"""
A test method that only rotates the drone
"""
# Reset flags
self.stopTask = False
self.wasTagCentered = False
self.findTagController.setPoint(0)
while not self.stopTask and (
self.recentNavdata.tags_xc[self.GetFrontTagIndex(self.recentNavdata)] > 480
or self.recentNavdata.tags_xc[self.GetFrontTagIndex(self.recentNavdata)] < 520
):
# Create a copy of the current navdata, so it doesn't get refreshed while we work on it
workingNavdata = self.recentNavdata
if workingNavdata.tags_count == 0:
# No tag visible
# We want the drone to turn in one place
steeringCommand = matrix33(
0.0, 0.0, 0.0, self.constants.FIND_TAG_TURN_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0
)
else:
# Receive the necessary actions
steeringCommand = self.TellMeFindTag(workingNavdata)
self.ExecuteCommand(steeringCommand)
print "Done"
def BringMeCenterLateral(self):
"""
A test method that only moves the drone laterally
"""
# Reset flags
self.stopTask = False
self.wasTagCentered = False
self.centerFrontController.setPoint(0)
while not self.stopTask:
# Create a copy of the current navdata, so it doesn't get refreshed while we work on it
workingNavdata = self.recentNavdata
# Receive the necessary actions
steeringCommand = self.TellMeCenter(workingNavdata)
self.ExecuteCommand(steeringCommand)
def GetMeAligned(self):
"""
Test method that rotates the drone above the bottom tag
"""
# Reset flags
self.stopTask = False
self.wasTagCentered = False
self.alignBottomController.setPoint(0)
while not self.stopTask:
workingNavdata = self.recentNavdata
steeringCommand = self.AlignBottomTag(workingNavdata)
self.ExecuteCommand(steeringCommand)
def GetMeReady(self):
"""
Test method, that centers the drone above the bottom tag
"""
# Reset flags
self.stopTask = False
self.wasTagCentered = False
self.centerBottomXController.setPoint(0)
self.centerBottomYController.setPoint(0)
while not self.stopTask:
workingNavdata = self.recentNavdata
steeringCommand = self.LandingPreparations(workingNavdata)
self.ExecuteCommand(steeringCommand)
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}")
class Aruco_tracker():
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 search_marker(self):
"""
Interrompe o movimento se nao encontrar o marcador por tres frames
consecutivos. Após 4 segundos, inicia movimento de rotação para
procurar pelo marcador.
"""
if ((3 <= self.notFoundCount <= 20) and self.mc.isRunning):
self.mc.stop()
self.mc.setIsRunning(False)
elif (self.notFoundCount > 20):
self.mc._set_vel_setpoint(0.0, 0.0, 0.0, 80)
self.mc.setIsRunning(True)
return
def update_motion(self):
"""
Atualiza as velocidades utilizando controlador PID.
"""
horizontal_distance = self.calculator.horizontal_distance
vertical_distance = self.calculator.vertical_distance
x_distance = self.calculator.distance_x
alpha = self.calculator.alpha
velocity_x = self.pid_foward.update(x_distance)
Velocity_z = self.pid_height.update(vertical_distance)
if (x_distance < (self.distance + 10)):
velocity_y = self.pid_angle.update(alpha)
else:
velocity_y = 0
velocity_z = 0
yaw = self.pid_yaw.update(horizontal_distance)
self.mc._set_vel_setpoint(velocity_x, velocity_y, 0, yaw)
self.mc.setIsRunning(True)
return
def track_marker(self):
"""
Programa principal, drone segue um marcador aruco.
"""
while (self.cap.isOpened()):
ret, frame = self.cap.read()
if (frame is None):
break
if (self.cont % 6 == 0):
thread = threading.Thread(
target=self.calculator.calculateDistance, args=(frame, ))
thread.setDaemon(True)
thread.start()
if (self.calculator.markerFound):
self.notFoundCount = 0
self.update_motion()
else:
self.notFoundCount += 1
self.search_marker()
self.calculator.writeDistance(frame)
cv2.imshow('frame', frame)
self.cont += 1
if cv2.waitKey(10) & 0xFF == ord('q'):
self.mc.land()
cv2.destroyAllWindows()
break
cv2.waitKey()
self.sync.close_link()
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')
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]}")
