Esempio n. 1
0
def main():
    sw.loadConfig("../conf/seawolf.conf")
    sw.init("Serial : IMU")

    sw.var.setAutoNotify(False)

    # setup
    device_real = sys.argv[1]
    imu = IMU()

    # Open and initialize the serial port */
    imu.connect(device_real)

    # start stream for continuous data
    imu.startStream()

    q = 0
    while q == 0:
        try:
            # Report values to seawolf
            (yaw, pitch, roll) = imu.getHeading()
            sw.var.set("SEA.Roll", roll)
            sw.var.set("SEA.Pitch", pitch)
            sw.var.set("SEA.Yaw", yaw)
            sw.notify.send("UPDATED", "IMU")
        except KeyboardInterrupt:
            imu.stopStream()
            imu.disconnect()
            q = 1

    # close application if outside mainloop
    sw.close()
Esempio n. 2
0
def main(args):
    # Connect to hub
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Thruster Test")

    # Default test function
    default = test_thruster_constant

    # Arguments
    opts = {"const": test_thruster_constant, "range": test_thruster_over_range}

    # If the name of a test function was provided by the user as an argument, use it
    func = opts.get(args[0], default) if args else default

    print("Running test", func.__name__, "\n")

    # Run the chosen test function against all the thrusters
    try:
        for thruster in THRUSTERS:
            # test_thruster_over_range(thruster, .3, -.3, -.1)
            func(thruster, duration=4)
            print()

    # Safely exit if CTRL+C
    except (KeyboardInterrupt, AssertionError) as e:
        print()
        print(e)
        print("Setting all thrusters to 0")
        for thruster in THRUSTERS:
            seawolf.var.set(thruster, 0.0)

    seawolf.close()
Esempio n. 3
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Pitch PID")

    seawolf.var.subscribe("PitchPID.p")
    seawolf.var.subscribe("PitchPID.i")
    seawolf.var.subscribe("PitchPID.d")
    seawolf.var.subscribe("PitchPID.Heading")
    seawolf.var.subscribe("PitchPID.Paused")
    seawolf.var.subscribe("SEA.Pitch")

    pitch = seawolf.var.get("SEA.Pitch")
    paused = seawolf.var.get("PitchPID.Paused")
    pid = seawolf.PID(seawolf.var.get("PitchPID.Heading"),
                      seawolf.var.get("PitchPID.p"),
                      seawolf.var.get("PitchPID.i"),
                      seawolf.var.get("PitchPID.d"))

    # set active region (region where response of the robot
    # is practically linear). Outside this region, thrusters
    # would be maxed out, and the ITerm would get staturated.
    # Outside this region, the we use PD control. Inside this
    # region, we use PID control.
    pid.setActiveRegion(ACTIVE_REGION_SIZE)

    dataOut(0.0)
    mv = 0.0

    while (True):
        seawolf.var.sync()
        if (seawolf.var.stale("SEA.Pitch") and paused == False):
            pitch = seawolf.var.get("SEA.Pitch")
            mv = pid.update(pitch)
        elif (seawolf.var.stale("PitchPID.p")
              or seawolf.var.stale("PitchPID.i")
              or seawolf.var.stale("PitchPID.d")):
            pid.setCoefficients(seawolf.var.get("PitchPID.p"),
                                seawolf.var.get("PitchPID.i"),
                                seawolf.var.get("PitchPID.d"))
            pid.resetIntegral()  #init e dt
        elif (seawolf.var.poked("PitchPID.Heading")):
            pid.setSetPoint(seawolf.var.get("PitchPID.Heading"))
            mv = pid.update(seawolf.var.get("SEA.Pitch"))
            if (paused):
                seawolf.var.set("PitchPID.Paused", 0.0)
        elif (seawolf.var.stale("PitchPID.Paused")):
            p = seawolf.var.get("PitchPID.Paused")
            if (p == paused):
                continue

            paused = p
            if (paused):
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Pitch")
                pid.pause()

        if (paused == False):
            dataOut(mv)

    seawolf.close()
Esempio n. 4
0
def main():
    sw.loadConfig("../conf/seawolf.conf")
    sw.init("Serial : IMU")

    sw.var.setAutoNotify(False)

    # setup
    device_real = sys.argv[1]
    imu = IMU()

    # Open and initialize the serial port */
    imu.connect(device_real)

    # start stream for continuous data
    imu.startStream()

    q = 0
    while q == 0:
        try:
            # Report values to seawolf
            (yaw, pitch, roll) = imu.getHeading()
            sw.var.set("SEA.Roll", roll)
            sw.var.set("SEA.Pitch", pitch)
            sw.var.set("SEA.Yaw", yaw)
            sw.notify.send("UPDATED", "IMU")
        except KeyboardInterrupt:
            imu.stopStream()
            imu.disconnect()
            q = 1

    # close application if outside mainloop
    sw.close()
Esempio n. 5
0
def main(args):
    # Connect to hub
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Thruster Test")

    # Default test function
    default = test_thruster_constant

    # Arguments
    opts = {
        "const": test_thruster_constant,
        "range": test_thruster_over_range
    }

    # If the name of a test function was provided by the user as an argument, use it
    func = opts.get(args[0], default) if args else default

    print("Running test", func.__name__, "\n")

    # Run the chosen test function against all the thrusters
    try:
        for thruster in THRUSTERS:
            # test_thruster_over_range(thruster, .3, -.3, -.1)
            func(thruster, duration=4)
            print()

    # Safely exit if CTRL+C
    except (KeyboardInterrupt, AssertionError) as e:
        print()
        print(e)
        print("Setting all thrusters to 0")
        for thruster in THRUSTERS:
            seawolf.var.set(thruster, 0.0)

    seawolf.close()
