def kill():
zero_motors()
depth_on.set(0)
pitch_on.set(0)
heading_on.set(0)
soft_kill.set(1)
def kill():
zero_motors()
depth_on.set(0)
pitch_on.set(0)
heading_on.set(0)
soft_kill.set(1)
def oscillate(m, x, t):
try:
while True:
m.set(x)
time.sleep(t)
m.set(-x)
time.sleep(t)
except:
zero_motors()
def oscillate(m, x, t):
try:
while True:
m.set(x)
time.sleep(t)
m.set(-x)
time.sleep(t)
except:
zero_motors()
def test_motor_floorit(log=sys.stdout, speed=1.0):
if check_soft_kill(log):
sleep(1.5/speed)
log.write("Flooring all motors...\n")
set_all_motors_from_seq([t.max_pwm for t in all_thrusters])
sleep(5/speed)
zero_motors()
sleep(1.5/speed)
log.write("Done\n")
def test_motor_floorit(log=sys.stdout, speed=1.0):
if check_soft_kill(log):
sleep(1.5/speed)
log.write("Flooring all motors...\n")
set_all_motors_from_seq([t.max_pwm for t in all_thrusters])
sleep(5/speed)
zero_motors()
sleep(1.5/speed)
log.write("Done\n")
def test_motor_spinup(log=sys.stdout, speed=1.0):
if check_soft_kill(log):
sleep(1.5/speed)
for motor in all_thrusters:
log.write("spinning %s to 127...\n" % motor.name)
motor.set(127)
sleep(1/speed)
sleep(1.5/speed)
zero_motors()
log.write("\tdone\n")
sleep(1.5/speed)
def test_motor_spinup(log=sys.stdout, speed=1.0):
if check_soft_kill(log):
sleep(1.5/speed)
for motor in all_thrusters:
log.write("spinning %s to 127...\n" % motor.name)
motor.set(127)
sleep(1/speed)
sleep(1.5/speed)
zero_motors()
log.write("\tdone\n")
sleep(1.5/speed)
def loop(kalman_watcher, quit_event):
# PID stepping class
pid = PIDLoop(speed=args['speed'])
pid.clean()
# Optimizer class
opt = Optimizer()
opt.DEBUG = bool(args['verbose'])
controller_enabled = True
tm = ThrusterManager()
kalman_watcher.watch(kalman)
# The main loop
while not quit_event.is_set():
if not args['rate']:
kalman_watcher.wait()
start_time = time.time()
g = kalman.get()
dt_qs = timed(tm.update, g)[1]
# Get passive forces and passive torques on the sub in model frame
passives, dt_pass = timed(vehicle.passive_forces, g, tm)
set_shm_wrench(control_passive_forces, passives)
# Handle controller enable / disable feature
if not settings_control.enabled.get() or \
switches.soft_kill.get() or switches.hard_kill.get():
# controller is disabled
if controller_enabled:
set_shm_wrench(control_internal_wrench, [0] * 6)
zero_motors()
controller_enabled = False
if args['rate']:
time.sleep(0.1)
continue
if not controller_enabled: # We have been re-enabled
controller_enabled = True
pid.clean()
# Execute one PID step for all PID controllers.
dt_pid = timed(pid.step)[1]
# Set motors from PID desires.
dt_opt = timed(opt.set_motors, tm, passives)[1]
total_time = time.time() - start_time
if args['verbose']:
times = [("Qs", dt_qs), ("Passives", dt_pass),
("PID", dt_pid), ("Optimizer", dt_opt), ("Total", total_time)]
for name, dt in times:
log("%s step ran in %0.2f HZ (%f ms)" % (name, (1/dt), 1000*dt), copy_to_stdout=True)
if args['rate']:
if total_time > step_time:
log(str(datetime.now()) + \
"## WARN: UNABLE TO MEET RATE. RUNNING AT %f HZ" % (1 / total_time), copy_to_stdout=True)
else:
if args['verbose']:
log("Rate capped at %f HZ" % args['rate'], copy_to_stdout=True)
log("-----", copy_to_stdout=True) #visual break
time.sleep(step_time - total_time)
pid.clean()
zero_motors()
from time import sleep
from control.thrusters import thrusters
from control.util import zero_motors
from shm import motor_desires
test_values = (-255,-190, -140, -100, 60, 0, 60, 100, 140, 190, 255)
#test_values = range(-128,128)
#test_values = (-255, -127, 0, 127, 255)
#test_values = (-255, 255)
def manage_thruster(thruster):
print("Thread to manage thruster {} started!".format(thruster.name))
while True:
sleep(random.uniform(1, 2))
speed = random.choice(test_values)
thruster.set(speed)
print("Setting thruster {} to speed {}.".format(thruster.name, speed))
for thruster in thrusters:
t = threading.Thread(target=manage_thruster, daemon=True, args=(thruster,))
t.start()
try:
while True:
sleep(120)
except:
zero_motors()
print("Exception caught, quitting gracefully")
def loop(kalman_watcher, quit_event):
# PID stepping class
pid = PIDLoop(speed=args['speed'])
pid.clean()
# Optimizer class
opt = Optimizer()
opt.DEBUG = bool(args['verbose'])
controller_enabled = True
tm = ThrusterManager()
kalman_watcher.watch(shm.kalman)
# The main loop
while not quit_event.is_set():
if not args['rate']:
kalman_watcher.wait()
start_time = time.time()
g = shm.kalman.get()
dt_qs = timed(tm.update, g)[1]
# Get passive forces and passive torques on the sub in model frame
passives, dt_pass = timed(vehicle.passive_forces, g, tm)
set_shm_wrench(shm.control_passive_forces, passives)
# Handle controller enable / disable feature
if not shm.settings_control.enabled.get() or \
shm.switches.soft_kill.get() or shm.switches.hard_kill.get():
# controller is disabled
if controller_enabled:
set_shm_wrench(shm.control_internal_wrench, [0] * 6)
zero_motors()
controller_enabled = False
if args['rate']:
time.sleep(0.1)
continue
if not controller_enabled: # We have been re-enabled
controller_enabled = True
pid.clean()
# Execute one PID step for all PID controllers.
dt_pid = timed(pid.step, tm)[1]
# Set motors from PID desires.
dt_opt = timed(opt.set_motors, tm, passives)[1]
total_time = time.time() - start_time
if args['verbose']:
times = [("Qs", dt_qs), ("Passives", dt_pass), ("PID", dt_pid),
("Optimizer", dt_opt), ("Total", total_time)]
for name, dt in times:
log("%s step ran in %0.2f HZ (%f ms)" %
(name, (1 / dt), 1000 * dt),
copy_to_stdout=True)
if args['rate']:
if total_time > step_time:
log(str(datetime.now()) + \
"## WARN: UNABLE TO MEET RATE. RUNNING AT %f HZ" % (1 / total_time), copy_to_stdout=True)
else:
if args['verbose']:
log("Rate capped at %f HZ" % args['rate'],
copy_to_stdout=True)
log("-----", copy_to_stdout=True) #visual break
time.sleep(step_time - total_time)
pid.clean()
zero_motors()
def loop(kalman_watcher, quit_event):
# PID stepping class
pid = PIDLoop(speed=args['speed'])
pid.clean()
# Optimizer class
opt = Optimizer()
opt.DEBUG = bool(args['verbose'])
controller_enabled = True
tm = ThrusterManager()
kalman_watcher.watch(kalman)
# The main loop
while not quit_event.is_set():
if not args['rate']:
kalman_watcher.wait()
start_time = time.time()
g = kalman.get()
tm.update(g)
# Get passive forces and passive torques on the sub in world space
passives = vehicle.passive_forces(g, tm)
set_shm_wrench(control_passive_forces, passives)
# Handle controller enable / disable feature
if not settings_control.enabled.get() or \
switches.soft_kill.get() or switches.hard_kill.get():
# controller is disabled
if controller_enabled:
zero_motors()
controller_enabled = False
if args['rate']:
time.sleep(0.1)
continue
if not controller_enabled: # We have been re-enabled
controller_enabled = True
pid.clean()
dt_pid = pid.step() # execute one PID step for all PID controllers
# Get the correct inertia tensor of the sub in world coordinates
I = vehicle.get_inertia_tensor(tm.orientation)
# Controller step
dt_opt = opt.set_motors(tm, passives, I)
total_time = time.time() - start_time
if args['verbose']:
sys.stdout.write("PID step ran in %0.2f HZ\n" % (1 / dt_pid))
sys.stdout.write("Optimizer ran in %0.2f HZ\n" % (1 / dt_opt))
sys.stdout.write("Total rate: %0.2f HZ\n" % (1 / total_time))
sys.stdout.flush()
if args['rate']:
total_time = dt_pid + dt_opt
if total_time > step_time:
print(str(datetime.now()) + \
"## WARN: UNABLE TO MEET RATE. RUNNING AT %f HZ" % (1 / total_time))
else:
if args['verbose']:
print("Rate capped at %f HZ" % args['rate'])
print("-----") #visual break
time.sleep(step_time - total_time)
pid.clean()
zero_motors()
def post(self, val):
"""Respond to POST requests by updating POSTed values and sending values back to the page"""
#length = int(self.headers.getheader('content-length'))
data_string = self.request.body
print "RECEIVED: %s" % data_string
log = MyLog();
global downwardCam
global visionThread
global visionBG
global updateImages
global runVision
if ":" in data_string: # set a value
k,v = data_string.split(":")
v_set(k.strip(),v.strip())
self.write(kv(k))
if k.strip()=="controller":
print >> log, "Controller set to " + v.strip()
elif "#" in data_string: # trigger a control function
k,v = data_string.split("#")
actControl = v.strip()
log = MyLog();
if actControl=="torpedo_left":
print >> log, "Firing Left Torpedo..."
torpedo_left.set(1)
time.sleep(0.5)
torpedo_left.set(0)
elif actControl=="torpedo_right":
print >> log, "Firing Right Torpedo..."
torpedo_right.set(1)
time.sleep(0.5)
torpedo_right.set(0)
elif actControl=="marker_1":
print >> log, "Firing Marker 1..."
marker_1.set(1)
time.sleep(0.5)
marker_1.set(0)
elif actControl=="marker_2":
print >> log, "Firing Marker 2..."
marker_2.set(1)
time.sleep(0.5)
marker_2.set(0)
elif actControl=="grabber_port_grab":
print >> log, "Engaging Port Grabber..."
grabber_port_grab.set(1)
time.sleep(0.5)
grabber_port_grab.set(0)
elif actControl=="grabber_port_release":
print >> log, "Disengaging Port Grabber..."
grabber_port_release.set(1)
time.sleep(0.5)
grabber_port_release.set(0)
elif actControl=="grabber_starboard_grab":
print >> log, "Engaging Port Grabber..."
grabber_starboard_grab.set(1)
time.sleep(0.5)
grabber_starboard_grab.set(0)
elif actControl=="grabber_starboard_release":
print >> log, "Disengaging Port Grabber..."
grabber_starboard_release.set(1)
time.sleep(0.5)
grabber_starboard_release.set(0)
elif actControl=="grabber_aft_grab":
print >> log, "Engaging Port Grabber..."
grabber_aft_grab.set(1)
time.sleep(0.5)
grabber_aft_grab.set(0)
elif actControl=="grabber_aft_release":
print >> log, "Disengaging Port Grabber..."
grabber_aft_release.set(1)
time.sleep(0.5)
grabber_aft_release.set(0)
elif actControl == "servo_toggle":
stat = servo.get()
if stat:
print >> log, "Turning off servo"
servo.set(0)
else:
print >> log, "Setting servo PWM to 255"
servo.set(255)
elif actControl=="vision":
if not visionThread.isAlive():
print >> log, "Starting Vision..."
visionThread = KThread(target = visionHandler)
runVision = True
visionThread.start()
visionStart()
updateImages = 1
else:
print >> log, "Vision Stopping..."
visionThread.kill()
runVision = False
os.kill(visionBG.pid, signal.SIGINT) #kills vision background process nicely w/ interupt
updateImages = updateImages + 1 #update image to show "vision not enabled" message
elif actControl=="camtoggle":
if downwardCam:
downwardCam=False
else:
downwardCam=True
elif actControl == "camlr":
shm.camera_settings.right_camera.set(1 - shm.camera_settings.right_camera.get())
elif actControl=="halt":
print >> log, "Sub is now shutting down..."
time.sleep(1)
os.system("poweroff")
elif actControl=="reboot":
print >> log, "Sub is now rebooting..."
time.sleep(1)
os.system("reboot")
elif actControl=="kill":
shm.switches.soft_kill.set(True)
elif actControl=="unkill":
shm.switches.soft_kill.set(False)
elif data_string=="refresh": # send all vars to page
self.write(self.get_data_str())
elif data_string=="lcd_pr": #activate LCD_pr mode
global lcdPrThread
global lcdline1
global lcdline2
if not lcdPrThread.isAlive():
#lcdline1 = lcd_line1.get()
#lcdline2 = lcd_line2.get()
lcdPrThread.start()
print >> log, "LCD Trimming Mode Activated..."
else:
#kill it
lcdPrThread.kill()
lcdPrThread = None
lcdPrThread = KThread(target = lcd_pr)
#lcd_line1.set(lcdline1)
#lcd_line2.set(lcdline2)
print >> log, "LCD Trimming Mode Deactivated."
self.write("Done.")
elif data_string=="auv_self_test": # execute a self test
#Self Test Thread
global self_test_thread
global self_test_data
self_test_data = "Running..."
if not self_test_thread.isAlive():
self_test_thread = KThread(target = run_self_test)
self_test_thread.start()
print >> log, "Automated sensor test starting..."
elif data_string == "actuator_test":
global actuator_test_thread
if not actuator_test_thread.isAlive():
actuator_test_thread = KThread(target=run_actuator_test)
actuator_test_thread.start()
print >> log, "Automated actuator test starting..."
elif data_string=="mission_start_off": # executes a thruster test
shm.mission_start_switch.mission_start.set(False)
elif data_string=="thruster_test": # executes a thruster test
global thrusterThread
if not thrusterThread.isAlive():
#start thruster test
thrusterThread.start()
else:
#kill thruster test
thrusterThread.kill()
thrusterThread = None
thrusterThread = KThread(target=test_motor_dockside, kwargs = {'log': myLog, 'speed': 2.4}) #create a new KThread
zero_motors()
print >> log, "Thruster test aborted."
else: # toggle the boolean **(assumes that the variable is a boolean so varname:true is not necessary)
if data_string in shmvars:
v_set(data_string, v_get(data_string)^ True)
self.write(kv(data_string))
if(data_string=="soft_kill"):
print >> log, "Soft Kill set to " + str(v_get(data_string))
if(str(v_get(data_string))=="True"):
kill() #uses debug method to also set thrusters to 0 when killed
#stops vision if running
if visionThread.isAlive():
print "Vision Stopping for HTTP Server shutdown..."
visionThread.kill()
runVision = False
os.kill(visionBG.pid, signal.SIGINT) #kills vision background process nicely w/ interupt
#stops lcd_pr if running
if lcdPrThread.isAlive():
#kill it
lcdPrThread.kill()
lcdPrThread = None
lcdPrThread = KThread(target = lcd_pr)
lcd_line1.set(lcdline1)
lcd_line2.set(lcdline2)
#stops thruster test if running
if thrusterThread.isAlive():
#kill thruster test
thrusterThread.kill()
thrusterThread = None
thrusterThread = KThread(target=test_motor_dockside, kwargs={'log': myLog, 'speed': 2.4}) #create a new KThread
zero_motors()
time.sleep(0.2)
#stops server
io_loop.stop()
print "\n\nHTTP Server is now stopping..."