class Main():
def __init__(self, GUI, DEBUG, fps, stick_sens, saveFile):
self.GUI = GUI
self.DEBUG = DEBUG
self.max_fps = fps
self.stick_sens = stick_sens
self.saveFile = saveFile
pygame.init()
self.clock = pygame.time.Clock()
self.run = True
self.started = False
self.main_throttle = 0
self.throttle = [0,0,0,0]
self.holdPosition = 0
self.holdHeight = 0
self.isGamepad = 1
self.gamepad = Gamepad()
self.gamepad_axes = [0,0,0]
self.gamepad_throttle = 0
self.bno = BNO()
self.m1 = Motor(15)
self.m2 = Motor(27)
self.m3 = Motor(10)
self.m4 = Motor(7)
self.motors = [self.m1, self.m2, self.m3, self.m4]
if self.GUI:
self.gui = Gui()
if self.GUI:
self.gui.showMessage("Press 'start'-Button to start!")
if self.DEBUG:
# Print system status and self test result.
status, self_test, error = self.bno.get_system_status()
print("DEBUG: System status: {0}".format(status))
print("DEBUG: Self test result (0x0F is normal): 0x{0:02X}".format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print("DEBUG: System error: {0}".format(error))
print("DEBUG: See datasheet section 4.3.59 for the meaning.")
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = self.bno.get_revision()
print("DEBUG: Software version: {0}".format(sw))
print("DEBUG: Bootloader version: {0}".format(bl))
print("DEBUG: Accelerometer ID: 0x{0:02X}".format(accel))
print("DEBUG: Magnetometer ID: 0x{0:02X}".format(mag))
print("DEBUG: Gyroscope ID: 0x{0:02X}\n".format(gyro))
self.load()
if self.DEBUG:
print("DEBUG: READY!")
self.loop()
def eventHandler(self):
for event in pygame.event.get():
if event.type == QUIT:
if self.DEBUG:
print("DEBUG: SHUTDOWN")
pygame.quit()
if self.GUI:
curses.endwin()
sys.exit()
elif event.type == JOYBUTTONDOWN:
self.gamepad.handleButtonDown(event.button)
elif event.type == JOYBUTTONUP:
self.gamepad.handleButtonUp(event.button)
def getGamepadValues(self):
self.gamepad_throttle = self.gamepad.getThrottle()
self.gamepad_axes = self.gamepad.getAxis()
def checkCalibButtons(self):
if self.gamepad.isSave:
if self.DEBUG:
print("DEBUG: Save Calibration-Data")
self.save()
if self.DEBUG:
print("DEBUG: Data saved")
if self.gamepad.isCalib:
if self.DEBUG:
print("DEBUG: calibrate BNO")
self.bno.calibrateBNO()
if self.DEBUG:
print("DEBUG: new offset: {0}".format(self.bno.offset))
def save(self):
if self.DEBUG:
print("DEBUG: Open File: "+self.saveFile)
f = open(os.path.abspath(self.saveFile), "w")
f.write(str(self.bno.offset))
f.write("\n")
f.write(str(self.gamepad.offset))
f.close()
self.gamepad.isSave = False
def load(self):
if self.DEBUG:
print("DEBUG: Open File: "+self.saveFile)
try:
f = open(os.path.abspath(self.saveFile), "r")
except IOError:
if self.DEBUG:
print("DEBUG: No SaveFile - Nothing to load")
return
if self.DEBUG:
print("DEBUG: Loading...")
loaded = f.read().replace(" ", "").split("\n")
loaded = [loaded[0][1:-1].split(","), loaded[1][1:-1].split(",")]
self.bno.offset = [float(loaded[0][0]), float(loaded[0][1]), float(loaded[0][2])]
self.gamepad.offset = [float(loaded[1][0]), float(loaded[1][1]), float(loaded[1][2])]
if self.DEBUG:
print("Loaded! - bno-offset: {0} - gamepad-offset: {1}".format(self.bno.offset, self.gamepad.offset))
f.close()
def start_motors(self):
if self.DEBUG:
print("DEBUG: Calibrate Throttle!")
if self.GUI:
self.gui.hideMessage()
self.gui.showMessage("Calibrate Throttle!")
while self.gamepad.getThrottle()<=50:
pygame.event.pump()
while self.gamepad.getThrottle()!=0:
pygame.event.pump()
if self.DEBUG:
print("DEBUG: Initializing motors")
for m in self.motors:
m.start()
self.started = True
if self.DEBUG:
print("DEBUG: STARTED!")
if self.GUI:
self.gui.hideMessage()
def stop_motors(self):
if self.DEBUG:
print("DEBUG: stopping motors")
for m in self.motors:
m.stop()
self.started = False
if self.DEBUG:
print("DEBUG: STOPPED!")
print("DEBUG: READY!")
if self.GUI:
self.gui.showMessage("Press 'start'-Button to start!")
def changeMotorSpeed(self):
self.throttle = [0,0,0,0]
self.getGamepadValues()
self.bno.tick()
if (self.gamepad.isBNO):
if self.DEBUG:
print('DEBUG: Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}'.format(self.bno.heading, self.bno.roll, self.bno.pitch))
pitch = self.bno.pitch
roll = self.bno.roll
if pitch > 10:
pitch = 10
elif pitch < -10:
pitch = -10
if roll > 10:
roll = 10
elif roll < -10:
roll = -10
self.throttle[0] += pitch-roll
self.throttle[1] += pitch+roll
self.throttle[2] += -pitch+roll
self.throttle[3] += -pitch-roll
#Pad-Motor-Steuerung
self.throttle[0] += -self.gamepad_axes[0]*self.stick_sens+self.gamepad.offset[0] + self.gamepad_axes[1]*self.stick_sens+self.gamepad.offset[1] - self.gamepad.offset[2]
self.throttle[1] += -self.gamepad_axes[0]*self.stick_sens+self.gamepad.offset[0] - self.gamepad_axes[1]*self.stick_sens-self.gamepad.offset[1] + self.gamepad.offset[2]
self.throttle[2] += +self.gamepad_axes[0]*self.stick_sens-self.gamepad.offset[0] - self.gamepad_axes[1]*self.stick_sens-self.gamepad.offset[1] - self.gamepad.offset[2]
self.throttle[3] += +self.gamepad_axes[0]*self.stick_sens-self.gamepad.offset[0] + self.gamepad_axes[1]*self.stick_sens+self.gamepad.offset[1] + self.gamepad.offset[2]
self.throttle = [int(self.gamepad_throttle+self.throttle[i]) for i in range(4)]
for i in range(4):
if self.throttle[i] > 100:
self.throttle[i] = 100
elif self.throttle[i] < 0:
self.throttle[i] = 0
self.motors[i].setW(self.throttle[i])
def loop(self):
while self.run:
self.clock.tick(self.max_fps)
pygame.event.pump()
self.eventHandler()
if self.GUI:
self.gui.guiTick(self.clock.get_fps(), self.throttle, self.gamepad.isStart, self.gamepad.isBNO, [self.bno.heading, self.bno.pitch, self.bno.roll])
if self.gamepad.isStart:
if not self.started:
self.start_motors()
self.changeMotorSpeed()
else:
self.checkCalibButtons()
if self.started:
self.stop_motors()
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()
'''