Esempio n. 6
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Yaw PID")

    seawolf.var.subscribe("YawPID.p")
    seawolf.var.subscribe("YawPID.i")
    seawolf.var.subscribe("YawPID.d")
    seawolf.var.subscribe("YawPID.Heading")
    seawolf.var.subscribe("YawPID.Paused")
    seawolf.var.subscribe("SEA.Yaw")

    paused = seawolf.var.get("YawPID.Paused")
    heading = seawolf.var.get("YawPID.Heading")

    pid = seawolf.PID(
        0.0,
        seawolf.var.get("YawPID.p"),
        seawolf.var.get("YawPID.i"),
        seawolf.var.get("YawPID.d")
    )

    dataOut(0.0)

    while(True):

        seawolf.var.sync()

        if seawolf.var.stale("SEA.Yaw"):
            yaw = seawolf.var.get("SEA.Yaw")

        if (seawolf.var.stale("YawPID.p") or seawolf.var.stale("YawPID.i") or seawolf.var.stale("YawPID.d")):

            pid.setCoefficients(
                seawolf.var.get("YawPID.p"),
                seawolf.var.get("YawPID.i"),
                seawolf.var.get("YawPID.d")
            )
            pid.resetIntegral()

        if (seawolf.var.poked("YawPID.Heading")):
            heading = seawolf.var.get("YawPID.Heading")

            if paused:
                seawolf.var.set("YawPID.Paused", 0.0)

        if (seawolf.var.stale("YawPID.Paused")):
            paused = seawolf.var.get("YawPID.Paused")

            if paused:
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Yaw")
                pid.pause()

        if not paused:
            mv = pid.update(angleError(heading, yaw))
            dataOut(mv)

    seawolf.close()
Esempio n. 7
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Roll PID")

    seawolf.var.subscribe("RollPID.p")
    seawolf.var.subscribe("RollPID.i")
    seawolf.var.subscribe("RollPID.d")
    seawolf.var.subscribe("RollPID.Heading")
    seawolf.var.subscribe("RollPID.Paused")
    seawolf.var.subscribe("SEA.Roll")

    roll = seawolf.var.get("SEA.Roll")
    paused = seawolf.var.get("RollPID.Paused")
    pid = seawolf.PID(
        seawolf.var.get("RollPID.Heading"),
        seawolf.var.get("RollPID.p"),
        seawolf.var.get("RollPID.i"),
        seawolf.var.get("RollPID.d")
    )

    dataOut(0.0)
    mv = 0.0

    while(True):
        seawolf.var.sync()

        if seawolf.var.stale("SEA.Roll"):
            roll = seawolf.var.get("SEA.Roll")

        if seawolf.var.poked("RollPID.Heading"):
            pid.setSetPoint(seawolf.var.get("RollPID.Heading"))

            if paused:
                seawolf.var.set("RollPID.Paused", 0.0)

        if (seawolf.var.stale("RollPID.p") or seawolf.var.stale("RollPID.i") or seawolf.var.stale("RollPID.d")):

            pid.setCoefficients(
                seawolf.var.get("RollPID.p"),
                seawolf.var.get("RollPID.i"),
                seawolf.var.get("RollPID.d")
            )
            pid.resetIntegral()  # init e dt

        if (seawolf.var.stale("RollPID.Paused")):
            paused = seawolf.var.get("RollPID.Paused")

            if paused:
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Roll")
                pid.pause()

        if not paused:
            mv = pid.update(roll)
            dataOut(mv)

    seawolf.close()
Esempio n. 8
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Depth PID")

    seawolf.var.subscribe("DepthPID.p")
    seawolf.var.subscribe("DepthPID.i")
    seawolf.var.subscribe("DepthPID.d")
    seawolf.var.subscribe("DepthPID.Heading")
    seawolf.var.subscribe("DepthPID.Paused")
    seawolf.var.subscribe("Depth")

    depth = seawolf.var.get("Depth")
    paused = seawolf.var.get("DepthPID.Paused")

    pid = seawolf.PID( seawolf.var.get("DepthPID.Heading"), seawolf.var.get("DepthPID.p"), seawolf.var.get("DepthPID.i"), seawolf.var.get("DepthPID.d"))

    e_dt = initial_e_dt( seawolf.var.get("DepthPID.i") )
    dataOut(0.0)

    while(True):
        seawolf.var.sync()

        if(seawolf.var.stale("Depth")):
            depth = seawolf.var.get("Depth")

        if(seawolf.var.stale("DepthPID.p") or seawolf.var.stale("DepthPID.i") or seawolf.var.stale("DepthPID.d")):
            pid.setCoefficients(seawolf.var.get("DepthPID.p"), seawolf.var.get("DepthPID.i"), seawolf.var.get("DepthPID.d"))

            e_dt = initial_e_dt( seawolf.var.get("DepthPID.i") )

        if(seawolf.var.poked("DepthPID.Heading")):
            pid.setSetPoint(seawolf.var.get("DepthPID.Heading"))
            if(paused):
                seawolf.var.set("DepthPID.Paused", 0.0)
                seawolf.var.set("PitchPID.Paused", 0.0)

        if(seawolf.var.stale("DepthPID.Paused")):
            paused = seawolf.var.get("DepthPID.Paused")
            if(paused):
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Depth")
                pid.pause()
                e_dt = initial_e_dt( seawolf.var.get("DepthPID.i") )

        if(depth > panic_depth):
            seawolf.logging.log(CRITICAL, "Depth: {}\n".format(depth))
            seawolf.logging.log(CRITICAL, "I'm too deep! Rising full force!\n")

            dataOut(-1.0)
            time.sleep(panic_time)
        elif(paused == False):
            mv = pid.update(depth)
            mv = in_range(-thruster_cap, mv, thruster_cap)
            dataOut(mv)

    seawolf.close();
Esempio n. 9
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Roll PID")

    seawolf.var.subscribe("RollPID.p")
    seawolf.var.subscribe("RollPID.i")
    seawolf.var.subscribe("RollPID.d")
    seawolf.var.subscribe("RollPID.Heading")
    seawolf.var.subscribe("RollPID.Paused")
    seawolf.var.subscribe("SEA.Roll")

    roll = seawolf.var.get("SEA.Roll")
    paused = seawolf.var.get("RollPID.Paused")
    pid = seawolf.PID(seawolf.var.get("RollPID.Heading"),
                      seawolf.var.get("RollPID.p"),
                      seawolf.var.get("RollPID.i"),
                      seawolf.var.get("RollPID.d"))

    dataOut(0.0)
    mv = 0.0

    while (True):
        seawolf.var.sync()

        if seawolf.var.stale("SEA.Roll"):
            roll = seawolf.var.get("SEA.Roll")

        if seawolf.var.poked("RollPID.Heading"):
            pid.setSetPoint(seawolf.var.get("RollPID.Heading"))

            if paused:
                seawolf.var.set("RollPID.Paused", 0.0)

        if (seawolf.var.stale("RollPID.p") or seawolf.var.stale("RollPID.i")
                or seawolf.var.stale("RollPID.d")):

            pid.setCoefficients(seawolf.var.get("RollPID.p"),
                                seawolf.var.get("RollPID.i"),
                                seawolf.var.get("RollPID.d"))
            pid.resetIntegral()  # init e dt

        if (seawolf.var.stale("RollPID.Paused")):
            paused = seawolf.var.get("RollPID.Paused")

            if paused:
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Roll")
                pid.pause()

        if not paused:
            mv = pid.update(roll)
            dataOut(mv)

    seawolf.close()
