Exemple #1
0
    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
Exemple #2
0
    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()
Exemple #3
0
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
Exemple #4
0
.. 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
Exemple #5
0
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.
Exemple #6
0
: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
Exemple #7
0
    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()