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)
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]}")