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...')
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
class Controller(object): 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 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 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)
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()