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_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'])