Esempio n. 10
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Yaw PID")

    seawolf.var.subscribe("YawPID.p")
    seawolf.var.subscribe("YawPID.i")
    seawolf.var.subscribe("YawPID.d")
    seawolf.var.subscribe("YawPID.Heading")
    seawolf.var.subscribe("YawPID.Paused")
    seawolf.var.subscribe("SEA.Yaw")

    paused = seawolf.var.get("YawPID.Paused")
    heading = seawolf.var.get("YawPID.Heading")

    pid = seawolf.PID(0.0, seawolf.var.get("YawPID.p"),
                      seawolf.var.get("YawPID.i"), seawolf.var.get("YawPID.d"))

    dataOut(0.0)

    while (True):

        seawolf.var.sync()

        if seawolf.var.stale("SEA.Yaw"):
            yaw = seawolf.var.get("SEA.Yaw")

        if (seawolf.var.stale("YawPID.p") or seawolf.var.stale("YawPID.i")
                or seawolf.var.stale("YawPID.d")):

            pid.setCoefficients(seawolf.var.get("YawPID.p"),
                                seawolf.var.get("YawPID.i"),
                                seawolf.var.get("YawPID.d"))
            pid.resetIntegral()

        if (seawolf.var.poked("YawPID.Heading")):
            heading = seawolf.var.get("YawPID.Heading")

            if paused:
                seawolf.var.set("YawPID.Paused", 0.0)

        if (seawolf.var.stale("YawPID.Paused")):
            paused = seawolf.var.get("YawPID.Paused")

            if paused:
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Yaw")
                pid.pause()

        if not paused:
            mv = pid.update(angleError(heading, yaw))
            dataOut(mv)

    seawolf.close()
Esempio n. 11
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Pitch PID")
    
    seawolf.var.subscribe("PitchPID.p")
    seawolf.var.subscribe("PitchPID.i")
    seawolf.var.subscribe("PitchPID.d")
    seawolf.var.subscribe("PitchPID.Heading")
    seawolf.var.subscribe("PitchPID.Paused")
    seawolf.var.subscribe("SEA.Pitch")

    pitch = seawolf.var.get("SEA.Pitch")
    paused = seawolf.var.get("PitchPID.Paused")
    pid = seawolf.PID( seawolf.var.get("PitchPID.Heading"), seawolf.var.get("PitchPID.p"), seawolf.var.get("PitchPID.i"), seawolf.var.get("PitchPID.d"))

    # set active region (region where response of the robot
    # is practically linear). Outside this region, thrusters
    # would be maxed out, and the ITerm would get staturated.
    # Outside this region, the we use PD control. Inside this
    # region, we use PID control.
    pid.setActiveRegion(ACTIVE_REGION_SIZE)
    
    dataOut(0.0)
    mv = 0.0

    while(True):
        seawolf.var.sync()
        if( seawolf.var.stale("SEA.Pitch") and paused == False):
            pitch = seawolf.var.get("SEA.Pitch")
            mv = pid.update(pitch)
        elif( seawolf.var.stale("PitchPID.p") or seawolf.var.stale("PitchPID.i") or seawolf.var.stale("PitchPID.d") ):
            pid.setCoefficients( seawolf.var.get("PitchPID.p"), seawolf.var.get("PitchPID.i"), seawolf.var.get("PitchPID.d"))
            pid.resetIntegral() #init e dt
        elif( seawolf.var.poked("PitchPID.Heading") ):
            pid.setSetPoint(seawolf.var.get("PitchPID.Heading"))
            mv = pid.update( seawolf.var.get("SEA.Pitch") )
            if(paused):
                seawolf.var.set("PitchPID.Paused", 0.0)
        elif( seawolf.var.stale("PitchPID.Paused")  ):
            p = seawolf.var.get("PitchPID.Paused")
            if(p == paused):
                continue
            
            paused = p
            if(paused):
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Pitch")
                pid.pause()
        
        if(paused == False):
            dataOut(mv)

    seawolf.close()
Esempio n. 12
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Pitch PID")

    seawolf.var.subscribe("PitchPID.p")
    seawolf.var.subscribe("PitchPID.i")
    seawolf.var.subscribe("PitchPID.d")
    seawolf.var.subscribe("PitchPID.Heading")
    seawolf.var.subscribe("PitchPID.Paused")
    seawolf.var.subscribe("SEA.Pitch")

    pitch = seawolf.var.get("SEA.Pitch")
    paused = seawolf.var.get("PitchPID.Paused")
    pid = seawolf.PID(seawolf.var.get("PitchPID.Heading"),
                      seawolf.var.get("PitchPID.p"),
                      seawolf.var.get("PitchPID.i"),
                      seawolf.var.get("PitchPID.d"))

    dataOut(0.0)
    mv = 0.0

    while (True):
        seawolf.var.sync()
        if (seawolf.var.stale("SEA.Pitch") and paused == False):
            pitch = seawolf.var.get("SEA.Pitch")
            mv = pid.update(pitch)
        elif (seawolf.var.stale("PitchPID.p")
              or seawolf.var.stale("PitchPID.i")
              or seawolf.var.stale("PitchPID.d")):
            pid.setCoefficients(seawolf.var.get("PitchPID.p"),
                                seawolf.var.get("PitchPID.i"),
                                seawolf.var.get("PitchPID.d"))
            pid.resetIntegral()  #init e dt
        elif (seawolf.var.poked("PitchPID.Heading")):
            pid.setSetPoint(seawolf.var.get("PitchPID.Heading"))
            mv = pid.update(seawolf.var.get("SEA.Pitch"))
            if (paused):
                seawolf.var.set("PitchPID.Paused", 0.0)
        elif (seawolf.var.stale("PitchPID.Paused")):
            p = seawolf.var.get("PitchPID.Paused")
            if (p == paused):
                continue

            paused = p
            if (paused):
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Pitch")
                pid.pause()

        if (paused == False):
            dataOut(mv)

    seawolf.close()
