def run(self,
point,
deadband=(15, 15),
px=.001,
ix=0,
dx=0,
py=.001,
iy=0,
dy=0,
target=(510, 510),
modulo_error=False):
point = evaluate_or_call(point)
self.log('Running ForwardTarget on ({0}, {1}).'.format(
point[0], point[1]))
self.pid_loop_x(input_value=point[0],
p=px,
i=ix,
d=dx,
target=target[0],
deadband=deadband[0],
negate=True)
self.pid_loop_y(input_value=point[1],
p=py,
i=iy,
d=dy,
target=target[1],
deadband=deadband[1],
negate=True)
if finished(self.pid_loop_x, self.pid_loop_y):
self._finish()
def loop(self):
try:
self.log_socket = zmq.Context().socket(zmq.PUB)
self.log_socket.bind('tcp://*:6667')
self.log('', '(initialize socket)', '')
time.sleep(0.2)
self.running.value = True
self.last = time.time()
while True:
if finished(self.task):
self.log('', 'Mission finished!', 'CRITICAL')
self.kill()
break
self.log(
'',
'Running task {0} with args {1} and kwargs {2}.'.format(
repr(self.task), self.args, self.kwargs), 'DEBUG')
self.task(*self.args, **self.kwargs)
# Calculate time to sleep to target a rate
self.telapsed = time.time() - self.last
# print('EST RATE: {0}'.format(1 / self.telapsed))
self.last = time.time()
if not self.pause.value:
self.run()
elif self.step.value:
self.step.value = False
self.run()
time.sleep(max(0, 0.005 - (time.time() - self.last)))
except KeyboardInterrupt:
self.kill()
except Exception as e:
self.handle(e)
def run(self,
point,
deadband=(15, 15),
px=.0005,
ix=0,
dx=0,
py=.001,
iy=0,
dy=0,
target=(512, 384),
min_out=None):
point = evaluate_or_call(point)
self.pid_loop_x(input_value=point[0],
p=px,
i=ix,
d=dx,
target=target[0],
deadband=deadband[0],
min_out=min_out)
self.pid_loop_y(input_value=point[1],
p=py,
i=iy,
d=dy,
target=target[1],
deadband=deadband[1],
min_out=min_out)
if finished(self.pid_loop_x, self.pid_loop_y):
self._finish()
def run(self, subtask):
subtask()
if finished(subtask):
shm.desires.speed.set(0)
shm.desires.sway_speed.set(0)
shm.desires.heading.set(shm.kalman.heading.get())
self._finish()
def run(self, *tasks, subtasks=()):
subtasks = tasks + tuple(subtasks)
for t in subtasks:
t()
if finished(*subtasks):
self._finish()
def run(self, *args, **kwargs):
if time.time() >= self.deadline:
self._finish()
return
self.subtask()
if finished(self.subtask):
self._finish()
def run(self, *tasks, subtasks=()):
subtasks = tasks + tuple(subtasks)
for task in subtasks:
task()
if not finished(task):
return
self._finish()
def run(self, *args, **kwargs):
try:
self.subtask()
if finished(self.subtask):
self._finish()
except BecomeException as e:
self.subtask = e.x
def run(self, master, *tasks, subtasks=()):
subtasks = tasks + tuple(subtasks)
master()
if finished(master):
for t in subtasks:
t._finish()
self._finish()
return
for t in subtasks:
t()
def run(self,
angle,
deadband=3,
p=.001,
i=0,
d=0,
target=0,
modulo_error=True):
angle = evaluate_or_call(angle)
self.pid_loop_heading(input_value=angle,
p=p,
i=i,
d=d,
target=target,
deadband=deadband,
negate=True)
if finished(self.pid_loop_heading):
self._finish()
def run(self, distance, deadband=.1, p=1, i=0, d=0):
"""
:param distance: The distance to move. Positive values will move the sub right.
:param deadband: The accepted error for when the task should finish. If the sub has moved forward in the range
[distance - deadband, distance + deadband], then the task will finish.
"""
self.total_distance += kalman.vely.get() * (self.this_run -
self.last_run)
distance = evaluate_or_call(distance)
self.pid_loop_y(input_value=self.total_distance,
p=p,
i=i,
d=d,
deadband=deadband,
target=distance)
if finished(self.pid_loop_y):
self._finish()
def run(self):
# XXX todo global timeout, but should be at a higher level...
self.old_state = self.state
if self.state is State.APPROACHING:
log("state", "approaching")
if not finished(self.approach_task):
self.approach_task()
else:
self.set_SURVEYING_ALIGNED()
elif self.state is State.SURVEYING:
if time.time() - self.survey_start >= SURVEY_TIMEOUT:
drop_dropper()
drop_dropper()
self.fail("Survey timed out. ")
return
if len(self.todo) == 0:
shm.desires.heading.set(self.initial_heading)
log("mission accomplished")
self._finish("Mission accomplished!")
return
if not finished(self.survey_task):
log("state", "Surveying")
self.survey_task()
else:
decided = False
handle = shm.shape_handle.get()
if handle.p != 0:
if Goal.LID in self.todo:
self.set_REMOVING(handle.x, handle.y, normalized_hd())
lower_grabber()
decided = True
if not decided:
primary = DROP_PRIMARY_GROUP.get()
if primary.p != 0 and Goal.PRIMARY in self.todo:
self.set_DROPPING(Goal.PRIMARY, primary.x, primary.y)
decided = True
if not decided:
secondary = DROP_SECONDARY_GROUP.get()
if secondary.p != 0 and Goal.SECONDARY in self.todo:
self.set_DROPPING(Goal.SECONDARY, secondary.x,
secondary.y)
decided = True
if not decided:
self.set_SURVEYING_ALIGNED()
else:
self.survey_start_north = shm.kalman.north.get()
self.survey_start_east = shm.kalman.east.get()
print("Initial position saved: %f, %f" %
(self.survey_start_north, self.survey_start_east))
elif self.state is State.REMOVING:
if time.time() - self.remove_start >= REMOVE_TIMEOUT:
log("removing timed out, removing to surveying")
self.set_SURVEYING_ALIGNED()
elif not finished(self.remove_positioning_task):
self.remove_positioning_task()
else:
# XXX Debugging
#self._finish()
# XXX end
self.set_DISCARDING()
elif self.state is State.DROPPING:
global grabber_lowered
# Do this here so there is some sort of delay.
# We don't want to raise immediately after dropping
# in case it gets stuck...
if grabber_lowered:
raise_grabber()
grabber_lowered = False
if time.time() - self.drop_start >= DROP_TIMEOUT:
log("drop timed out, returning to surveying")
self.set_SURVEYING_ALIGNED()
elif not finished(self.drop_positioning_task):
self.drop_positioning_task()
else:
if self.drop_target_group is DROP_PRIMARY_GROUP:
self.todo.remove(Goal.PRIMARY)
else:
self.todo.remove(Goal.SECONDARY)
self.set_SURVEYING_ALIGNED()
elif self.state is State.DISCARDING:
elapsed = time.time() - self.discard_start
if elapsed < DISCARD_WAIT_S:
log("discard", "waiting...")
shm.desires.speed.set(0)
shm.desires.sway_speed.set(0)
elif elapsed < DISCARD_WAIT_S + DISCARD_DEPART_S:
log("discard", "going out..")
shm.desires.sway_speed.set(DISCARD_SPEED)
elif elapsed < DISCARD_WAIT_S + DISCARD_DEPART_S + 2:
if not self.discard_dropped:
log("dropping")
drop_handle()
self.discard_dropped = True
log("discard", "turning around...")
shm.desires.sway_speed.set(0)
elif elapsed < DISCARD_WAIT_S + DISCARD_DEPART_S * 2 + 2:
log("discard", "coming back...")
shm.desires.sway_speed.set(-DISCARD_SPEED)
elif elapsed < DISCARD_WAIT_S + DISCARD_DEPART_S * 2 + 2:
shm.desires.sway_speed.set(0)
else:
# grabber raise delayed in dropping
# no longer center on it
shape_groups.remove(shm.shape_handle)
self.todo.remove(Goal.LID)
shm.desires.sway_speed.set(0)
# at this point we should be aligned to the line of things
# (because we picked up the handle and swayed back and forth)
shm.desires.heading.set(self.initial_heading)
self.set_SURVEYING_ALIGNED()
else:
log("invalid state")
self.log("Invalid state.")
self._finish()
if self.state is not self.old_state:
log(self.old_state.name + " -> " + self.state.name)
def run(self, north, east):
if not finished(self.rp):
self.rp()
else:
self._finish()
def run(self, subtask, sec):
if time.time() > self.deadline or finished(subtask):
self._finish()
else:
subtask()
def run(self, *args, **kwargs):
self.subtask()
if finished(self.subtask):
self._finish()
