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)