Esempio n. 13
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Pitch PID")
    
    seawolf.var.subscribe("PitchPID.p")
    seawolf.var.subscribe("PitchPID.i")
    seawolf.var.subscribe("PitchPID.d")
    seawolf.var.subscribe("PitchPID.Heading")
    seawolf.var.subscribe("PitchPID.Paused")
    seawolf.var.subscribe("SEA.Pitch")

    pitch = seawolf.var.get("SEA.Pitch")
    paused = seawolf.var.get("PitchPID.Paused")
    pid = seawolf.PID( seawolf.var.get("PitchPID.Heading"), seawolf.var.get("PitchPID.p"), seawolf.var.get("PitchPID.i"), seawolf.var.get("PitchPID.d"))

    dataOut(0.0)
    mv = 0.0

    while(True):
        seawolf.var.sync()
        if( seawolf.var.stale("SEA.Pitch") and paused == False):
            pitch = seawolf.var.get("SEA.Pitch")
            mv = pid.update(pitch)
        elif( seawolf.var.stale("PitchPID.p") or seawolf.var.stale("PitchPID.i") or seawolf.var.stale("PitchPID.d") ):
            pid.setCoefficients( seawolf.var.get("PitchPID.p"), seawolf.var.get("PitchPID.i"), seawolf.var.get("PitchPID.d"))
            pid.resetIntegral() #init e dt
        elif( seawolf.var.poked("PitchPID.Heading") ):
            pid.setSetPoint(seawolf.var.get("PitchPID.Heading"))
            mv = pid.update( seawolf.var.get("SEA.Pitch") )
            if(paused):
                seawolf.var.set("PitchPID.Paused", 0.0)
        elif( seawolf.var.stale("PitchPID.Paused")  ):
            p = seawolf.var.get("PitchPID.Paused")
            if(p == paused):
                continue
            
            paused = p
            if(paused):
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Pitch")
                pid.pause()
        
        if(paused == False):
            dataOut(mv)

    seawolf.close()
Esempio n. 14
0
def main(args):
    # Connect to hub
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Thruster Test")


    #setting value
    global VAL
    if len(sys.argv) == 2:
      VAL = sys.argv[1]
    
    #print values:
    out =""
    for txt in THRUSTERS:
      out+= "\n\t" +txt
    print("INPUTS: " + out)
    

    # Arguments
    try:
      while True:
        selected = raw_input("Thruster to test (q to quit): ")
        if selected == 'q':
          print("quitting")
          break
        found = False
        for thrust in THRUSTERS:
          if str(thrust).lower() == str(selected).lower():
            found = True
            test_thruster(thrust, VAL)
        
        if not found:
          print("input not found")
          print("accepted values are: " + out)
        

    except Exception as e:
      print(e)
    finally:
      for thrust in THRUSTERS:
        seawolf.var.set(thrust, 0)
      seawolf.close()
Esempio n. 15
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf") # <--Contains IP for connecting to hub
    seawolf.init("RazorIMU.py") #<--Gives program a name under hub
    
    # open IMU object
    imu = IMU()
    imu.connect(DEVICE_PATH,threaded=True, auto_sync=AUTO_SYNC)
    #imu.setOutputMode('t')
    
    # issue help text
    if AUTO_SYNC==False:
        print("auto_sync disabled! If you see strange values, be sure to hit\n"+
              "ENTER once the stream of data starts. This will immediately\n"+
              "synchronize the IMU stream parser with the frame head of the\n"+
              "IMU data stream.")
        
        raw_input("\t\t  <Hit ENTER to continue>")
    
    # start streaming
    imu.startStream()
    imu.streamTest()
    
    
    # send a sync command and see what happens
    val = None
    while val !='q':
        if val == '':
            imu.synchronize_flag = 1
        if val == 'a':
            print("\n\nhello\n")
        val = raw_input()
    
    # close the program
    imu.stopStream()
    imu.disconnect()   
    seawolf.close() #<--Gracefully disconnects program from hub
Esempio n. 16
0
        d.paused[1] = sw.var.get("PitchPID.Paused")
        d.paused[2] = sw.var.get("YawPID.Paused")
        d.paused[3] = sw.var.get("DepthPID.Paused")

        setVars()
        d.rect(330, 555, 80, 30, d.BACKGROUND_COLOR)
        d.textAtC(330, 580, str(round(sw.var.get("Depth"), 2)), (0, 0, 255))
        if d.change[12] == 1:
            d.change[12] = 0
            if d.toggle[12] == True:
                sw3.ZeroThrusters().start()
                d.setSlideValue(6, sw.var.get("Bow"))
                d.setSlideValue(7, sw.var.get("Stern"))
                d.setSlideValue(8, sw.var.get("Port"))
                d.setSlideValue(9, sw.var.get("Star"))
                d.setSlideValue(10, sw.var.get("StrafeT"))
                d.setSlideValue(11, sw.var.get("StrafeB"))

        #play and pause button

    #d.actualBearing[0] += 1/3.14
    #if d.actualBearing[0] >= 2*3.14:
    #d.actualBearing[0] = 0

    if k == 107:  #hit k for kill
        sw.close()
        cv2.destroyWindow('image')
        break
    elif k == ord('a'):
        print mouseX, mouseY
Esempio n. 17
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Yaw PID")

    # initialize system variables for yaw PID system
    seawolf.var.subscribe("YawPID.p")
    seawolf.var.subscribe("YawPID.i")
    seawolf.var.subscribe("YawPID.d")
    seawolf.var.subscribe("YawPID.Heading")
    seawolf.var.subscribe("YawPID.Paused")
    seawolf.var.subscribe("SEA.Yaw")

    yaw = seawolf.var.get("SEA.Yaw")
    paused = seawolf.var.get("YawPID.Paused")
    heading = seawolf.var.get("YawPID.Heading")
    yaw = seawolf.var.get("SEA.Yaw")

    pid = seawolf.PID(0.0, seawolf.var.get("YawPID.p"),
                      seawolf.var.get("YawPID.i"), seawolf.var.get("YawPID.d"))

    # set active region (region where response of the robot
    # is practically linear). Outside this region, thrusters
    # would be maxed out, and the ITerm would get staturated.
    # Outside this region, the we use PD control. Inside this
    # region, we use PID control.
    pid.setActiveRegion(ACTIVE_REGION_SIZE)
    pid.setDerivativeBufferSize(2)

    dataOut(0.0)
    mv = 0.0

    while (True):

        # wait for all variables to update
        seawolf.var.sync()

        # if seawolf's yaw has changed, save it to the local yaw variable.
        if seawolf.var.stale("SEA.Yaw"):
            yaw = seawolf.var.get("SEA.Yaw")

        # if PID controller's parameters have changed, update pid settings and reset controller
        if (seawolf.var.stale("YawPID.p") or seawolf.var.stale("YawPID.i")
                or seawolf.var.stale("YawPID.d")):

            pid.setCoefficients(seawolf.var.get("YawPID.p"),
                                seawolf.var.get("YawPID.i"),
                                seawolf.var.get("YawPID.d"))
            pid.resetIntegral()

        # if user has (possibly) modified seawolf's target heading, store the value
        # and unpause the yaw PID. The unpausing may have been done in order to accomodate
        # functionality whereby a user-level change to yaw PID heading will automatically,
        # take the robot out of pause mode. This is a means of saving the user from having
        # to manually unpause the robot to make changes, which would be somewhat un-intuitive
        # and bound cause confusion.
        if (seawolf.var.poked("YawPID.Heading")):
            heading = seawolf.var.get("YawPID.Heading")

            # if yawPID has been paused, unpause it.
            if paused:
                seawolf.var.set("YawPID.Paused", 0.0)

        # if Yaw PID pause state has changed, store it's value
        if (seawolf.var.stale("YawPID.Paused")):
            paused = seawolf.var.get("YawPID.Paused")

            # if paused, zero yaw thrusters and notify user
            if paused:
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Yaw")
                pid.pause()

        # go ahead and run PID and output new thruster value if unpaused.
        if not paused:
            mv = pid.update(angleError(heading, yaw))
            dataOut(mv)

    seawolf.close()
