yaw_heading = sw3.util.add_angle(yaw_heading, 5)
sw3.pid.yaw.heading = yaw_heading
elif event.name == "padY" and event.x != 0:
if event.x > 0:
depth_heading = min(8, depth_heading + 0.50)
sw3.pid.depth.heading = depth_heading
else:
depth_heading = max(0, depth_heading - 0.50)
sw3.pid.depth.heading = depth_heading
elif event.value == 1:
if event.name == "button1":
print "Zeroing thrusters"
sw3.nav.do(sw3.ZeroThrusters())
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())
else:
print "Canceled."
elif event.name == "button5":
print_help()
elif event.name == "button3":
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()
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()
d.actualBearing[1] = realToDisplayRadians(
math.radians(sw.var.get("SEA.Pitch")))
d.actualBearing[2] = realToDisplayRadians(
math.radians(sw.var.get("SEA.Yaw")))
d.paused[0] = sw.var.get("RollPID.Paused")
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()