def __init__(self, Kp, Ki, Kd, dControllerTime, integratorMin,
integratorMax):
'''
Moves the Sub based on XYZ coordinates and desired yaw, pitch, and roll.
**Parameters**: \n
* **Kp** - Factor for proportional gain.
* **Ki** - Factor for integral or error over time gain.
* **Kd** - Factor for dampening
* **integratorMin** - Minimum integrator factor.
* **integratorMax** - Maximum integrator factor.
**Returns**: \n
* **No Return.**\n
'''
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.derivator = 0
self.integrator = 0
self.integratorMin = integratorMin
self.integratorMax = integratorMax
self.dControllerTimer = utilities.Timer()
self.dControllerTime = dControllerTime
def run(self):
self.sendDebugMessage("Starting...")
missionIndex = 0
self.running = True
waypointError = None
'''
This block of code is only called when in manual control mode, and there for does not use the mission code
'''
if self.manualMode == True and self.debug == False:
controllerData = None
self.pressedDepthChange = False
self.desiredMoveDepth = 0
controllerDepthButtonReleaseTimer = utilities.Timer()
self.toggleLeftBumper = False
self.toggleRightBumper = False
self.quickButtonPressDepth = True
while self.running:
while len(self.joystickController.getList) > 0:
controllerData = self.joystickController.getList.pop(0)
print controllerData
xPwm = 0
zPwm = 0
yDesiredRotation = 0
xDesiredRotation = 0
poseData = copy.copy(self.externalCommThread.orientation)
poseData.append(self.externalCommThread.dvlData[3])
if controllerData == None:
print "No Controller Data"
#continue
else:
#print controllerData
joystickInput = controllerData[1]
buttonInput = controllerData[2]
buttons = buttonInput
hats = controllerData[3]
if buttonInput[
4] == 1 and self.pressedDepthChange == False:
self.manualDepthValue -= 1
self.pressedDepthChange = True
if buttonInput[
5] == 1 and self.pressedDepthChange == False:
self.manualDepthValue += 1
self.pressedDepthChange = True
if buttonInput[4] == 0 and buttonInput[5] == 0:
self.pressedDepthChange = False
if buttonInput[7] == 1:
for i in range(0, 9):
self.maestroSerial.write(
bytearray([0xFF, i, 0x7F]))
maxSpeed = 80
xPwm = maxSpeed * joystickInput[0] * 2
zPwm = maxSpeed * joystickInput[1] * -2
poseData = copy.copy(
self.externalCommThread.orientation)
poseData.append(self.externalCommThread.dvlData[3])
yDesiredRotation = maxSpeed / 4 * -joystickInput[4]
xDesiredRotation = maxSpeed / 4 * -joystickInput[3]
axis0, axis1, axis2, axis3, axis4 = int(
joystickInput[0] * 204), int(
joystickInput[1] * 204), int(
joystickInput[2] * 204), int(
joystickInput[3] * 204), int(
joystickInput[4] * 204)
#DEPTH
if hats[0][
1] != 0: #Have the ability to increment depth
if self.quickButtonPressDepth == True: #Eliminates Schmitt trigger effect
self.desiredMoveDepth += -hats[0][1] * 0.5
if self.desiredMoveDepth < -1:
self.desiredMoveDepth = -1
controllerDepthButtonReleaseTimer.restartTimer(
)
self.quickButtonPressDepth = False
#print self.desiredMoveDepth, "depth inc"
elif hats[0][1] == 0:
netControllerDepthButtonReleaseTimer = controllerDepthButtonReleaseTimer.netTimer(
controllerDepthButtonReleaseTimer.
cpuClockTimeInSeconds())
if netControllerDepthButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressDepth = True
controllerDepthButtonReleaseTimer.restartTimer(
)
#TOGGLE ORIENTATION LOCK
if buttons[5] == True:
if self.quickButtonPressOrientationLock == True: #Eliminates Schmitt trigger effect
self.toggleRightBumper = not self.toggleRightBumper
self.quickButtonPressOrientationLock = False
elif buttons[5] == False:
self.quickButtonPressOrientationLock = True
if buttons[4] == True:
if self.quickButtonPressPositionLock == True: #Eliminates Schmitt trigger effect
self.toggleLeftBumper = not self.toggleLeftBumper
self.quickButtonPressPositionLock = False
self.lockedPosition = self.dvlGuiData[0]
currentDepth = self.medianExternalDepth
elif buttons[4] == False:
self.quickButtonPressPositionLock = True
#ORIENTATION LOCK
if self.toggleRightBumper == True and self.toggleLeftBumper == False: #Orientation lock
if self.toggleLeftBumper == True:
pass
#print "Position and Orientation Lock"
#poseData = [ahrsData[0], ahrsData[1], ahrsData[2], self.dvlGuiData[0][1], self.medianExternalDepth, self.dvlGuiData[0][0]]
#self.thrusterPWMs, error, yawDesired = self.moveController.advancedMove(poseData, self.lockedPosition[1], self.desiredMoveDepth, self.lockedPosition[0], self.desiredMovePitch, self.desiredMoveYaw, self.desiredMoveRoll, 0)
#self.desiredMissionOrientation = [True, self.desiredMoveYaw, ahrsData[0], ahrsData[1], ahrsData[2], self.lockedPosition[0], self.lockedPosition[1], self.desiredMoveDepth]
else:
print "Orientation Lock"
self.thrusterPWMs = self.MovementController.advancedMove(
poseData, axis0, self.desiredMoveDepth,
-axis1, self.desiredMovePitch,
self.desiredMoveYaw, self.desiredMoveRoll
) #By not updating the ahrs value, it allows the last values from the ahrs before the button was pushed to be my new baseline
if hats[0][
0] != 0: #Have the ability to increment yaw while orientation is locked
if self.quickButtonPressYaw == True: #Eliminates Schmitt trigger effect
self.desiredMoveYaw = (
(self.desiredMoveYaw +
hats[0][0] * 5)) % 360
controllerYawButtonReleaseTimer.restartTimer(
)
self.quickButtonPressYaw = False
#print self.desiredMoveYaw, "yaw inc"
elif hats[0][0] == 0:
netControllerYawButtonReleaseTimer = controllerYawButtonReleaseTimer.netTimer(
controllerYawButtonReleaseTimer.
cpuClockTimeInSeconds())
if netControllerYawButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressYaw = True
controllerYawButtonReleaseTimer.restartTimer(
)
if buttons[0] or buttons[
3] != 0: #Have the ability to increment pitch while orientation is locked
if self.quickButtonPressPitch == True: #Eliminates Schmitt trigger effect
self.desiredMovePitch = self.desiredMovePitch + buttons[
3] + (-1 * buttons[0])
controllerPitchButtonReleaseTimer.restartTimer(
)
self.quickButtonPressPitch = False
#print self.desiredMoveYaw, "yaw inc"
elif buttons[0] == 0 and buttons[3] == 0:
netControllerPitchButtonReleaseTimer = controllerPitchButtonReleaseTimer.netTimer(
controllerPitchButtonReleaseTimer.
cpuClockTimeInSeconds())
if netControllerPitchButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressPitch = True
controllerPitchButtonReleaseTimer.restartTimer(
)
if buttons[2] or buttons[
1] != 0: #Have the ability to increment roll while orientation is locked
if self.quickButtonPressRoll == True: #Eliminates Schmitt trigger effect
self.desiredMoveRoll = self.desiredMoveRoll + buttons[
2] + (-1 * buttons[1])
controllerRollButtonReleaseTimer.restartTimer(
)
self.quickButtonPressRoll = False
#print self.desiredMoveYaw, "yaw inc"
elif buttons[2] == 0 and buttons[1] == 0:
netControllerRollButtonReleaseTimer = controllerRollButtonReleaseTimer.netTimer(
controllerRollButtonReleaseTimer.
cpuClockTimeInSeconds())
if netControllerRollButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressRoll = True
controllerRollButtonReleaseTimer.restartTimer(
)
#Position Lock
elif self.toggleLeftBumper == True and self.toggleRightBumper == False: # Position Lock
#print self.lockedPosition
poseData = [
ahrsData[0], ahrsData[1], ahrsData[2],
self.dvlGuiData[0][1],
self.medianExternalDepth, self.dvlGuiData[0][0]
]
print "Position Lock"
if self.subInPosition == False:
self.thrusterPWMs, error, yawDesired = self.lockedMoveController.advancedMove(
poseData, self.lockedPosition[1],
self.desiredMoveDepth,
self.lockedPosition[0],
self.desiredMovePitch, self.desiredMoveYaw,
self.desiredMoveRoll, 0,
[False, axis4, axis3, -axis2])
else:
self.thrusterPWMs, error, yawDesired = self.lockedMoveController.advancedMove(
poseData, self.lockedPosition[1],
self.desiredMoveDepth,
self.lockedPosition[0],
self.desiredMovePitch, self.desiredMoveYaw,
self.desiredMoveRoll, 0,
[True, axis4, axis3, -axis2])
xError, yError, zError = error[0], error[1], error[
2]
pitchError, yawError, rollError = error[3], error[
4], error[5]
if abs(xError) < 1 and abs(yError) < 1 and abs(
zError) < 1:
self.subInPosition = True
else:
self.subInPosition = False
#Orientation and Position Lock
elif self.toggleRightBumper == True and self.toggleLeftBumper == True:
#print "Combo!!!!!"
poseData = [
ahrsData[0], ahrsData[1], ahrsData[2],
self.dvlGuiData[0][1],
self.medianExternalDepth, self.dvlGuiData[0][0]
]
self.thrusterPWMs, error, yawDesired = self.MovementController.advancedMove(
poseData, self.lockedPosition[1],
self.desiredMoveDepth, self.lockedPosition[0],
self.desiredMovePitch, self.desiredMoveYaw,
self.desiredMoveRoll, 0)
if hats[0][
0] != 0: #Have the ability to increment yaw while orientation is locked
if self.quickButtonPressYaw == True: #Eliminates Schmitt trigger effect
self.desiredMoveYaw = (
(self.desiredMoveYaw +
hats[0][0] * 5)) % 360
controllerYawButtonReleaseTimer.restartTimer(
)
self.quickButtonPressYaw = False
#print self.desiredMoveYaw, "yaw inc"
elif hats[0][0] == 0:
netControllerYawButtonReleaseTimer = controllerYawButtonReleaseTimer.netTimer(
controllerYawButtonReleaseTimer.
cpuClockTimeInSeconds())
if netControllerYawButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressYaw = True
controllerYawButtonReleaseTimer.restartTimer(
)
if buttons[0] or buttons[
3] != 0: #Have the ability to increment pitch while orientation is locked
if self.quickButtonPressPitch == True: #Eliminates Schmitt trigger effect
self.desiredMovePitch = self.desiredMovePitch + buttons[
3] + (-1 * buttons[0])
controllerPitchButtonReleaseTimer.restartTimer(
)
self.quickButtonPressPitch = False
#print self.desiredMoveYaw, "yaw inc"
elif buttons[0] == 0 and buttons[3] == 0:
netControllerPitchButtonReleaseTimer = controllerPitchButtonReleaseTimer.netTimer(
controllerPitchButtonReleaseTimer.
cpuClockTimeInSeconds())
if netControllerPitchButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressPitch = True
controllerPitchButtonReleaseTimer.restartTimer(
)
if buttons[2] or buttons[
1] != 0: #Have the ability to increment roll while orientation is locked
if self.quickButtonPressRoll == True: #Eliminates Schmitt trigger effect
self.desiredMoveRoll = self.desiredMoveRoll + buttons[
2] + (-1 * buttons[1])
controllerRollButtonReleaseTimer.restartTimer(
)
self.quickButtonPressRoll = False
#print self.desiredMoveYaw, "yaw inc"
elif buttons[2] == 0 and buttons[1] == 0:
netControllerRollButtonReleaseTimer = controllerRollButtonReleaseTimer.netTimer(
controllerRollButtonReleaseTimer.
cpuClockTimeInSeconds())
if netControllerRollButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressRoll = True
controllerRollButtonReleaseTimer.restartTimer(
)
#self.desiredMissionOrientation = [True, self.desiredMoveYaw, ahrsData[0], ahrsData[1], ahrsData[2], self.lockedPosition[0], self.lockedPosition[1], self.desiredMoveDepth]
#NORMAL OPERATION
elif self.toggleRightBumper == False and self.toggleLeftBumper == False:
self.thrusterPWMs = self.MovementController.move(
self.externalCommThread.position[2], axis0,
self.desiredMoveDepth, -axis1, axis3, axis4,
-axis2)
self.desiredMoveYaw, self.desiredMovePitch, self.desiredMoveRoll = self.externalCommThread.orientation[
0], self.externalCommThread.orientation[
1], self.externalCommThread.orientation[2]
self.desiredMissionOrientation = [
False, 0, 0, 0, 0, 0, 0, 0
]
print "Under normal Operation, desired epth is: ", self.desiredMoveDepth
return
'''
This block of code is only called when NOT in manual control mode. It dictates the actuall movement of the sub when in autonomous mode
'''
while self.running and missionIndex < len(self.missionList):
currentOrientation = [0, 0, 0]
currentLocation = [0, 0, 0]
mission = self.missionList[missionIndex]
if mission != self.currentMission:
self.sendDebugMessage("Starting Mission: " +
mission.parameters["name"])
self.currentMission = mission
maxTime = int(mission.parameters["maxTime"])
startTime = time.time()
#Initalize a bunch of mission dependant stuff
mission.startTime = startTime
mission.setMovementController(
self.MovementController
) #Sets the movement controller so the mission can move on its own
if not self.debug:
currentLocation = self.externalCommThread.position
currentOrientation = self.externalCommThread.orientation
mission.updateLocation(currentLocation, currentOrientation)
mission.initalizeOnMissionStart()
#while ((startTime - time.time()) <= maxTime or self.missionDebug==True) and self.running:
while self.running and ((time.time() - startTime) <= maxTime):
if self.changeMission == True and missionIndex <= len(
self.missionList) - 1:
self.changeMission = None
missionIndex += 1
break #Break out and load new mission
elif self.changeMission == False and missionIndex >= 0:
self.changeMission = None
missionIndex -= 1
break #Break out and load new mission
if not self.debug:
#This is where it will get position and orietnation data when the gui is not in debug mode
currentLocation = self.externalCommThread.position
currentOrientation = self.externalCommThread.orientation
mission.updateLocation(currentLocation, currentOrientation)
mission.updateDetectionData(
self.externalCommThread.detectionData)
else:
self.missionDebug = True
#This is where we call the mission code to run
missionReturnedValue = mission.update()
if missionReturnedValue == 1: #Mission completed
missionIndex += 1
break
if (time.time() - startTime) > maxTime:
missionIndex += 1
self.sendDebugMessage("Finished Run")
if self.debug == False:
self.killThrusters()
import sparton_ahrs
import externalLoggingSystem
import glob
import displayArduino
import pressureArduino
import motherBoard as motherboard
import killSwitch
useMaestro = True
advM = utilities.AdvancedMath()
e1 = advM.e1 # Unit vector for x
e2 = advM.e2 # Unit vector for y
e3 = advM.e3 # Unit vector for z
controllerTimer = utilities.Timer()
controllerYawButtonReleaseTimer = utilities.Timer()
controllerDepthButtonReleaseTimer = utilities.Timer()
class ExternalComm(QtCore.QObject):
"""
Part of the GUI thread. This class acts as an intermediary for the gui thread
and the embedded thread. Use this class to pull data from the external devices
then store that data in the class instances. The gui classes can then call those
instances.
"""
def __init__(self, mainWindow):
QtCore.QObject.__init__(self)
self.mainWindowClass = mainWindow
self.isDebug = True
.. module:: external_sparton_ahrs
:synopsis: Sparton Altitude Heading Reference System (AHRS) device communication.
:Author: Austin Owens <*****@*****.**>
:Date: Created on May 28, 2014
:Description: This module contains the commands for all the functions that can be called with the legacy protocol for the Sparton AHRS
'''
import serial
import threading
import time
import struct
import lib.Utils.utilities as utilities
reqestTimer = utilities.Timer()
class SpartonAhrsDataPacket():
'''
This class contains setters and getters commands for the Sparton AHRS in Legacy format. It only handles the sending values.
'''
def __init__(self, comPort):
'''
Initializes the Sparton AHRS and array of location values.
**Parameters**: \n
* **comPort** - Serial port number that the Sparton AHRS is connected to.
**Return**: \n
* **No Return.**\n
Copyright 2014, Mechatronics, All rights reserved.
.. module:: microcontroller_sib
:synopsis: Defines the various movement options for the Sub.
:Author: Mechatronics <*****@*****.**>
:Date: Created on Mar 29, 2015
:Description: Contains movement options and PID controllers for moving the based on position, way, pitch, and roll.
'''
import lib.Utils.utilities as utilities
import time
import math
import struct
import numpy as np
sendTCBDataTimer = utilities.Timer()
noTranslationTimer = utilities.Timer()
initialTime = time.time()
#TCB = microcontroller.MicrocontrollerDataPackets()
advM = utilities.AdvancedMath()
e1 = advM.e1
e2 = advM.e2
e3 = advM.e3
class BrushedThruster():
def __init__(self, motorID, orientation, location):
'''
Orients the sub based on the rectangular orange path.
:Author: Felipe Jared Guerrero Moreno <*****@*****.**>
:Date: Created on Jul 14, 2017
:Description: This module sets up communication with the electrical boards.
'''
import datetime
import time
import numpy
import struct
import threading
import math
import data_packet_generator
import lib.Utils.utilities as utilities
import serial
requestTimer = utilities.Timer()
wcbTimer = utilities.Timer()
class motherBoardDataPackets():
def __init__(self, serialObject):
'''
Sends messages to the Motherboard when necessary.
The Frame HEX is hard coded and needs to be adjusted if changed.
'''
self.motherCom = serialObject
def sendWeapon1Command(self):
messageID = int('0x7A', 0) # 0000 0111 1010
byte0 = int('0xEE', 0) # Starting byte, EE in hex
def run(self):
self.sendDebugMessage("Starting...")
missionIndex = 0
sentFlagMessage = False
self.running = True
waypointError = None
'''
This block of code is only called when in manual control mode, and there for does not use the mission code
'''
if self.manualMode == True and self.debug == False:
controllerData = None
self.pressedDepthChange = False
self.desiredMoveDepth = 0
controllerDepthButtonReleaseTimer = utilities.Timer()
self.toggleLeftBumper = False
self.toggleRightBumper = False
self.quickButtonPressDepth = True
controllerInputTimer = time.time()
while self.running:
if abs(time.time() - controllerInputTimer) > .5:
self.killThrusters()
while len(self.joystickController.getList) > 0:
controllerInputTimer = time.time()
controllerData = self.joystickController.getList.pop(0)
xPwm = 0
zPwm = 0
yDesiredRotation = 0
xDesiredRotation = 0
poseData = copy.copy(self.externalCommThread.orientation)
poseData.append(self.externalCommThread.dvlData[3])
if controllerData == None:
print "No Controller Data"
self.killAllThrusters()
continue
else:
#print controllerData
joystickInput = controllerData[1]
buttonInput = controllerData[2]
buttons = buttonInput
hats = controllerData[3]
if buttonInput[4] == 1 and self.pressedDepthChange == False:
self.manualDepthValue -= 1
self.pressedDepthChange = True
if buttonInput[5] == 1 and self.pressedDepthChange == False:
self.manualDepthValue += 1
self.pressedDepthChange = True
if buttonInput[4] == 0 and buttonInput[5] == 0:
self.pressedDepthChange = False
if buttonInput[7] == 1:
for i in range(0,9):
self.maestroSerial.write(bytearray([0xFF, i, 0x7F]))
maxSpeed = 80
xPwm = maxSpeed * joystickInput[0] * 2
zPwm = maxSpeed * joystickInput[1] * -2
poseData = copy.copy(self.externalCommThread.orientation)
poseData.append(self.externalCommThread.dvlData[3])
yDesiredRotation = maxSpeed/4 * -joystickInput[4]
xDesiredRotation = maxSpeed/4 * -joystickInput[3]
axis0, axis1, axis2, axis3, axis4 = int(joystickInput[0]*204), int(joystickInput[1]*204), int(joystickInput[2]*204), int(joystickInput[3]*204), int(joystickInput[4]*204)
#DEPTH
if hats[0][1] != 0: #Have the ability to increment depth
if self.quickButtonPressDepth == True: #Eliminates Schmitt trigger effect
self.desiredMoveDepth += -hats[0][1]*0.5
if self.desiredMoveDepth < -1:
self.desiredMoveDepth = -1
controllerDepthButtonReleaseTimer.restartTimer()
self.quickButtonPressDepth = False
#print self.desiredMoveDepth, "depth inc"
elif hats[0][1] == 0:
netControllerDepthButtonReleaseTimer = controllerDepthButtonReleaseTimer.netTimer(controllerDepthButtonReleaseTimer.cpuClockTimeInSeconds())
if netControllerDepthButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressDepth = True
controllerDepthButtonReleaseTimer.restartTimer()
#TOGGLE ORIENTATION LOCK
if buttons[5] == True:
if self.quickButtonPressOrientationLock == True: #Eliminates Schmitt trigger effect
self.toggleRightBumper = not self.toggleRightBumper
self.quickButtonPressOrientationLock = False
elif buttons[5] == False:
self.quickButtonPressOrientationLock = True
if buttons[4] == True:
if self.quickButtonPressPositionLock == True: #Eliminates Schmitt trigger effect
self.toggleLeftBumper = not self.toggleLeftBumper
self.quickButtonPressPositionLock = False
self.lockedPosition = self.dvlGuiData[0]
currentDepth = self.medianExternalDepth
elif buttons[4] == False:
self.quickButtonPressPositionLock = True
#ORIENTATION LOCK
if self.toggleRightBumper == True and self.toggleLeftBumper == False: #Orientation lock
if self.toggleLeftBumper == True:
pass
#print "Position and Orientation Lock"
#poseData = [ahrsData[0], ahrsData[1], ahrsData[2], self.dvlGuiData[0][1], self.medianExternalDepth, self.dvlGuiData[0][0]]
#self.thrusterPWMs, error, yawDesired = self.moveController.advancedMove(poseData, self.lockedPosition[1], self.desiredMoveDepth, self.lockedPosition[0], self.desiredMovePitch, self.desiredMoveYaw, self.desiredMoveRoll, 0)
#self.desiredMissionOrientation = [True, self.desiredMoveYaw, ahrsData[0], ahrsData[1], ahrsData[2], self.lockedPosition[0], self.lockedPosition[1], self.desiredMoveDepth]
else:
print "Orientation Lock"
self.thrusterPWMs = self.MovementController.advancedMove(poseData, axis0, self.desiredMoveDepth, -axis1, self.desiredMovePitch, self.desiredMoveYaw, self.desiredMoveRoll) #By not updating the ahrs value, it allows the last values from the ahrs before the button was pushed to be my new baseline
if hats[0][0] != 0: #Have the ability to increment yaw while orientation is locked
if self.quickButtonPressYaw == True: #Eliminates Schmitt trigger effect
self.desiredMoveYaw = ((self.desiredMoveYaw + hats[0][0]*5))%360
controllerYawButtonReleaseTimer.restartTimer()
self.quickButtonPressYaw = False
#print self.desiredMoveYaw, "yaw inc"
elif hats[0][0] == 0:
netControllerYawButtonReleaseTimer = controllerYawButtonReleaseTimer.netTimer(controllerYawButtonReleaseTimer.cpuClockTimeInSeconds())
if netControllerYawButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressYaw = True
controllerYawButtonReleaseTimer.restartTimer()
if buttons[0] or buttons[3] != 0: #Have the ability to increment pitch while orientation is locked
if self.quickButtonPressPitch == True: #Eliminates Schmitt trigger effect
self.desiredMovePitch = self.desiredMovePitch + buttons[3] + (-1*buttons[0])
controllerPitchButtonReleaseTimer.restartTimer()
self.quickButtonPressPitch = False
#print self.desiredMoveYaw, "yaw inc"
elif buttons[0] == 0 and buttons[3] == 0:
netControllerPitchButtonReleaseTimer = controllerPitchButtonReleaseTimer.netTimer(controllerPitchButtonReleaseTimer.cpuClockTimeInSeconds())
if netControllerPitchButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressPitch = True
controllerPitchButtonReleaseTimer.restartTimer()
if buttons[2] or buttons[1] != 0: #Have the ability to increment roll while orientation is locked
if self.quickButtonPressRoll == True: #Eliminates Schmitt trigger effect
self.desiredMoveRoll = self.desiredMoveRoll + buttons[2] + (-1*buttons[1])
controllerRollButtonReleaseTimer.restartTimer()
self.quickButtonPressRoll = False
#print self.desiredMoveYaw, "yaw inc"
elif buttons[2] == 0 and buttons[1] == 0:
netControllerRollButtonReleaseTimer = controllerRollButtonReleaseTimer.netTimer(controllerRollButtonReleaseTimer.cpuClockTimeInSeconds())
if netControllerRollButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressRoll = True
controllerRollButtonReleaseTimer.restartTimer()
#Position Lock
elif self.toggleLeftBumper == True and self.toggleRightBumper == False: # Position Lock
#print self.lockedPosition
poseData = [ahrsData[0], ahrsData[1], ahrsData[2], self.dvlGuiData[0][1], self.medianExternalDepth, self.dvlGuiData[0][0]]
print "Position Lock"
if self.subInPosition == False:
self.thrusterPWMs, error, yawDesired = self.lockedMoveController.advancedMove(poseData, self.lockedPosition[1], self.desiredMoveDepth, self.lockedPosition[0], self.desiredMovePitch, self.desiredMoveYaw, self.desiredMoveRoll, 0, [False, axis4, axis3, -axis2])
else:
self.thrusterPWMs, error, yawDesired = self.lockedMoveController.advancedMove(poseData, self.lockedPosition[1], self.desiredMoveDepth, self.lockedPosition[0], self.desiredMovePitch, self.desiredMoveYaw, self.desiredMoveRoll, 0, [True, axis4, axis3, -axis2])
xError, yError, zError = error[0], error[1], error[2]
pitchError, yawError, rollError = error[3], error[4], error[5]
if abs(xError) < 1 and abs(yError) < 1 and abs(zError) < 1:
self.subInPosition = True
else:
self.subInPosition = False
#Orientation and Position Lock
elif self.toggleRightBumper == True and self.toggleLeftBumper == True:
#print "Combo!!!!!"
poseData = [ahrsData[0], ahrsData[1], ahrsData[2], self.dvlGuiData[0][1], self.medianExternalDepth, self.dvlGuiData[0][0]]
self.thrusterPWMs, error, yawDesired = self.MovementController.advancedMove(poseData, self.lockedPosition[1], self.desiredMoveDepth, self.lockedPosition[0], self.desiredMovePitch, self.desiredMoveYaw, self.desiredMoveRoll, 0)
if hats[0][0] != 0: #Have the ability to increment yaw while orientation is locked
if self.quickButtonPressYaw == True: #Eliminates Schmitt trigger effect
self.desiredMoveYaw = ((self.desiredMoveYaw + hats[0][0]*5))%360
controllerYawButtonReleaseTimer.restartTimer()
self.quickButtonPressYaw = False
#print self.desiredMoveYaw, "yaw inc"
elif hats[0][0] == 0:
netControllerYawButtonReleaseTimer = controllerYawButtonReleaseTimer.netTimer(controllerYawButtonReleaseTimer.cpuClockTimeInSeconds())
if netControllerYawButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressYaw = True
controllerYawButtonReleaseTimer.restartTimer()
if buttons[0] or buttons[3] != 0: #Have the ability to increment pitch while orientation is locked
if self.quickButtonPressPitch == True: #Eliminates Schmitt trigger effect
self.desiredMovePitch = self.desiredMovePitch + buttons[3] + (-1*buttons[0])
controllerPitchButtonReleaseTimer.restartTimer()
self.quickButtonPressPitch = False
#print self.desiredMoveYaw, "yaw inc"
elif buttons[0] == 0 and buttons[3] == 0:
netControllerPitchButtonReleaseTimer = controllerPitchButtonReleaseTimer.netTimer(controllerPitchButtonReleaseTimer.cpuClockTimeInSeconds())
if netControllerPitchButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressPitch = True
controllerPitchButtonReleaseTimer.restartTimer()
if buttons[2] or buttons[1] != 0: #Have the ability to increment roll while orientation is locked
if self.quickButtonPressRoll == True: #Eliminates Schmitt trigger effect
self.desiredMoveRoll = self.desiredMoveRoll + buttons[2] + (-1*buttons[1])
controllerRollButtonReleaseTimer.restartTimer()
self.quickButtonPressRoll = False
#print self.desiredMoveYaw, "yaw inc"
elif buttons[2] == 0 and buttons[1] == 0:
netControllerRollButtonReleaseTimer = controllerRollButtonReleaseTimer.netTimer(controllerRollButtonReleaseTimer.cpuClockTimeInSeconds())
if netControllerRollButtonReleaseTimer >= 0.01: #Need to have let go of it for at least 10 miliseconds to register as button not being pressed
self.quickButtonPressRoll = True
controllerRollButtonReleaseTimer.restartTimer()
#self.desiredMissionOrientation = [True, self.desiredMoveYaw, ahrsData[0], ahrsData[1], ahrsData[2], self.lockedPosition[0], self.lockedPosition[1], self.desiredMoveDepth]
#NORMAL OPERATION
elif self.toggleRightBumper == False and self.toggleLeftBumper == False:
self.thrusterPWMs = self.MovementController.move(self.externalCommThread.position[2], axis0, self.desiredMoveDepth, -axis1, axis3, axis4, -axis2)
print "PWM", self.thrusterPWMs
self.desiredMoveYaw, self.desiredMovePitch, self.desiredMoveRoll = self.externalCommThread.orientation[0],self.externalCommThread.orientation[1],self.externalCommThread.orientation[2]
self.desiredMissionOrientation = [False, 0, 0, 0, 0, 0, 0, 0]
print "Under normal Operation, desired epth is: ", self.desiredMoveDepth
#self.MovementController.move(poseData[3], xPwm, poseData[3], zPwm, xDesiredRotation, yDesiredRotation, 0)
#self.MovementController.move(poseData[3], xPwm, self.manualDepthValue, zPwm, xDesiredRotation, yDesiredRotation, 0)
# try:
# self.MovementController.move(poseData[3], xPwm, poseData[3], zPwm, xDesiredRotation, yDesiredRotation, 0)
# except:
# print "Something Went Wrong:"
# print poseData[3], xPwm, poseData[3], zPwm, xDesiredRotation, yDesiredRotation, 0
#self.MovementController.advancedMove(poseData, 3,0,0, poseData[1], poseData[0], poseData[2])
print "Finished Manual Controll Loop"
return
'''
This block of code is only called when NOT in manual control mode. It dictates the actuall movement of the sub when in autonomous mode
'''
while missionIndex < len(self.missionList) and self.running:
currentOrientation = [0,0,0]
currentLocation = [0,0,0]
mission = self.missionList[missionIndex]
if mission != self.currentMission:
print ("Starting Mission: " + mission.parameters["name"])
self.currentMission = mission
maxTime = int(mission.parameters["maxTime"])
startTime = time.time()
#Modify the General Waypoint if it is set to use the same waypoint as another mission
if mission.parameters["useGeneralWaypoint"] != None:
print "Using Waypoint: ", mission.parameters["useGeneralWaypoint"]
for i,v in enumerate(self.missionList):
if v.name == mission.parameters["useGeneralWaypoint"]:
mission.generalWaypoint = v.lastKnownPosition
break
#while ((startTime - time.time()) <= maxTime or self.missionDebug==True) and self.running:
while ((time.time()-startTime) <= maxTime) and self.running:
#print mission.name
if self.changeMission == True and missionIndex <= len(self.missionList)-1:
self.changeMission = None
missionIndex +=1
break
elif self.changeMission == False and missionIndex >=0:
self.changeMission = None
missionIndex -= 1
break
if mission.resetFlagBoolean == True:
sentFlagMessage = False
mission.resetFlagBoolean = False
#Check for Socket Data from Computer Vision Process
# NEED TO WRITE THIS CODE
#socket.listen or something like that
#This array will contain the position and orientation of the obstacle relative to the sub
obstacleLocation = []
#Collect Sensor Data
if self.changeFlag != None:
sentFlagMessage = False
if self.changeFlag == True:
if mission.readyToExecute:
mission.executed = True
if mission.reachedSpecificWaypoint:
mission.readyToExecute = True
if mission.locatedObstacles:
mission.reachedSpecificWaypoint = True
if mission.reachedGeneralWaypoint:
mission.locatedObstacles = True
mission.reachedGeneralWaypoint = True
elif self.changeFlag == False:
if mission.locatedObstacles == False:
mission.reachedGeneralWaypoint = False
if mission.reachedSpecificWaypoint == False:
mission.locatedObstacles = False
if mission.readyToExecute == False:
mission.reachedSpecificWaypoint = False
if mission.executed == False:
mission.readyToExecute = False
mission.readyToExecute= False
self.changeFlag = None
if not self.debug:
#This is where it will get position and orietnation data when the gui is not in debug mode
if mission.parameters["useKalmanFilter"] == True:
currentLocation = self.externalCommThread.position
currentOrientation = self.externalCommThread.orientation
else:
currentLocation = self.externalCommThread.position
currentOrientation = self.externalCommThread.orientation
else:
self.missionDebug = True
#Check each flag to see if the mission is ready to move on
if not mission.reachedGeneralWaypoint:
#print "Haven't reached general waypoint"
reachedWaypoint = True
#Sends a message saying which flag it is on, but only once
if not sentFlagMessage:
self.sendDebugMessage("Begining General Waypoint")
print "Begining new waypoint"
sentFlagMessage = True
if "ignoreGeneralWaypoint" in mission.parameters:
#print "variable is set"
if mission.parameters["ignoreGeneralWaypoint"] == "True":
#print "Actually ignored it"
self.reachedGeneralWaypoint = True
continue
if not self.debug:
if waypointError != None:
#print "Waypont Error: ", waypointError
#print "Genral Distance Error", mission.generalDistanceError
#print "Orientation error", mission.generalRotationError
if abs(waypointError[0]) < mission.generalDistanceError and abs(waypointError[1]) < mission.generalDistanceError and abs(waypointError[2]) < mission.generalDistanceError:
pass #This will only be called if we are actually at the waypoint in terms of position
else:
reachedWaypoint = False
if abs(waypointError[3]) < mission.generalRotationError and abs(waypointError[4]) <mission.generalRotationError and abs(waypointError[5]) < mission.generalRotationError:
pass #Only will be called if we have the correct orientaiton
#print "Not there orientation"
else:
reachedWaypoint = False
else:
print "Waypoint error was weird"
reachedWaypoint = False
#print "Waypoint Status: ", reachedWaypoint
if reachedWaypoint and not self.missionDebug:
#print "Reahed the waypoint"
if mission.parameters["startTime"] == 0:
print "Reseting time"
mission.parameters["startTime"] = time.time()
if time.time() - mission.parameters["startTime"] >= int(mission.parameters["waitTime"]):
mission.reachedGeneralWaypoint = True
mission.parameters["startTime"] = 0
print "Finished General Waypoint"
self.sendDebugMessage("Reached General Waypoint")
self.relativeMoveWaypoint = None
sentFlagMessage = False
'''
At this point we would tell the neural network to look for the object
socket.send("Look for: " + mission.obstacleName)
'''
continue
else:
#print time.time() - mission.parameters["startTime"]
pass
#If we haven't reached the waypoint and the gui isn't in debug mode, then attempt to move to the general waypoint
if not self.debug:
if mission.parameters["useRelativeWaypoint"] == "True":
print "Using relative waypoint"
if self.relativeMoveWaypoint == None:
print "Going to: ", mission.generalWaypoint
pose, n, e, u, x, y, z = self.MovementController.relativeMove(currentOrientation+currentLocation,None, None, None, None, None, None, mission.generalWaypoint[1], mission.generalWaypoint[2], mission.generalWaypoint[0],
mission.generalWaypoint[4], mission.generalWaypoint[3], 0)
self.relativeMoveWaypoint = [n, e, u, y, x, z]
#print "relative Waypoint: ",self.relativeMoveWaypoint
waypointError = self.MovementController.advancedMove(currentOrientation + currentLocation, self.relativeMoveWaypoint[0], self.relativeMoveWaypoint[1], mission.generalWaypoint[2], self.relativeMoveWaypoint[4], self.relativeMoveWaypoint[3], self.relativeMoveWaypoint[5])[1]
elif mission.parameters["useRelativeWorld"] == "True":
print "Using relative world"
if self.relativeMoveWaypoint == None:
pose, n, e, u, x, y, z = self.MovementController.relativeMove(currentOrientation + currentLocation, mission.generalWaypoint[0], mission.generalWaypoint[1], mission.generalWaypoint[2],
mission.generalWaypoint[4], mission.generalWaypoint[3], 0)
self.relativeMoveWaypoint = [n,e,u,y,x,z]
waypointError = self.MovementController.advancedMove(currentOrientation + currentLocation, self.relativeMoveWaypoint[0], self.relativeMoveWaypoint[1], mission.generalWaypoint[2], self.relativeMoveWaypoint[4], self.relativeMoveWaypoint[3], self.relativeMoveWaypoint[5])[1]
else:
#print "Using absolute"
waypointError = self.MovementController.advancedMove(currentOrientation+currentLocation, mission.generalWaypoint[0], mission.generalWaypoint[1], mission.generalWaypoint[2],
mission.generalWaypoint[4], mission.generalWaypoint[3], mission.generalWaypoint[5], mission.parameters["drivingMode"])[1]
#print "Waypoint Error: ", waypointError
continue
#At this point it is suppose to turn until it locates the obstacle it is looking for
#The obstacle data is coming in from the neural network process
if not mission.locatedObstacles:
if not sentFlagMessage:
self.sendDebugMessage("Begining Object Detection")
sentFlagMessage = True
if not self.debug:
foundObstacles = mission.lookForObstacles(self.externalCommThread, self.MovementController)
if foundObstacles and not self.missionDebug:
print "Found obstacles"
mission.locatedObstacles = True
sentFlagMessage = False
continue
#Work with the neural network process and dynamically change the specificWaypoint to get
#right in front of the obstacle or where ever we need to be
if not mission.reachedSpecificWaypoint:
#Sends a message saying which flag it is on, but only once
if not sentFlagMessage:
self.sendDebugMessage("Begining Specific Waypoint")
sentFlagMessage = True
reachedWaypoint = True
if not self.debug:
reachedWaypoint = mission.calculateSpecificWaypoint(self.externalCommThread, self.MovementController)
'''
if not self.debug:
for n,p in enumerate(currentLocation):
if not ((p + mission.specificDistanceError >= mission.specificWaypoint[n]) and (p - mission.specificDistanceError <= mission.specificWaypoint[n])):
reachedWaypoint = False
for n, p in enumerate(currentOrientation):
if not ((p - mission.specificRotationError <= mission.specificWaypoint[n+3]) and (p + mission.specificRotationError >= mission.specificWaypoint[n +3])):
reachedWaypoint = False
'''
if reachedWaypoint and not self.missionDebug:
mission.reachedSpecificWaypoint = True
sentFlagMessage = False
continue
#Do last minute alignment checks to make sure that we are ready to execute
if not mission.readyToExecute:
#Sends a message saying which flag it is on, but only once
if not sentFlagMessage:
self.sendDebugMessage("Begining Ready to Execute")
sentFlagMessage = True
if not self.debug:
readyToExecute = mission.getExecutionPositionDifference(self.externalCommThread, self.MovementController)
if readyToExecute and not self.missionDebug:
mission.readyToExecute = True
sentFlagMessage = False
continue
continue
#Once ready to execute, EXECUTE!
elif not mission.executed:
if not self.debug:
mission.executionTask(self.externalCommThread, self.MovementController)
self.sendDebugMessage("Executed!")
mission.executed = True
mission.success = True
missionIndex += 1
break
if (time.time() - startTime) > maxTime:
missionIndex += 1
self.sendDebugMessage("Finished Run")
if self.debug == False:
self.killThrusters()