Esempio n. 18
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Yaw PID")

    # initialize system variables for yaw PID system
    seawolf.var.subscribe("YawPID.p")
    seawolf.var.subscribe("YawPID.i")
    seawolf.var.subscribe("YawPID.d")
    seawolf.var.subscribe("YawPID.Heading")
    seawolf.var.subscribe("YawPID.Paused")
    seawolf.var.subscribe("SEA.Yaw")

    yaw = seawolf.var.get("SEA.Yaw")
    paused = seawolf.var.get("YawPID.Paused")
    heading = seawolf.var.get("YawPID.Heading")
    yaw = seawolf.var.get("SEA.Yaw")

    pid = seawolf.PID(
        0.0,
        seawolf.var.get("YawPID.p"),
        seawolf.var.get("YawPID.i"),
        seawolf.var.get("YawPID.d")
    )

    # set active region (region where response of the robot
    # is practically linear). Outside this region, thrusters
    # would be maxed out, and the ITerm would get staturated.
    # Outside this region, the we use PD control. Inside this
    # region, we use PID control.
    pid.setActiveRegion(ACTIVE_REGION_SIZE)
    pid.setDerivativeBufferSize(2)

    dataOut(0.0)
    mv = 0.0

    while(True):

        # wait for all variables to update
        seawolf.var.sync()

        # if seawolf's yaw has changed, save it to the local yaw variable.
        if seawolf.var.stale("SEA.Yaw"):
            yaw = seawolf.var.get("SEA.Yaw")

        # if PID controller's parameters have changed, update pid settings and reset controller
        if (seawolf.var.stale("YawPID.p") or seawolf.var.stale("YawPID.i") or seawolf.var.stale("YawPID.d")):

            pid.setCoefficients(
                seawolf.var.get("YawPID.p"),
                seawolf.var.get("YawPID.i"),
                seawolf.var.get("YawPID.d")
            )
            pid.resetIntegral()

        # if user has (possibly) modified seawolf's target heading, store the value
        # and unpause the yaw PID. The unpausing may have been done in order to accomodate
        # functionality whereby a user-level change to yaw PID heading will automatically,
        # take the robot out of pause mode. This is a means of saving the user from having
        # to manually unpause the robot to make changes, which would be somewhat un-intuitive
        # and bound cause confusion.
        if (seawolf.var.poked("YawPID.Heading")):
            heading = seawolf.var.get("YawPID.Heading")

            # if yawPID has been paused, unpause it.
            if paused:
                seawolf.var.set("YawPID.Paused", 0.0)

        # if Yaw PID pause state has changed, store it's value
        if (seawolf.var.stale("YawPID.Paused")):
            paused = seawolf.var.get("YawPID.Paused")

            # if paused, zero yaw thrusters and notify user
            if paused:
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Yaw")
                pid.pause()

        # go ahead and run PID and output new thruster value if unpaused.
        if not paused:
            mv = pid.update(angleError(heading, yaw))
            dataOut(mv)

    seawolf.close()
Esempio n. 19
0
            textAt(330,580, str(round(sw.var.get("Depth"),2)), RED )
            #if sliders need to be reZeroed (for if zeroing the thrusters came undone)
            if reZeroSliders:
                    zeroSliders()
                    reZeroSliders = False
            if Pause.change:
                    Pause.change = False
                    if Pause.pressed == True:
                            #zero out dials

                            Roll.change = True
                            Roll.desiredBearing = -math.pi/2
                            writeHub(Roll)

                            Pitch.change = True
                            Pitch.desiredBearing = -math.pi/2
                            writeHub(Pitch)

                            Yaw.change = True
                            Yaw.desiredBearing = Yaw.actualBearing
                            writeHub(Yaw)
                            #zero out other sliders
                            zeroSliders()
                            reZeroSliders = True
    #if the k key is hit
    if k == 107 or k == 27 or cv2.getWindowProperty('Control Panel',1) < 1:
            sw.close()
            cv2.destroyWindow('Control Panel')
            break

