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()
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()
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()
def main():
sw.loadConfig("../../conf/seawolf.conf")
sw.init("HUD")
sw.var.subscribe("Depth")
sw.var.subscribe("DepthPID.Heading")
sw.var.subscribe("Port")
sw.var.subscribe("Star")
sw.var.subscribe("Bow")
sw.var.subscribe("Stern")
sw.var.subscribe("Strafe")
sw.var.subscribe("SEA.Pitch")
sw.var.subscribe("SEA.Roll")
global window
# For now we just pass glutInit one empty argument. I wasn't sure what should or could be passed in (tuple, list, ...)
# Once I find out the right stuff based on reading the PyOpenGL source, I'll address this.
glutInit(())
# Select type of Display mode
# Double buffer
# RGBA color
# Alpha components supported
# Depth buffer
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH)
# get a 640 x 480 window
glutInitWindowSize(840, 480)
# the window starts at the upper left corner of the screen
glutInitWindowPosition(0, 0)
# Okay, like the C version we retain the window id to use when closing, but for those of you new
# to Python (like myself), remember this assignment would make the variable local and not global
# if it weren't for the global declaration at the start of main.
window = glutCreateWindow("Seawolf Software HUD")
# Register the drawing function with glut, BUT in Python land, at least using PyOpenGL, we need to
# set the function pointer and invoke a function to actually register the callback, otherwise it
# would be very much like the C version of the code.
glutDisplayFunc (DrawGLScene)
# Uncomment this line to get full screen.
#glutFullScreen()
# When we are doing nothing, redraw the scene.
# glutTimerFunc(0,Update(),0)
glutTimerFunc(0,Update,0)
# Register the function called when our window is resized.
glutReshapeFunc(ReSizeGLScene)
# Register the function called when the keyboard is pressed.
glutKeyboardFunc (keyPressed)
# Initialize our window.
InitGL(640, 480)
# Start Event Processing Engine
glutMainLoop()
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()
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()
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()
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()
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();
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()
def main(objects):
#connecting to hub
sw.loadConfig("../conf/seawolf.conf");
sw.init("Simulator : Main");
pid = pidSim()
robo = posSim(location = [0, 0, 0], axis = [-50, 50], objects= objects)
view= viewSim(objects)
while True:
pid.updateHeading()
robo.updatePosition()
view.updateViews(robo.pos())
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()
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()
def main():
#connecting to hub
sw.loadConfig("../conf/seawolf.conf")
sw.init("Simulator : Main")
objects = setup()
pid = pidSim()
robo = posSim(location=[0, 0, 0], axis=[-20, 20], objects=objects)
view = viewSim(objects)
while True:
pid.updateHeading()
robo.updatePosition()
view.updateViews(robo.pos())
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()
def init(self, which_path = 0, angle_hint = 0, max_angle_distance = 80):
self.which_path = which_path
self.angle_hint = angle_hint
self.max_angle_distance = max_angle_distance
self.path = None
self.path_manager = PathManager(self.angle_hint, self.max_angle_distance)
self.hough_threshold = 55
self.lines_to_consider = 10
sw.loadConfig("../conf/seawolf.conf")
sw.init("DoublePath")
def init(self, which_path=0, angle_hint=0, max_angle_distance=80):
self.which_path = which_path
self.angle_hint = angle_hint
self.max_angle_distance = max_angle_distance
self.path = None
self.path_manager = PathManager(self.angle_hint,
self.max_angle_distance)
self.hough_threshold = 55
self.lines_to_consider = 10
sw.loadConfig("../conf/seawolf.conf")
sw.init("DoublePath")
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()
def runTest(tests):
sw.loadConfig("../conf/seawolf.conf");
sw.init("Simulator : Test");
sw.notify.filter(sw.FILTER_ACTION, "MISSION_DONE");
try:
while True:
(event, msg) = sw.notify.get()
if (event == "MISSION_DONE" and msg == "DONE"):
break
if (event == "MISSION_DONE"):
for test in [test for test in tests if test.missionName == msg]:
test.run()
print test
finally:
print("---------------DONE PRINTING ALL RESULTS-------------")
for t in tests:
print t
def __init__(self, process_manager, wait_for_go=False):
'''
Arguments:
process_manager
'''
self.process_manager = process_manager
self.wait_for_go = wait_for_go
self.mission_queue = deque()
self.current_mission = None
self.last_mission = None
# libseawolf init
application_name = "Mission Control"
if seawolf.getName() != application_name: # Check if seawolf is already initialized
seawolf.loadConfig("../conf/seawolf.conf")
seawolf.init(application_name)
seawolf.notify.filter(seawolf.FILTER_ACTION, "GO")
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()
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
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()
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()
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()
def main():
sw.loadConfig("../../conf/seawolf.conf")
sw.init("HUD")
sw.var.subscribe("Depth")
sw.var.subscribe("DepthPID.Heading")
sw.var.subscribe("Port")
sw.var.subscribe("Star")
sw.var.subscribe("Bow")
sw.var.subscribe("Stern")
sw.var.subscribe("StrafeT")
sw.var.subscribe("StrafeB")
sw.var.subscribe("SEA.Pitch")
sw.var.subscribe("SEA.Roll")
global window
# For now we just pass glutInit one empty argument. I wasn't sure what should or could be passed in (tuple, list, ...)
# Once I find out the right stuff based on reading the PyOpenGL source, I'll address this.
glutInit(())
# Select type of Display mode
# Double buffer
# RGBA color
# Alpha components supported
# Depth buffer
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH)
# get a 640 x 480 window
glutInitWindowSize(840, 480)
# the window starts at the upper left corner of the screen
glutInitWindowPosition(0, 0)
# Okay, like the C version we retain the window id to use when closing, but for those of you new
# to Python (like myself), remember this assignment would make the variable local and not global
# if it weren't for the global declaration at the start of main.
window = glutCreateWindow("Seawolf Software HUD")
# Register the drawing function with glut, BUT in Python land, at least using PyOpenGL, we need to
# set the function pointer and invoke a function to actually register the callback, otherwise it
# would be very much like the C version of the code.
glutDisplayFunc(DrawGLScene)
# Uncomment this line to get full screen.
#glutFullScreen()
# When we are doing nothing, redraw the scene.
# glutTimerFunc(0,Update(),0)
glutTimerFunc(0, Update, 0)
# Register the function called when our window is resized.
glutReshapeFunc(ReSizeGLScene)
# Register the function called when the keyboard is pressed.
glutKeyboardFunc(keyPressed)
# Initialize our window.
InitGL(640, 480)
# Start Event Processing Engine
glutMainLoop()
#importing the picomodule
from pico_module import pm
#importing the fft/toa calc code
from pythonTestCode import calcOrientation
#importing numpy
import numpy as np
import csv
import time
#import hub
import seawolf as sw
sw.loadConfig("../seawolf/conf/seawolf.conf");
sw.init("Acoustics : Main");
DB = True
#connecting to picotec 1 is db 0 is no db
pico = pm.pico_init(0)#1 if DB else 0)
pf = 22 * 10**3
#entering data loop
try:
while True:
#getting data and making frequency right
dataO = pm.pico_get_data(pico)
Fs = (1.0 / pm.pico_get_sample_interval()) * 10**9
#transposing data and making numpy array
data = np.array(zip(*dataO))
#!/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
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()
# Wait for mission control to acknowledge reset
i = 0
while seawolf.var.get("MissionReset"):
print "Thruster's zero'd. Waiting for mission reset"
sleep(0.1)
zero_thrusters()
running = False
else:
seawolf.logging.log(seawolf.ERROR, 'Recieved invalid event "%s"' % event)
if __name__ == "__main__":
seawolf.loadConfig("../conf/seawolf.conf")
seawolf.init("Conductor")
zero_thrusters()
app_processes = {}
for app_name in APPS_TO_START:
app_processes[app_name] = Popen(app_name)
sleep(2)
try:
main()
except Exception:
print "TERMINATING APPLICATIONS..."
for app in app_processes.itervalues:
app.terminate()
raise
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():
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():
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 hubConnect():
""" connecting to hub """
sw.loadConfig("../conf/seawolf.conf");
sw.init("missionControl : Main");
#!/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
import sys
import seawolf
sys.path.insert(0, '../mission_control')
import sw3
sys.path.insert(0, '../mission_control/sw3')
from pneumatics import missiles
seawolf.loadConfig("../conf/local-seawolf.conf")
seawolf.init("Pneumatics Test")
x = 1
while x:
x = raw_input(
'Number of the pin to test or the name of the part to test or q to quit: '
)
if (x == 'q'):
break
if len(x) < 2:
x = int(x)
sw3.nav.do(sw3.Fire(x))
def hubConnect():
""" Connecting to seawolf hub """
sw.loadConfig("../conf/seawolf.conf")
sw.init("visionServer : Main") #Taken from run.py in mission control which said missionControl : Main
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()
