class FunctionalGUI(Ui_Form):
'''
-----------------INITIALIZATION--------------------------------------------
'''
def __init__(self, window):
super().__init__()
self.ser = None
self.read_timer = None
self.gamepad_timer = None
self.gamepad = None
self.bytes = 0 # Used to track the amount of bytes sent
self.packets = 0 # Used to track the amount of packets sent
self.setupUi(window)
self.setButtons()
self.createGamepad()
def setButtons(self):
self.throttle_send_btn.clicked.connect(self.sendThrottle)
self.throttle_reset_btn.clicked.connect(self.resetThrottle)
self.angles_send_btn.clicked.connect(self.sendAngles)
self.angles_reset_btn.clicked.connect(self.resetAngles)
self.mode_raw_btn.clicked.connect(self.mode_raw)
self.mode_rate_btn.clicked.connect(self.mode_rate)
self.mode_angle_btn.clicked.connect(self.mode_angle)
self.start_btn.clicked.connect(self.start)
self.stop_btn.clicked.connect(self.stop)
self.serial_open_btn.clicked.connect(self.openConnection)
self.pid_send_btn.clicked.connect(self.sendPID)
self.yaw_reset_btn.clicked.connect(self.resetYaw)
self.gamepad_connect_btn.clicked.connect(self.createGamepad)
self.gamepad_apply_btn.clicked.connect(self.setGamepadFrequency)
'''
-----------------GAMEPAD---------------------------------------------------
'''
def createGamepad(self):
if self.gamepad == None:
print("Connecting gamepad...")
self.gamepad = Gamepad(0)
if self.gamepad.enabled():
self.gamepad.connectButton(0, self.addMinThrottle)
self.gamepad.connectButton(1, self.removeMinThrottle)
self.gamepad.connectButton(6, self.stop)
self.gamepad.connectButton(7, self.start)
self.gamepad.connectButton(3, self.disableGamepad)
self.gamepad_timer = PyQt5.QtCore.QTimer()
self.gamepad_timer.timeout.connect(self.checkGamepad)
self.setGamepadFrequency() # This method starts gamepad_timer
else:
self.gamepad.close()
self.gamepad = None
else:
print("Gamepad already exists!")
def setGamepadFrequency(self):
if self.gamepad != None:
self.gamepad_timer.stop()
freq = self.gamepad_frequency_spinBox.value()
interval = 1000 / freq # Milliseconds
self.gamepad_timer.start(interval)
print("Gamepad updating rate is now", freq,
"({:f} ms)".format(interval))
else:
print("Gamepad doesn't exist!")
def checkGamepad(self):
if self.gamepad != None:
self.gamepad.update()
y, p, r = self.gamepad.getYPR()
throttle = self.gamepad.getThrottle()
self.changeYPR(y, p, r)
self.changeThrottle(throttle)
if self.angles_sendOnChange_bool.isChecked():
self.sendAngles()
if self.throttle_sendOnChange_bool.isChecked():
self.sendThrottle()
def disableGamepad(self):
print("Disabling gamepad")
self.resetThrottle()
if self.gamepad != None:
self.gamepad.close()
self.gamepad = None
'''
-----------------BUTTONS---------------------------------------------------
'''
# --------------Primary control--------------------------------------------
def changeYPR(self, yaw, pitch, roll):
self.yaw_doubleSpinBox.setValue(yaw * self.yaw_max_spinbox.value())
self.pitch_doubleSpinBox.setValue(pitch *
self.pitch_max_spinbox.value())
self.roll_doubleSpinBox.setValue(roll * self.roll_max_spinbox.value())
def changeThrottle(self, throttle_input):
# Input goes from 0 to 1
self.throttle_spinBox.setValue(throttle_input *
self.throttle_max_spinBox.value())
def addMinThrottle(self):
prev = self.throttle_offset_spinBox.value()
increment = self.throttle_offset_step_spinBox.value()
self.throttle_offset_spinBox.setValue(prev + increment)
print("Setting default throttle to:",
self.throttle_offset_spinBox.value())
def removeMinThrottle(self):
prev = self.throttle_offset_spinBox.value()
increment = self.throttle_offset_step_spinBox.value()
self.throttle_offset_spinBox.setValue(prev - increment)
if self.throttle_offset_spinBox.value() < 0:
self.throttle_offset_spinBox.setValue(0)
print("Setting default throttle to:",
self.throttle_offset_spinBox.value())
def sendAngles(self):
y = self.yaw_doubleSpinBox.value(
) + self.yaw_offset_doubleSpinBox.value()
p = self.pitch_doubleSpinBox.value(
) + self.pitch_offset_doubleSpinBox.value()
r = self.roll_doubleSpinBox.value(
) + self.roll_offset_doubleSpinBox.value()
string = 'y' + str(y)
string += 'p' + str(p)
string += 'r' + str(r)
self.send(string)
def sendThrottle(self):
thr = self.throttle_spinBox.value(
) + self.throttle_offset_spinBox.value()
string = 't' + str(thr)
self.send(string)
# --------------Secondary control------------------------------------------
def start(self):
self.send(chr(17))
print("Activating engines")
def stop(self):
#self.resetAngles()
self.resetThrottle()
self.send(chr(20)) #DC4
print("Disabling engines")
def resetYaw(self):
'''
Resets the yaw axis of copter to zero.
'''
self.send('r')
print("Resetting yaw axis")
def mode_raw(self):
'''
Starts the raw controlling mode.
Angles correspond to motor powers without PID.
'''
self.send('m2')
print("Switching to raw control")
def mode_rate(self):
'''
Starts the rate controlling mode.
Angles correspond to angular rates.
'''
self.send('m1')
print("Switching to rate control")
def mode_angle(self):
'''
Starts the angle controlling mode.
Angles correspond to actual orientation of the copter.
'''
self.send('m0')
print("Switching to angle control")
def resetAngles(self):
self.yaw_doubleSpinBox.setValue(0)
self.pitch_doubleSpinBox.setValue(0)
self.roll_doubleSpinBox.setValue(0)
self.yaw_offset_doubleSpinBox.setValue(0)
self.pitch_offset_doubleSpinBox.setValue(0)
self.roll_offset_doubleSpinBox.setValue(0)
self.sendAngles()
def resetThrottle(self):
self.throttle_offset_spinBox.setValue(0)
self.throttle_spinBox.setValue(0)
self.sendThrottle()
# --------------Other commands---------------------------------------------
def sendPID(self):
self.send('p' + str(self.pid_rollpitch_p.value()))
self.send('i' + str(self.pid_rollpitch_i.value()))
self.send('w' + str(self.pid_yaw_p.value()))
print("All pid values sent!")
# ----------------GUI------------------------------------------------------
'''
-----------------SERIAL STUFF----------------------------------------------
'''
def openConnection(self):
print("Opening bluetooth connection...")
self.ser = serial.Serial(
port=None,
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS,
writeTimeout=2) # Wait maximum 2 seconds on write
if (self.read_timer == None):
self.read_timer = PyQt5.QtCore.QTimer()
self.read_timer.timeout.connect(self.readSerial)
self.read_timer.start(100)
for i in range(10):
try:
print("Trying port", i)
self.ser.port = '/dev/rfcomm' + str(i)
self.ser.open()
print("Bluetooth open!")
return # Success!
except serial.serialutil.SerialException:
print("Cannot open bluetooth at port", i)
self.ser = None # Opening failed
def send(self, message):
if (self.ser != None):
message = chr(2) + message #STX
message += chr(3) #ETX
self.packets += 1
self.bytes += len(message)
self.data_label.setText(str(self.bytes))
self.packet_label.setText(str(self.packets))
try:
self.ser.write(message.encode())
except serial.serialutil.SerialTimeoutException:
print("WARNING: Write timeout exceeded!")
print("WARNING: Bluetooth disconnected")
self.ser = None
def readSerial(self):
if (self.ser != None):
while (self.ser.inWaiting() > 0):
try:
c = self.ser.read(1).decode()
print("{:}".format(c), end='')
if (c == chr(5)):
self.send(chr(6))
except UnicodeDecodeError:
print("UnicodeDecodeError: Received bad data")
else:
self.read_timer.stop()
'''
class FunctionalGUI(Ui_Form):
'''
-----------------INITIALIZATION--------------------------------------------
'''
def __init__(self, window):
super().__init__()
self.ser = None
self.read_timer = None
self.gamepad_timer = None
self.gamepad = None
self.bytes = 0 # Used to track the amount of bytes sent
self.packets = 0 # Used to track the amount of packets sent
self.setupUi(window)
self.setButtons()
self.createGamepad()
def setButtons(self):
self.throttle_send_btn.clicked.connect(self.sendThrottle)
self.throttle_reset_btn.clicked.connect(self.resetThrottle)
self.angles_send_btn.clicked.connect(self.sendAngles)
self.angles_reset_btn.clicked.connect(self.resetAngles)
self.mode_raw_btn.clicked.connect(self.mode_raw)
self.mode_rate_btn.clicked.connect(self.mode_rate)
self.mode_angle_btn.clicked.connect(self.mode_angle)
self.start_btn.clicked.connect(self.start)
self.stop_btn.clicked.connect(self.stop)
self.serial_open_btn.clicked.connect(self.openConnection)
self.pid_send_btn.clicked.connect(self.sendPID)
self.yaw_reset_btn.clicked.connect(self.resetYaw)
self.gamepad_connect_btn.clicked.connect(self.createGamepad)
self.gamepad_apply_btn.clicked.connect(self.setGamepadFrequency)
'''
-----------------GAMEPAD---------------------------------------------------
'''
def createGamepad(self):
if self.gamepad == None:
print("Connecting gamepad...")
self.gamepad = Gamepad(0)
if self.gamepad.enabled():
self.gamepad.connectButton(0, self.addMinThrottle)
self.gamepad.connectButton(1, self.removeMinThrottle)
self.gamepad.connectButton(6, self.stop)
self.gamepad.connectButton(7, self.start)
self.gamepad.connectButton(3, self.disableGamepad)
self.gamepad_timer = PyQt5.QtCore.QTimer()
self.gamepad_timer.timeout.connect(self.checkGamepad)
self.setGamepadFrequency() # This method starts gamepad_timer
else:
self.gamepad.close()
self.gamepad = None
else:
print("Gamepad already exists!")
def setGamepadFrequency(self):
if self.gamepad != None:
self.gamepad_timer.stop()
freq = self.gamepad_frequency_spinBox.value()
interval = 1000 / freq # Milliseconds
self.gamepad_timer.start(interval)
print("Gamepad updating rate is now", freq, "({:f} ms)".format(interval))
else:
print("Gamepad doesn't exist!")
def checkGamepad(self):
if self.gamepad != None:
self.gamepad.update()
y, p, r = self.gamepad.getYPR()
throttle = self.gamepad.getThrottle()
self.changeYPR(y, p, r)
self.changeThrottle(throttle)
if self.angles_sendOnChange_bool.isChecked():
self.sendAngles()
if self.throttle_sendOnChange_bool.isChecked():
self.sendThrottle()
def disableGamepad(self):
print("Disabling gamepad")
self.resetThrottle()
if self.gamepad != None:
self.gamepad.close()
self.gamepad = None
'''
-----------------BUTTONS---------------------------------------------------
'''
# --------------Primary control--------------------------------------------
def changeYPR(self, yaw, pitch, roll):
self.yaw_doubleSpinBox.setValue(yaw * self.yaw_max_spinbox.value())
self.pitch_doubleSpinBox.setValue(pitch * self.pitch_max_spinbox.value())
self.roll_doubleSpinBox.setValue(roll * self.roll_max_spinbox.value())
def changeThrottle(self, throttle_input):
# Input goes from 0 to 1
self.throttle_spinBox.setValue(throttle_input * self.throttle_max_spinBox.value())
def addMinThrottle(self):
prev = self.throttle_offset_spinBox.value()
increment = self.throttle_offset_step_spinBox.value()
self.throttle_offset_spinBox.setValue(prev + increment)
print("Setting default throttle to:", self.throttle_offset_spinBox.value())
def removeMinThrottle(self):
prev = self.throttle_offset_spinBox.value()
increment = self.throttle_offset_step_spinBox.value()
self.throttle_offset_spinBox.setValue(prev - increment)
if self.throttle_offset_spinBox.value() < 0:
self.throttle_offset_spinBox.setValue(0)
print("Setting default throttle to:", self.throttle_offset_spinBox.value())
def sendAngles(self):
y = self.yaw_doubleSpinBox.value() + self.yaw_offset_doubleSpinBox.value()
p = self.pitch_doubleSpinBox.value() + self.pitch_offset_doubleSpinBox.value()
r = self.roll_doubleSpinBox.value() + self.roll_offset_doubleSpinBox.value()
string = 'y' + str(y)
string += 'p' + str(p)
string += 'r' + str(r)
self.send(string)
def sendThrottle(self):
thr = self.throttle_spinBox.value() + self.throttle_offset_spinBox.value()
string = 't' + str(thr)
self.send(string)
# --------------Secondary control------------------------------------------
def start(self):
self.send(chr(17))
print("Activating engines")
def stop(self):
#self.resetAngles()
self.resetThrottle()
self.send(chr(20)) #DC4
print("Disabling engines")
def resetYaw(self):
'''
Resets the yaw axis of copter to zero.
'''
self.send('r')
print("Resetting yaw axis")
def mode_raw(self):
'''
Starts the raw controlling mode.
Angles correspond to motor powers without PID.
'''
self.send('m2')
print("Switching to raw control")
def mode_rate(self):
'''
Starts the rate controlling mode.
Angles correspond to angular rates.
'''
self.send('m1')
print("Switching to rate control")
def mode_angle(self):
'''
Starts the angle controlling mode.
Angles correspond to actual orientation of the copter.
'''
self.send('m0')
print("Switching to angle control")
def resetAngles(self):
self.yaw_doubleSpinBox.setValue(0)
self.pitch_doubleSpinBox.setValue(0)
self.roll_doubleSpinBox.setValue(0)
self.yaw_offset_doubleSpinBox.setValue(0)
self.pitch_offset_doubleSpinBox.setValue(0)
self.roll_offset_doubleSpinBox.setValue(0)
self.sendAngles()
def resetThrottle(self):
self.throttle_offset_spinBox.setValue(0)
self.throttle_spinBox.setValue(0)
self.sendThrottle()
# --------------Other commands---------------------------------------------
def sendPID(self):
self.send('p' + str(self.pid_rollpitch_p.value()))
self.send('i' + str(self.pid_rollpitch_i.value()))
self.send('w' + str(self.pid_yaw_p.value()))
print("All pid values sent!")
# ----------------GUI------------------------------------------------------
'''
-----------------SERIAL STUFF----------------------------------------------
'''
def openConnection(self):
print("Opening bluetooth connection...")
self.ser = serial.Serial(
port=None,
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS,
writeTimeout=2) # Wait maximum 2 seconds on write
if (self.read_timer == None):
self.read_timer = PyQt5.QtCore.QTimer()
self.read_timer.timeout.connect(self.readSerial)
self.read_timer.start(100)
for i in range(10):
try:
print("Trying port", i)
self.ser.port = '/dev/rfcomm' + str(i)
self.ser.open()
print("Bluetooth open!")
return # Success!
except serial.serialutil.SerialException:
print("Cannot open bluetooth at port", i)
self.ser = None # Opening failed
def send(self, message):
if (self.ser != None):
message = chr(2) + message #STX
message += chr(3) #ETX
self.packets += 1
self.bytes += len(message)
self.data_label.setText(str(self.bytes))
self.packet_label.setText(str(self.packets))
try:
self.ser.write(message.encode())
except serial.serialutil.SerialTimeoutException:
print("WARNING: Write timeout exceeded!")
print("WARNING: Bluetooth disconnected")
self.ser = None
def readSerial(self):
if (self.ser != None):
while (self.ser.inWaiting() > 0):
try:
c = self.ser.read(1).decode()
print("{:}".format(c), end='')
if (c == chr(5)):
self.send(chr(6))
except UnicodeDecodeError:
print("UnicodeDecodeError: Received bad data")
else:
self.read_timer.stop()
'''