Esempio n. 20
0
def main():
    sw.loadConfig("../conf/seawolf.conf")
    sw.init("Joystick Controller")

    yaw_heading = sw3.data.imu.yaw()

    devices = joystick.get_devices()
    peripheral = Peripherals()
    if len(devices) == 0:
        sys.stderr.write("No joysticks found\n")
        sys.exit(1)

    depth_heading = 0
    forward_heading = 0
    Lrate_limiter = ratelimit.RateLimiter(10, update_Laxis)
    Rrate_limiter = ratelimit.RateLimiter(10, update_Raxis)
    js = joystick.Joystick(devices[0], joystick.LOGITECH)

    print_help()
    peripheral.printActive()

    while True:
        event = js.poll()

        if isinstance(event, joystick.Axis):
            if (event.name == "leftStick") :
                Lrate_limiter.provide(event)
                
            elif (event.name=="rightStick"):
                Rrate_limiter.provide(event)
                
            elif event.name == "hat":
                if event.x < 0:
                    yaw_heading = sw3.util.add_angle(yaw_heading, -2.5)
                    sw3.pid.yaw.heading = yaw_heading
                elif event.x > 0:
                    yaw_heading = sw3.util.add_angle(yaw_heading, 2.5)
                    sw3.pid.yaw.heading = yaw_heading
                elif event.y < 0:
                    forward_heading = FORWARD_SPEED
                    sw3.nav.do(sw3.Forward(forward_heading))
                elif event.y > 0:
                    forward_heading = -FORWARD_SPEED
                    sw3.nav.do(sw3.Forward(forward_heading))
                else:
                    forward_heading = 0
                    sw3.nav.do(sw3.Forward(forward_heading))

        elif event.value == 1:
            if event.name == "button8":
                depth_heading = min(8, depth_heading + 0.50)
                sw3.pid.depth.heading = depth_heading

            elif event.name == "button6":
                depth_heading = max(0, depth_heading - 0.50)
                sw3.pid.depth.heading = depth_heading

            elif event.name == "button1":
                print "Zeroing thrusters"
                sw3.nav.do(sw3.ZeroThrusters())
                depth_heading = 0

            elif event.name == "button4":
                while js.poll().name != "button4":
                    pass
                print "Press again to confirm breech"
                if js.poll().name == "button4":
                    print "Breeching!"
                    sw3.nav.do(sw3.EmergencyBreech())
                    depth_heading = 0
                else:
                    print "Canceled."

            elif event.name == "button9":
                print_help()

            elif event.name == "button3":
                variables = "Depth", "DepthPID.Heading", "SEA.Yaw", "YawPID.Heading"
                print_table(variables, ["%.2f" % sw.var.get(v) for v in variables])
                print

                variables = "Port", "Star", "Bow", "Stern", "StrafeT", "StrafeB"
                print_table(variables, ["%.2f" % sw.var.get(v) for v in variables])
                print

            elif event.name == "button10":
                print "Quitting"
                sw3.nav.do(sw3.ZeroThrusters())
                break

            elif event.name == "button5":
                peripheral.next()
                peripheral.printActive()

            elif event.name == "button7":
                peripheral.prev()
                peripheral.printActive()         

            elif event.name == "button2":
                peripheral.fire()

    sw.close()
Esempio n. 21
0
#!/usr/bin/env python

from __future__ import division, print_function

import time
import sys
import seawolf

seawolf.loadConfig("../conf/seawolf.conf")
seawolf.init("Sensor Test")
"""
Do stuff here
"""

seawolf.close()
Esempio n. 22
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Roll PID")

    seawolf.var.subscribe("RollPID.p")
    seawolf.var.subscribe("RollPID.i")
    seawolf.var.subscribe("RollPID.d")
    seawolf.var.subscribe("RollPID.Heading")
    seawolf.var.subscribe("RollPID.Paused")
    seawolf.var.subscribe("SEA.Roll")

    roll = seawolf.var.get("SEA.Roll")
    paused = seawolf.var.get("RollPID.Paused")
    pid = seawolf.PID(
        seawolf.var.get("RollPID.Heading"),
        seawolf.var.get("RollPID.p"),
        seawolf.var.get("RollPID.i"),
        seawolf.var.get("RollPID.d"),
    )

    # set active region (region where response of the robot
    # is practically linear). Outside this region, thrusters
    # would be maxed out, and the ITerm would get staturated.
    # Outside this region, the we use PD control. Inside this
    # region, we use PID control.
    pid.setActiveRegion(ACTIVE_REGION_SIZE)

    dataOut(0.0)
    mv = 0.0

    while True:
        seawolf.var.sync()

        if seawolf.var.stale("SEA.Roll"):
            roll = seawolf.var.get("SEA.Roll")

        if seawolf.var.poked("RollPID.Heading"):
            pid.setSetPoint(seawolf.var.get("RollPID.Heading"))

            if paused:
                seawolf.var.set("RollPID.Paused", 0.0)

        if seawolf.var.stale("RollPID.p") or seawolf.var.stale("RollPID.i") or seawolf.var.stale("RollPID.d"):

            pid.setCoefficients(
                seawolf.var.get("RollPID.p"), seawolf.var.get("RollPID.i"), seawolf.var.get("RollPID.d")
            )
            pid.resetIntegral()  # init e dt

        if seawolf.var.stale("RollPID.Paused"):
            paused = seawolf.var.get("RollPID.Paused")

            if paused:
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Roll")
                pid.pause()

        if not paused:
            mv = pid.update(roll)
            dataOut(mv)

    seawolf.close()
Esempio n. 23
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Depth PID")

    seawolf.var.subscribe("DepthPID.p")
    seawolf.var.subscribe("DepthPID.i")
    seawolf.var.subscribe("DepthPID.d")
    seawolf.var.subscribe("DepthPID.Heading")
    seawolf.var.subscribe("DepthPID.Paused")
    seawolf.var.subscribe("Depth")

    depth = seawolf.var.get("Depth")
    paused = seawolf.var.get("DepthPID.Paused")

    pid = seawolf.PID(seawolf.var.get("DepthPID.Heading"),
                      seawolf.var.get("DepthPID.p"),
                      seawolf.var.get("DepthPID.i"),
                      seawolf.var.get("DepthPID.d"))

    e_dt = initial_e_dt(seawolf.var.get("DepthPID.i"))

    # set active region (region where response of the robot
    # is practically linear). Outside this region, thrusters
    # would be maxed out, and the ITerm would get staturated.
    # Outside this region, the we use PD control. Inside this
    # region, we use PID control.
    pid.setActiveRegion(ACTIVE_REGION_SIZE)

    dataOut(0.0)

    while (True):
        seawolf.var.sync()

        if (seawolf.var.stale("Depth")):
            depth = seawolf.var.get("Depth")

        if (seawolf.var.stale("DepthPID.p") or seawolf.var.stale("DepthPID.i")
                or seawolf.var.stale("DepthPID.d")):
            pid.setCoefficients(seawolf.var.get("DepthPID.p"),
                                seawolf.var.get("DepthPID.i"),
                                seawolf.var.get("DepthPID.d"))

            e_dt = initial_e_dt(seawolf.var.get("DepthPID.i"))

        if (seawolf.var.poked("DepthPID.Heading")):
            pid.setSetPoint(seawolf.var.get("DepthPID.Heading"))
            if (paused):
                seawolf.var.set("DepthPID.Paused", 0.0)
                seawolf.var.set("PitchPID.Paused", 0.0)

        if (seawolf.var.stale("DepthPID.Paused")):
            paused = seawolf.var.get("DepthPID.Paused")
            if (paused):
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Depth")
                pid.pause()
                e_dt = initial_e_dt(seawolf.var.get("DepthPID.i"))

        if (depth > panic_depth):
            seawolf.logging.log(seawolf.CRITICAL, "Depth: {}\n".format(depth))
            seawolf.logging.log(seawolf.CRITICAL,
                                "I'm too deep! Rising full force!\n")

            dataOut(-1.0)
            time.sleep(panic_time)
        elif (paused == False):
            mv = pid.update(depth)
            mv = in_range(-thruster_cap, mv, thruster_cap)
            dataOut(mv)

    seawolf.close()
