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():
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():
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():
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():
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"))
# 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("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():
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(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
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
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()
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
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()
#!/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()
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("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()
#!/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():
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();
#!/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")
# 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()