def on_run(self, test_success, test_fail, count, total, invert):
test_result_success = call_if_function(test_success)
test_result_fail = call_if_function(test_fail)
if self.checker_success.check(not test_result_success if invert else test_result_success):
self.finish(success=True)
elif self.checker_fail.check(not test_result_fail if invert else test_result_fail):
self.finish(success=False)
def on_run(self, velocity, desire_setter, current, target=None, error=-1, modulo_error=False, *args, **kwargs):
"""
Note: This does not 0 the desire when completed.
:param velocity: A Number or function that when called with no arguments returns a Number that represents the
value to be targeted. This target will be multiplied with the time in seconds from the last call and be added to
the current value.
:param desire_setter: A SHM variable (object with a set method) that will be called with a single argument to
target.
:param current: A Number or function that when called with no arguments returns the current value as a Number.
:param target: A Number (or function) that represents the target velocity (units/second).
:param error: A Number representing the allowed error before a wrapper is finished.
:param modulo_error: a Boolean that is true only if the error calculated should be with respect to modulo 360.
"""
velocity, current, target = call_if_function(velocity), call_if_function(current), call_if_function(target)
target_for_velocity = velocity * (self.this_run_time - self.last_run_time)
self.relative_to_current_setter.on_run(offset=target_for_velocity, desire_setter=desire_setter, current=current,
error=error, modulo_error=modulo_error)
if target is not None or within_deadband(target, current, error, use_mod_error=modulo_error):
self.finish()
else:
desire_setter()
def on_run(self,
offset,
desire_setter,
current,
default_error,
error=None,
modulo_error=False,
*args,
**kwargs):
"""
Note: This does not 0 the desire when completed.
:param offset: A Number or function that when called with no arguments returns a Number that represents the
value to be targeted. This offset will be added to the current value.
:param desire_setter: A SHM variable (object with a set method) that will be called with a single argument to
target.
:param current: A Number or function that when called with no arguments returns the current value as a Number.
:param error: A Number representing the allowed error before a wrapper is finished.
:param modulo_error: a Boolean that is true only if the error calculated should be with respect to modulo 360.
"""
if error is None:
error = default_error
offset, current = call_if_function(offset), call_if_function(current)
desire_setter(current + offset)
if within_deadband(current + offset,
current,
error,
use_mod_error=modulo_error):
self.finish()
def norm_to_vision_forward(x, y=None):
if y is None:
x, y = call_if_function(x)
else:
x, y = call_if_function(x), call_if_function(y)
w, h = shm.camera.forward_width.get(), shm.camera.forward_height.get()
return ((x / 2 + 0.5) * w, (y / 2 + 0.5) * h)
def vision_to_norm_downward(x, y=None):
if y is None:
x, y = call_if_function(x)
else:
x, y = call_if_function(x), call_if_function(y)
w, h = shm.camera.downward_width.get(), shm.camera.downward_height.get()
return ((x / w - 0.5) * 2, (y / h - 0.5) * 2)
def on_run(self,
point,
target,
deadband=(0.01875, 0.01875),
px=None,
ix=0,
dx=0,
py=None,
iy=0,
dy=0,
max_out=None,
valid=True,
min_target_x=None,
max_target_x=None,
min_target_y=None,
max_target_y=None,
*args,
**kwargs):
if px is None:
px = self.px_default
if py is None:
py = self.py_default
try:
max_out_x, max_out_y = call_if_function(max_out)
except:
max_out_x = max_out_y = call_if_function(max_out)
if valid:
point = call_if_function(point)
target = call_if_function(target)
self.pid_loop_x(input_value=point[0],
p=px,
i=ix,
d=dx,
target=target[0],
deadband=deadband[0],
max_out=max_out_x,
min_target=min_target_x,
max_target=max_target_x)
self.pid_loop_y(input_value=point[1],
p=py,
i=iy,
d=dy,
target=target[1],
deadband=deadband[1],
max_out=max_out_x,
min_target=min_target_y,
max_target=max_target_y)
else:
self.stop()
if self.pid_loop_x.finished and self.pid_loop_y.finished:
# TODO: Should the output be zeroed on finish?
self.finish()
def on_run(self, input_value, output_function, target=0, modulo_error=False, deadband=1, p=1, d=0, i=0,
negate=False, *args, **kwargs):
# TODO: minimum_output too?
input_value = call_if_function(input_value)
target = call_if_function(target)
output = self.pid.tick(value=input_value, desired=target, p=p, d=d, i=i)
output_function(-output if negate else output)
if within_deadband(input_value, target, deadband=deadband, use_mod_error=modulo_error):
# TODO: Should this zero on finish? Or set to I term?
self.finish()
def clamp_target_to_range(target, min_target=None, max_target=None):
target, min_target, max_target = call_if_function(
target), call_if_function(min_target), call_if_function(max_target)
if min_target is not None and max_target is not None and min_target > max_target:
raise Exception("min_target is greater than max_target")
if min_target is not None and target < min_target:
target = min_target
if max_target is not None and target > max_target:
target = max_target
return target
def on_run(self,
velocity,
desire_setter,
current,
default_error,
target=None,
error=None,
modulo_error=False,
min_target=None,
max_target=None,
*args,
**kwargs):
"""
Note: This does not 0 the desire when completed.
:param velocity: A Number or function that when called with no arguments returns a Number that represents the
value to be targeted. This target will be multiplied with the time in seconds from the last call and be added to
the current value.
:param desire_setter: A SHM variable (object with a set method) that will be called with a single argument to
target.
:param current: A Number or function that when called with no arguments returns the current value as a Number.
:param target: A Number (or function) that represents the target velocity (units/second).
:param error: A Number representing the allowed error before a wrapper is finished.
:param modulo_error: a Boolean that is true only if the error calculated should be with respect to modulo 360.
"""
if error is None:
error = default_error
velocity, current, target = call_if_function(
velocity), call_if_function(current), call_if_function(target)
target_for_velocity = velocity * (self.this_run_time -
self.last_run_time)
self.relative_to_current_setter.on_run(offset=target_for_velocity,
desire_setter=desire_setter,
current=current,
error=error,
modulo_error=modulo_error,
min_target=min_target,
max_target=max_target)
if target is not None or within_deadband(
target, current, error, use_mod_error=modulo_error):
if target is not None:
desire_setter()
self.finish()
else:
desire_setter()
def on_run(self, input_value, output_function, target=0, modulo_error=False, deadband=1, p=1, d=0, i=0,
negate=False, max_out=None, *args, **kwargs):
# TODO: minimum_output too?
input_value = call_if_function(input_value)
target = call_if_function(target)
if max_out is None:
max_out = float('inf')
output = self.pid.tick(value=input_value, desired=target, p=p, d=d, i=i)
output = math.copysign(min(abs(output), max_out), output)
output_function(-output if negate else output)
if within_deadband(input_value, target, deadband=deadband, use_mod_error=modulo_error):
# TODO: Should this zero on finish? Or set to I term?
self.finish()
def on_run(self, point, target, deadband=(15, 15), px=None, ix=0, dx=0, py=None, iy=0, dy=0, min_out=None, valid=True):
if px is None:
px = self.px_default
if py is None:
py = self.py_default
if valid:
point = call_if_function(point)
target = call_if_function(target)
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)
else:
self.stop()
if self.pid_loop_x.finished and self.pid_loop_y.finished:
# TODO: Should the output be zeroed on finish?
self.finish()
def on_run(self, search_task, visible, consistent_frames=(3, 5)):
visible = call_if_function(visible)
if self.found_checker.check(visible):
self.finish()
else:
search_task()
if search_task.finished:
self.finish(success=False)
def on_run(self, seconds, *args, **kwargs):
"""
Args:
seconds: The amount of seconds to be waited before finishing.
"""
self.seconds = call_if_function(seconds)
if (self.this_run_time - self.first_run_time) >= self.seconds:
self.finish()
def on_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 = call_if_function(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 self.pid_loop_x.finished and self.pid_loop_y.finished:
# TODO: Should the output be zeroed on finish?1
self.finish()
def on_run(self, marker):
marker = call_if_function(marker)
if PositionMarkers().unset(marker):
self.log('Marker {} unset'.format(marker), level='info')
self.finish(success=True)
return
self.log('Attempted to unset nonexistent marker {}'.format(marker), level='error')
self.finish(success=False)
def on_run(self, seconds, *args, **kwargs):
"""
Args:
seconds: The amount of seconds to be waited before finishing.
"""
self.seconds = call_if_function(seconds)
if (self.this_run_time - self.first_run_time) >= self.seconds:
self.finish()
def on_first_run(self, vector, deadband=0.1):
vector = call_if_function(vector)
self.use_task(
WithPositionalControl(
Concurrent(PositionN(target=vector[0], error=deadband),
PositionE(target=vector[1], error=deadband),
finite=False))
)
def on_run(self, offset, desire_setter, current, error=-1, modulo_error=False, *args, **kwargs):
"""
Note: This does not 0 the desire when completed.
:param offset: A Number or function that when called with no arguments returns a Number that represents the
value to be targeted. This offset will be added to the current value on the first run.
:param desire_setter: A SHM variable (object with a set method) that will be called with a single argument to target.
:param current: A Number or function that when called with no arguments returns the current value as a Number.
:param error: A Number representing the allowed error before a wrapper is finished.
:param modulo_error: a Boolean that is true only if the error calculated should be with respect to modulo 360.
"""
offset, current = call_if_function(offset), call_if_function(current)
if within_deadband(self.initial_value + offset, current, error, use_mod_error=modulo_error):
self.finish()
else:
desire_setter(self.initial_value + offset)
def on_run(self, desire, relative_desire, relative_deadband, *args, **kwargs):
current = shm.kalman.heading.get()
diff = heading_sub_degrees(call_if_function(desire), current)
if abs(diff) < relative_deadband:
self.finish()
return
relative = math.copysign(relative_desire, diff)
self.rel_curr_heading(relative)
def on_first_run(self, marker, deadband=0.2):
marker = call_if_function(marker)
target = PositionMarkers().get(marker)
if target is not None:
self.log('Moving to marker {} at {}'.format(marker, target), level='info')
self.use_task(MoveNE(target, deadband))
return
self.log('Attempted to move to nonexistent marker {}'.format(marker), level='error')
self.finish(success=False)
def on_run(self, point, deadband=(15, 15), px=.01, ix=0, dx=0, py=.001, iy=0, dy=0, target=(510, 510), modulo_error=False):
point = call_if_function(point)
self.logd('Running HeadingTarget 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 self.pid_loop_x.finished and self.pid_loop_y.finished:
# TODO: Should the output be zeroed on finish?1
self.finish()
def on_run(self,
test,
count,
total,
invert=False,
result=True,
*args,
**kwargs):
test_result = call_if_function(test)
if self.checker.check(not test_result if invert else test_result):
self.finish(success=self.result)
def on_run(self, seconds, task, restore, *args, **kwargs):
"""
Args:
seconds: The amount of seconds to be waited before finishing.
"""
if self.state == 'run':
self.seconds = call_if_function(seconds)
task()
if task.finished:
self.finish()
if (self.this_run_time - self.first_run_time) >= self.seconds:
self.state = 'restore'
return
if self.state == 'restore':
restore()
if restore.finished: self.finish()
def on_run(self,
angle,
deadband=0.05,
p=.8,
i=0,
d=0,
target=0,
modulo_error=True):
angle = call_if_function(angle)
self.pid_loop_heading(input_value=angle,
p=p,
i=i,
d=d,
target=target,
deadband=deadband,
negate=True)
if self.pid_loop_heading.finished:
self.finish()
def on_run(self, test, count, total, invert, result):
test_result = call_if_function(test)
if self.checker.check(not test_result if invert else test_result):
self.finish(success=result)
def on_run(self, seconds, function, value, *args, **kwargs):
self.timer()
if call_if_function(function) == value and self.timer.finished:
self.finish()
def on_run(self, message, level="info"):
self.log(call_if_function(message), level=level)
def on_run(self, search_task, visible, consistent_frames=(3, 5)):
visible = call_if_function(visible)
if self.found_checker.check(visible):
self.finish()
else:
search_task()
def on_run(self, angle, deadband=3, p=.001, i=0, d=0, target=0, modulo_error=True):
angle = call_if_function(angle)
self.pid_loop_heading(input_value=angle, p=p, i=i, d=d, target=target, deadband=deadband, negate=True)
if self.pid_loop_heading.finished:
self.finish()
def desired_heading():
return shm.kalman.heading.get() - (heading_sub_degrees(
call_if_function(desire),
call_if_function(current),
mod=mod
))
def on_run(self, depth, error=0.08, *args, **kwargs):
diff = call_if_function(depth) - shm.kalman.depth.get()
relative = math.copysign(self.RELATIVE_DESIRE, diff)
RelativeToCurrentDepth(relative)()
if abs(diff) < error:
self.finish()
def on_first_run(self, *args, **kwargs):
self.initial_value = call_if_function(kwargs['current'])
def begin_iteration(self, task_func, condition):
success = not hasattr(self, 'task') or self.task.success
if success and call_if_function(condition):
self.task = task_func()
else:
self.finish(success=success)
def on_run(self, marker, value=None):
marker = call_if_function(marker)
value = call_if_function(value)
self.log("Setting marker {} at {}".format(marker, PositionMarkers().set(marker, value)), level='info')
self.finish()
def on_first_run(self, *args, **kwargs):
super().on_first_run(*args, **kwargs)
self.initial_value = call_if_function(kwargs['current'])