def main():
    sw.loadConfig("../conf/seawolf.conf")
    sw.init("Joystick Controller")

    # parse arguments
    global DEPTH_HOLD
    global ORIENTATION_HOLD
    parser = setup_parser()
    args = parser.parse_args()
    joystick_name = args.selected_controller
    DEPTH_HOLD = args.depth_hold
    ORIENTATION_HOLD = args.orientation_hold

    # get heading initial heading values
    yaw_heading = sw3.data.imu.yaw()
    depth_heading = 0
    forward_heading = 0

    # instantiate pnuematics controller
    peripheral = Peripherals()

    # instantiate joystick class
    devices = joystick.get_devices()

    if len(devices) == 0:
        sys.stderr.write("No joysticks found\n")
        sys.exit(1)

    js = joystick.Joystick(devices[0], joystick.LOGITECH)

    # attach functions to analog sticks
    Lrate_limiter = ratelimit.RateLimiter(10, update_Laxis)
    Rrate_limiter = ratelimit.RateLimiter(10, update_Raxis)

    # splash screen usage-prints
    print_help()
    peripheral.printActive()

    while True:
        event = js.poll()

        if isinstance(event, joystick.Axis):
            if (event.name == "leftStick"):
                Lrate_limiter.provide(event)

            elif (event.name == "rightStick"):
                Rrate_limiter.provide(event)

            elif event.name == "hat":
                if event.x < 0:
                    # yaw_heading = sw3.util.add_angle(yaw_heading, -2.5)
                    # sw3.pid.yaw.heading = yaw_heading
                    pass
                elif event.x > 0:
                    # yaw_heading = sw3.util.add_angle(yaw_heading, 2.5)
                    # sw3.pid.yaw.heading = yaw_heading
                    pass
                elif event.y < 0:
                    # forward_heading = FORWARD_SPEED
                    # sw3.nav.do(sw3.Forward(forward_heading))
                    pass
                elif event.y > 0:
                    # forward_heading = -FORWARD_SPEED
                    # sw3.nav.do(sw3.Forward(forward_heading))
                    pass
                else:
                    # forward_heading = 0
                    # sw3.nav.do(sw3.Forward(forward_heading))
                    pass

        elif event.value == 1:
            if event.name == "button1":
                print "Zeroing thrusters (and depth heading)"
                sw3.nav.do(sw3.ZeroThrusters())
                depth_heading = 0

            elif event.name == "button2":
                peripheral.fire()

            elif event.name == "button3":
                variables = "Depth", "DepthPID.Heading", "SEA.Yaw", "YawPID.Heading"
                print_table(variables,
                            ["%.2f" % sw.var.get(v) for v in variables])
                print

                variables = "Port", "Star", "Bow", "Stern", "StrafeT", "StrafeB"
                print_table(variables,
                            ["%.2f" % sw.var.get(v) for v in variables])
                print

            elif event.name == "button4":
                while js.poll().name != "button4":
                    pass
                print "Press again to confirm breech"
                if js.poll().name == "button4":
                    print "Breeching!"
                    sw3.nav.do(sw3.EmergencyBreech())
                    depth_heading = 0
                else:
                    print "Canceled."

            elif event.name == "button5":
                peripheral.next()
                peripheral.printActive()

            elif event.name == "button6":
                if DEPTH_HOLD:
                    depth_heading = max(0, depth_heading - 0.50)
                    sw3.pid.depth.heading = depth_heading
                else:
                    print(
                        "Depth holding disabled!!! ignoring depth-change command."
                    )

            elif event.name == "button7":
                peripheral.prev()
                peripheral.printActive()

            elif event.name == "button8":
                if DEPTH_HOLD:
                    depth_heading = min(16, depth_heading + 0.50)
                    sw3.pid.depth.heading = depth_heading
                else:
                    print(
                        "Depth holding disabled!!! ignoring depth-change command."
                    )

            elif event.name == "button9":
                print_help()

            elif event.name == "button10":
                print "Quitting"
                sw3.nav.do(sw3.ZeroThrusters())
                break

            elif event.name == "leftStickButton":
                if ORIENTATION_HOLD:
                    print "Reseting Orientation"
                    thruster_request('Yaw', 0, hold_orientation=True)
                    thruster_request('Pitch',
                                     0,
                                     hold_orientation=True,
                                     auto_level=True)
                    thruster_request('Roll',
                                     0,
                                     hold_orientation=True,
                                     auto_level=True)

    sw.close()
Esempio n. 25
0
#!/usr/bin/env python

# [test-vanilla.py]: a template for creating new test files.
# This folder contains the bare minimum for open a file under
# hub's supervision just like all the other scripts written for
# seawolf. This is meant to be generic and copy&paste-able for
# quickly creating new test scripts

from __future__ import division, print_function

import seawolf
# other imports here...

seawolf.loadConfig("../conf/seawolf.conf") # <--Contains IP for connecting to hub
seawolf.init("Sensor Test") #<--Gives program a name under hub

"""
Do stuff here
"""

