class MavControl(ButtonGui): # This is called when the pbPressed button is pressed. # Naming is similar for other functions. def on_pbTakeoff_pressed(self): print("TAKE OFF!") self.controller.SendTakeoff() def on_pbLand_pressed(self): print("LAND") self.controller.SendLand() def on_pbUp_pressed(self): print("Up") self.controller.SetCommand(roll=0, pitch=0, yaw_velocity=0, z_velocity=0.5) def on_pbUp_released(self): print("Up done.") self.controller.hover() def on_pbDown_pressed(self): print("Down") self.controller.SetCommand(roll=0, pitch=0, yaw_velocity=0, z_velocity=-0.5) def on_pbDown_released(self): print("Down done.") self.controller.hover() def on_pbForward_pressed(self): print("Forward") self.controller.SetCommand(roll=0, pitch=0.5, yaw_velocity=0, z_velocity=0) def on_pbForward_released(self): print("Forward done.") self.controller.hover() def on_pbBackward_pressed(self): print("Back") self.controller.SetCommand(roll=0, pitch=-0.5, yaw_velocity=0, z_velocity=0) def on_pbBackward_released(self): print("Backed it up.") self.controller.hover() def on_pbLeft_pressed(self): print("Left") self.controller.SetCommand(roll=0.3, pitch=0, yaw_velocity=0, z_velocity=0) def on_pbLeft_released(self): print("Left done.") self.controller.hover() def on_pbRight_pressed(self): print("Right") self.controller.SetCommand(roll=-0.3, pitch=0, yaw_velocity=0, z_velocity=0) def on_pbRight_released(self): print("Right done.") self.controller.hover() def on_pbRoLeft_pressed(self): print("Turn down") self.controller.SetCommand(roll=0,pitch=0,yaw_velocity=0.6,z_velocity=0) def on_pbRoLeft_released(self): print("For what!") self.controller.hover() def on_pbRoRight_pressed(self): print("Turn up!") self.controller.SetCommand(roll=0,pitch=0,yaw_velocity=-0.6,z_velocity=0) def on_pbRoRight_released(self): print("Turnt!") self.controller.hover() def on_pbEmergency_pressed(self): print("It's goin' down!") self.controller.SendEmergency() def on_pbFlat_pressed(self): print("flat") self.controller.SetFlatTrim() def on_pbLED_pressed(self): print("RAVE!") self.controller.SetLedAnimation(4,2,5) def on_pbCamChange_pressed(self): print("Look!") self.controller.ToggleCamera() @pyqtSlot(bool) def on_cbAuto_clicked(self, # True is the checkbox is checked; False if not. checked): if checked: # Initialize our state if we're just entering # auto mode. self.state = 1 # Create a timer for use in ``fly()``. self.elapsedTimer = QElapsedTimer() else: # Return to a hover when leaving auto mode. self.controller.hover() # This is only called when the Auto checkbox is checked. def fly(self, # The x coordinate of the center of the tracked area. # It ranges between 0 and ``self.lbVideo.width() - 1``. x_center, # The y coordinate of the center of the tracked area. # It ranges between 0 and ``self.lbVideo.height() - 1``. y_center, # The area, in pixels, of the tracked region. cont_area): # Clear the description of what this does. self.lbAuto.setText('') ori_area=cont_area # Decide what to do based on the state. if self.state == 1: # Take off # ^^^^^^^^ # The Auto checkbox was just checked. Take off # to start out mission. self.updateAutoLabel('Takeoff!') self.controller.SendTakeoff() print('Takeoff') #print(cont_area,'area') # Measure time from takeoff. self.elapsedTimer.start() #print(x_center,'xy',y_center) # Move to the next state, waiting for takeoff to # finish. self.state = 2 print('x_center',x_center) print('y_center',y_center) print(' area',cont_area) elif self.state == 2: # Wait until take off completed # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ self.updateAutoLabel('Wait until take off completed') print('state2') # Don't send any commands until we're flying. # So, wait 5 seconds then go to the next state. if self.elapsedTimer.elapsed() >= 5000: self.state = 8 # Starting timer elif self.state ==3: self.controller.hover() self.controller.SetCommand(roll=0,pitch=0,yaw_velocity=0.3,z_velocity=0) self.updateAutoLabel('search for color') if (x_center>0 and y_center>0): self.elapsedTimer.restart() self.state=7 elif self.state==7: self.controller.SetCommand(roll=0,pitch=0,yaw_velocity=0.2,z_velocity=0) print(x_center,'xy',y_center) if (x_center<160 and x_center>0 ): print(x_center,'xy',y_center) self.controller.hover() if self.elapsedTimer.elapsed() >= 3000: print('state3') print(cont_area) self.elapsedTimer.restart() self.state =8 elif self.state == 4: # ...your ideas... print('state 4') #if (x_center < ???): # FIX TABBING FOR XCENTER self.updateAutoLabel('Flying Foward State 4') #if(x_center>=160 or x_center==-1 ): # self.state=7 # Changed the speed for safety #self.controller.SetCommand(0, 0.0625, 0, 0) #print(cont_area) #if (cont_area>4000 ) : # self.elapsedTimer.restart() # self.controller.hover() self.state ==5 elif self.state == 5: self.controller.hover() if(x_center==-1 and y_center==-1): self.state=3 if(x_center>=160): xr_speed=((x_center-160)*0.0031)**1.5 if xr_speed<0.03: xr_speed=0.03 self.controller.SetCommand(roll=-xr_speed, pitch=0.01, yaw_velocity=0, z_velocity=0) print('move right') print('x_center',x_center) print('y_center',y_center) elif(x_center<=140): xl_speed=((140-x_center)*0.0031)**1.5 if xl_speed<0.03: xl_speed=0.03 self.controller.SetCommand(roll=xl_speed, pitch=0.01, yaw_velocity=0, z_velocity=0) print('move left') print('x_center',x_center) print('y_center',y_center) else: self.controller.hover() self.state=6 elif self.state==6: self.controller.hover() if(x_center==-1 and y_center==-1): self.state=3 if(y_center>=110): print('move downward') yu_speed=(y_center-110)*0.005 if yu_speed<0.03: yu_speed=0.03 self.controller.SetCommand(roll=0, pitch=0.01, yaw_velocity=0, z_velocity=-yu_speed) print('x_center',x_center) print('y_center',y_center) elif(y_center<=90): print('move upward') yd_speed=(90-y_center)*0.005 if yd_speed<0.03: yd_speed=0.03 self.controller.SetCommand(roll=0, pitch=0.01, yaw_velocity=0, z_velocity=yd_speed) print('x_center',x_center) print('y_center',y_center) else: self.elapsedTimer.restart() self.state=8 elif self.state==8: self.controller.hover() if(cont_area>9000): print('move backward') if(x_center==-1 and y_center==-1): self.state=3 b_speed=((cont_area-9000)*0.0001)**4 if b_speed>=0.3: b_speed=0.25 if(x_center>=160): xr_speed=((x_center-160)*0.0031)**1.5 if xr_speed<0.02: xr_speed=0.02 self.controller.SetCommand(roll=-xr_speed, pitch=-b_speed, yaw_velocity=0, z_velocity=0) print('move right backward') print('x_center',x_center) print('y_center',y_center) elif(x_center<=140): xl_speed=((140-x_center)*0.0031)**1.5 if xl_speed<0.02: xl_speed=0.02 self.controller.SetCommand(roll=xl_speed, pitch=-b_speed, yaw_velocity=0, z_velocity=0) print('move left backward') print('x_center',x_center) print('y_center',y_center) else: self.controller.SetCommand(roll=0, pitch=-b_speed, yaw_velocity=0, z_velocity=0) #self.controller.hover() print(' area',cont_area) elif(cont_area<7000): f_speed=((7000-cont_area)*0.0001)**3.5 if f_speed>=0.3: f_speed=0.25 if(x_center==-1 and y_center==-1): self.state=3 if(x_center>=160): xr_speed=((x_center-160)*0.0031)**2.5 self.controller.SetCommand(roll=-xr_speed, pitch=f_speed, yaw_velocity=0, z_velocity=0) print('move right forward') print('x_center',x_center) print('y_center',y_center) elif(x_center<=140): xl_speed=((140-x_center)*0.0031)**2.5 self.controller.SetCommand(roll=xl_speed, pitch=f_speed, yaw_velocity=0, z_velocity=0) print('move left forward') print('x_center',x_center) print('y_center',y_center) else: self.controller.SetCommand(roll=0, pitch=f_speed, yaw_velocity=0, z_velocity=0) print('move foreward') #self.controller.hover() print(' area',cont_area) print('speed:',f_speed) #if(cont_area<=2500 and (x_center>=170 or x_center<=140)): # self.controller.hover() #if(cont_area<=2000): # self.state=3 #if(x_center<=140 or x_center>= 190): # self.state=5 else: pef_area=cont_area self.controller.hover() if self.elapsedTimer.elapsed() >= 3000: self.elapsedTimer.restart() if(x_center<=140 or x_center>= 190): self.state=5 if(y_center>=110 or y_center<=90): self.state=6 if(x_center==-1 and y_center==-1): self.state=3 else: self.controller.hover() print(' area',cont_area) print('got you!') #if self.elapsedTimer.elapsed() >= 5000: # self.state=9 elif self.state == 9: print('state 5') self.updateAutoLabel('Land State 5') self.controller.hover() if self.elapsedTimer.elapsed() >= 2000: self.controller.SendLand() self.cbAuto.toggle() else: self.updateAutoLabel('Unknown state! Help!') # 1. Determine what to do by examining ``x_center``, # ``y_center``, etc. # # 2. Explain what your code will do: # ``self.updateAutoLabel('Flying up!')``. # # 3. Then do it, using something like: # ``self.controller.SetCommand(roll, pitch, # yaw_velocity, z_velocity)``, where you fill in # numbers in place of ``roll``, ``pitch``, etc. # # A template for your code:: # # if (x_center < ???): # self.updateAutoLabel('Flying left!') # self.controller.SetCommand(0.3, 0, 0, 0) # Explain what the drone is doing in auto mode by # displaying strings telling its intentions. def updateAutoLabel(self, # A string to add to the explanation. s): self.lbAuto.setText(self.lbAuto.text() + s)
class MainWindow(QMainWindow): def __init__( self, # Webcam index (0 = first, etc.). webcamIndex=0, # Resolution, as an index into ``self.supported_resolution``. webcamInitialResolution=-1, # Qt Designer file to load. uiFile=None, # Other args to pass on to Qt. *args, **kwargs ): super(MainWindow, self).__init__(*args, **kwargs) self.uiFile = uiFile or join(dirname(__file__), "cv_video.ui") self.webcamIndex = webcamIndex self.webcamInitialResolution = webcamInitialResolution def __enter__(self): # Load in the GUI. uic.loadUi(self.uiFile, self) # Open a webcam. self.cap = cv2.VideoCapture(self.webcamIndex) assert self.cap.isOpened() # Scan for possible resolutions from a list of common resolutions. self.supported_resolution = [] for resolution in ( # 16:9 aspect ratio: (1920, 1080), (1280, 720), # 4:3 aspect ratio: (1024, 768), (640, 480), ): self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, resolution[0]) self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, resolution[1]) if ( self.cap.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH) == resolution[0] and self.cap.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT) == resolution[1] ): self.supported_resolution.append(resolution) self.cbWebcamResolution.addItem("{}x{}".format(resolution[0], resolution[1]), resolution) if self.webcamInitialResolution < 0: self.webcamInitialResolution = len(self.supported_resolution) - 1 self.cbWebcamResolution.setCurrentIndex(self.webcamInitialResolution) # Set up a timer to read from the webcam. self.webcamTimer = QTimer(self) self.webcamTimer.setInterval(0) self.webcamTimer.timeout.connect(self._on_webcamTimer_timeout) self.webcamTimer.start() # Monitor the fps. self.fpsLabel = QLabel() self.statusBar().addPermanentWidget(self.fpsLabel) self.fpsTimer = QElapsedTimer() self.fpsTimer.start() return self def __exit__(self, exc_type, exc_value, traceback): self.cap.release() self.webcamTimer.timeout.disconnect(self._on_webcamTimer_timeout) self.webcamTimer.stop() @pyqtSlot(int) def on_cbWebcamResolution_currentIndexChanged( self, # The index of the current item in the combobox index, ): resolution = self.cbWebcamResolution.itemData(index) self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, resolution[0]) self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, resolution[1]) @pyqtSlot() def _on_webcamTimer_timeout(self): # Get a frame from the webcam. success_flag, cvImage = self.cap.read() assert success_flag self._processImage(cvImage) self.fpsLabel.setText("{} fps".format(round(1000.0 / self.fpsTimer.elapsed()))) self.fpsTimer.restart() # This routine is called whenever a webcam image is available to process. It # should also display the resulting image. def _processImage( self, # The OpenCV image to process. cvImage, ): # Don't process; to demonstrate, simply display the image. cvImageToQt(cvImage, self.lOrig)