seawolf.close() #<--Gracefully disconnects program from hub
Esempio n. 26
0
def main():
    seawolf.loadConfig("../conf/seawolf.conf")
    seawolf.init("Depth PID")

    seawolf.var.subscribe("DepthPID.p")
    seawolf.var.subscribe("DepthPID.i")
    seawolf.var.subscribe("DepthPID.d")
    seawolf.var.subscribe("DepthPID.Heading")
    seawolf.var.subscribe("DepthPID.Paused")
    seawolf.var.subscribe("Depth")

    depth = seawolf.var.get("Depth")
    paused = seawolf.var.get("DepthPID.Paused")

    pid = seawolf.PID( seawolf.var.get("DepthPID.Heading"), seawolf.var.get("DepthPID.p"), seawolf.var.get("DepthPID.i"), seawolf.var.get("DepthPID.d"))

    e_dt = initial_e_dt( seawolf.var.get("DepthPID.i") )
    
    # set active region (region where response of the robot
    # is practically linear). Outside this region, thrusters
    # would be maxed out, and the ITerm would get staturated.
    # Outside this region, the we use PD control. Inside this
    # region, we use PID control.
    pid.setActiveRegion(ACTIVE_REGION_SIZE)
    
    dataOut(0.0)

    while(True):
        seawolf.var.sync()

        if(seawolf.var.stale("Depth")):
            depth = seawolf.var.get("Depth")

        if(seawolf.var.stale("DepthPID.p") or seawolf.var.stale("DepthPID.i") or seawolf.var.stale("DepthPID.d")):
            pid.setCoefficients(seawolf.var.get("DepthPID.p"), seawolf.var.get("DepthPID.i"), seawolf.var.get("DepthPID.d"))

            e_dt = initial_e_dt( seawolf.var.get("DepthPID.i") )

        if(seawolf.var.poked("DepthPID.Heading")):
            pid.setSetPoint(seawolf.var.get("DepthPID.Heading"))
            if(paused):
                seawolf.var.set("DepthPID.Paused", 0.0)
                seawolf.var.set("PitchPID.Paused", 0.0)

        if(seawolf.var.stale("DepthPID.Paused")):
            paused = seawolf.var.get("DepthPID.Paused")
            if(paused):
                dataOut(0.0)
                seawolf.notify.send("PIDPAUSED", "Depth")
                pid.pause()
                e_dt = initial_e_dt( seawolf.var.get("DepthPID.i") )

        if(depth > panic_depth):
            seawolf.logging.log(seawolf.CRITICAL, "Depth: {}\n".format(depth))
            seawolf.logging.log(seawolf.CRITICAL, "I'm too deep! Rising full force!\n")

            dataOut(-1.0)
            time.sleep(panic_time)
        elif(paused == False):
            mv = pid.update(depth)
            mv = in_range(-thruster_cap, mv, thruster_cap)
            dataOut(mv)

    seawolf.close();
Esempio n. 27
0
#!/usr/bin/env python

# [test-vanilla.py]: a template for creating new test files.
# This folder contains the bare minimum for open a file under
# hub's supervision just like all the other scripts written for
# seawolf. This is meant to be generic and copy&paste-able for
# quickly creating new test scripts

from __future__ import division, print_function

import seawolf

# other imports here...

seawolf.loadConfig("../conf/seawolf.conf")  # <--Contains IP for connecting to hub
seawolf.init("Sensor Test")  # <--Gives program a name under hub

"""
Do stuff here
"""

seawolf.close()  # <--Gracefully disconnects program from hub
Esempio n. 28
0
def main():
    sw.loadConfig("../conf/seawolf.conf")
    sw.init("Joystick Controller")

    # parse arguments
    global DEPTH_HOLD
    global ORIENTATION_HOLD
    parser  = setup_parser()
    args    = parser.parse_args()
    joystick_name   = args.selected_controller
    DEPTH_HOLD      = args.depth_hold
    ORIENTATION_HOLD= args.orientation_hold
    
    # get heading initial heading values
    yaw_heading = sw3.data.imu.yaw()
    depth_heading = 0
    forward_heading = 0

    # instantiate pnuematics controller
    peripheral = Peripherals()

    # instantiate joystick class
    devices = joystick.get_devices()
    
    if len(devices) == 0:
        sys.stderr.write("No joysticks found\n")
        sys.exit(1)

    js = joystick.Joystick(devices[0], joystick.LOGITECH)

    # attach functions to analog sticks
    Lrate_limiter = ratelimit.RateLimiter(10, update_Laxis)
    Rrate_limiter = ratelimit.RateLimiter(10, update_Raxis)

    # splash screen usage-prints
    print_help()
    peripheral.printActive()

    while True:
        event = js.poll()

        if isinstance(event, joystick.Axis):
            if (event.name == "leftStick") :
                Lrate_limiter.provide(event)
                
            elif (event.name=="rightStick"):
                Rrate_limiter.provide(event)
                
            elif event.name == "hat":
                if event.x < 0:
                    # yaw_heading = sw3.util.add_angle(yaw_heading, -2.5)
                    # sw3.pid.yaw.heading = yaw_heading
                    pass
                elif event.x > 0:
                    # yaw_heading = sw3.util.add_angle(yaw_heading, 2.5)
                    # sw3.pid.yaw.heading = yaw_heading
                    pass
                elif event.y < 0:
                    # forward_heading = FORWARD_SPEED
                    # sw3.nav.do(sw3.Forward(forward_heading))
                    pass
                elif event.y > 0:
                    # forward_heading = -FORWARD_SPEED
                    # sw3.nav.do(sw3.Forward(forward_heading))
                    pass
                else:
                    # forward_heading = 0
                    # sw3.nav.do(sw3.Forward(forward_heading))
                    pass

        elif event.value == 1:
            if event.name == "button1":
                print "Zeroing thrusters (and depth heading)"
                sw3.nav.do(sw3.ZeroThrusters())
                depth_heading = 0

            elif event.name == "button2":
                peripheral.fire()
                
            elif event.name == "button3":
                variables = "Depth", "DepthPID.Heading", "SEA.Yaw", "YawPID.Heading"
                print_table(variables, ["%.2f" % sw.var.get(v) for v in variables])
                print

                variables = "Port", "Star", "Bow", "Stern", "StrafeT", "StrafeB"
                print_table(variables, ["%.2f" % sw.var.get(v) for v in variables])
                print

            elif event.name == "button4":
                while js.poll().name != "button4":
                    pass
                print "Press again to confirm breech"
                if js.poll().name == "button4":
                    print "Breeching!"
                    sw3.nav.do(sw3.EmergencyBreech())
                    depth_heading = 0
                else:
                    print "Canceled."

            elif event.name == "button5":
                peripheral.next()
                peripheral.printActive()

            elif event.name == "button6":
                if DEPTH_HOLD:
                    depth_heading = max(0, depth_heading - 0.50)
                    sw3.pid.depth.heading = depth_heading
                else:
                    print("Depth holding disabled!!! ignoring depth-change command.")

            elif event.name == "button7":
                peripheral.prev()
                peripheral.printActive()         

            elif event.name == "button8":
                if DEPTH_HOLD:
                    depth_heading = min(16, depth_heading + 0.50)
                    sw3.pid.depth.heading = depth_heading
                else:
                    print("Depth holding disabled!!! ignoring depth-change command.")

            elif event.name == "button9":
                print_help()

            elif event.name == "button10":
                print "Quitting"
                sw3.nav.do(sw3.ZeroThrusters())
                break

            elif event.name == "leftStickButton":
                if ORIENTATION_HOLD:
                    print "Reseting Orientation"
                    thruster_request('Yaw', 0, hold_orientation=True)
                    thruster_request('Pitch', 0, hold_orientation=True, auto_level=True)
                    thruster_request('Roll', 0, hold_orientation=True, auto_level=True)


    sw.close()