def on_run(self, *args):
if self.last_run_time is None:
return
dt = self.this_run_time - self.last_run_time
self.revolve_angle += self.degrees_per_second * dt
if self.revolve_angle > 390:
Heading(self.initial_heading)()
Pitch(0)()
Roll(0)()
self.finish()
return
rot_2d = rotate((0, 1), self.revolve_angle)
rotation_axis = np.array((rot_2d[0], rot_2d[1], 0))
conic_quat = quat_from_axis_angle(rotation_axis, self.conic_angle_rad)
# TODO How can we achieve the same effect without using heading?
heading, pitch, roll = conic_quat.hpr()
Heading(heading)()
Pitch(pitch)()
Roll(roll)()
def on_run(self, pitch=True, roll=True):
Depth(shm.kalman.depth.get())()
PositionN(shm.kalman.north.get(), positional_controls=None)()
PositionE(shm.kalman.east.get(), positional_controls=None)()
Pitch(0)() if pitch else Pitch(shm.kalman.pitch.get())()
Roll(0)() if roll else Roll(shm.kalman.roll.get())()
VelocityX(0, positional_controls=None)()
VelocityY(0, positional_controls=None)()
self.finish()
def on_run(self, *args):
if self.last_run_time is None:
return
dt = self.this_run_time - self.last_run_time
self.revolve_angle += self.degrees_per_second * dt
self.revolve_task(self.revolve_angle)
if self.revolve_angle > 360:
self.revolve_task(360)
Pitch(0)()
self.finish()
def pitch_pipe(grp):
return Sequential(
Depth(PIPE_SEARCH_DEPTH),
pitch_search_task(),
Zero(),
center(),
Pitch(0),
center(),
Depth(PIPE_FOLLOW_DEPTH),
Concurrent(
#center(),
align(),
finite=False,
),
PositionalControl(),
Zero(),
)
def on_first_run(self, *args, **kwargs):
self.use_task(
WithQuaternionControl(
Sequential(
Log('Time to celebrate!'),
Depth(3, error=0.2),
Pitch(90, error=5),
Log('Weeeeeeeeeee'),
MasterConcurrent(
self.CheckDepth(),
RelativeToCurrentDepth(-0.25),
RelativeToCurrentHeading(10),
),
FunctionTask(
lambda: shm.settings_control.enabled.set(False)),
Zero(),
)))
def on_first_run(self, angle, dps=45):
self.degrees_per_second = dps
self.revolve_angle = 0
self.revolve_task = RelativeToInitialHeading(
current=shm.kalman.heading.get())
Pitch(angle)()
def on_first_run(self, *args, **kwargs):
#heading = Heading((kalman.heading.get() + 180) % 360, error=1)
return Sequential(Pitch(0, error=1), Roll(0, error=1), Timer(1.5), Heading(lambda: kalman.heading.get() + 180, error=1), Timer(1))
def on_first_run(self, shm_group_getter, is_left, *args, **kwargs):
BOTH_DEPTH = settings.pick_up_both_depth
SEARCH_DEPTH_1 = settings.pick_up_search_depth_1
SEARCH_DEPTH_2 = settings.pick_up_search_depth_2
SEARCH_DEPTH_3 = settings.pick_up_search_depth_3
START_PICKUP_DEPTH = settings.pick_up_start_follow_depth
def downward_target_task(pt=(0, 0), deadband=(0.1, 0.1), max_out=0.04):
return DownwardTarget(
point=(lambda: shm_group_getter().center_x.get(), lambda: shm_group_getter().center_y.get()),
target=norm_to_vision_downward(*pt),
deadband=norm_to_vision_downward(-1.0 + deadband[0], -1.0 + deadband[1]),
px=0.0005,
py=0.001,
max_out=max_out,
)
def search_at_depth(depth, msg="", target=(0, 0), deadband=(0.1, 0.1), depth_timeout=20):
return Sequential(
timed(cons(Depth(depth)), depth_timeout),
cons(
downward_target_task(target, deadband=deadband),
success=0.80 * 2.5 * 60,
total=2.5 * 60,
debug=True,
),
stop(),
Log("Found at {} (depth={})".format(msg, depth)),
)
bottom_target = (0.8, -0.2) if is_left else (-0.7, -0.4)
self.use_task(
Sequential(
VisionSelector(downward=True),
# cons(Depth(BOTH_DEPTH)),
# cons(downward_target_task(max_out=0.1)),
# stop(),
# cons(Depth(SEARCH_DEPTH_1)),
# search_at_depth(SEARCH_DEPTH_1, "top", deadband=(0.1, 0.1), depth_timeout=15),
# search_at_depth(SEARCH_DEPTH_2, "mid", deadband=(0.05, 0.05), depth_timeout=10),
search_at_depth(SEARCH_DEPTH_3, "bot", target=bottom_target, deadband=(0.05, 0.05)),
# search_at_depth(3.5, "bot", target=bottom_target, deadband=(0.05, 0.05)),
Sequential(
*(timed(Depth(depth)) for depth in np.arange(SEARCH_DEPTH_3, START_PICKUP_DEPTH + 0.1, 0.1))
),
timed(
Concurrent(
Depth(START_PICKUP_DEPTH),
Roll(7.5 * (-1 if is_left else 1)),
Pitch(-7.5),
)
),
Log("PICKING UP??"),
timed(Depth(START_PICKUP_DEPTH + 0.3)),
Sequential(
*(timed(Depth(depth), timeout=1) for depth in np.arange(START_PICKUP_DEPTH, SEARCH_DEPTH_1 + 0.1, 0.1))
),
timed(
cons(
Concurrent(
Depth(SEARCH_DEPTH_1),
Roll(0),
Pitch(0),
)
),
15
),
),
)
search_task = lambda: SearchFor(
VelocityTSearch(forward=2, stride=3, rightFirst=PIPE_RIGHT_FIRST),
lambda: shm.pipe_results.heuristic_score.get() > 0,
consistent_frames=(10, 10))
pitch_search_task = lambda: SearchFor(PitchSearch(30),
lambda: shm.pipe_results.heuristic_score.
get() > 0,
consistent_frames=(6, 6))
pipe_test = lambda: Sequential(Depth(PIPE_SEARCH_DEPTH), search_task(), center(
), align(), Depth(PIPE_FOLLOW_DEPTH))
pitch_pipe_test = lambda: Sequential(
Depth(PIPE_SEARCH_DEPTH), pitch_search_task(), Zero(),
Concurrent(center(), Pitch(0)), Zero(), center(), align(),
Depth(PIPE_FOLLOW_DEPTH))
def check_seen():
visible = shm.pipe_results.heuristic_score.get()
#print(visible)
if visible > 0:
return True
else:
#print('Lost Pipe!')
return False
class Timeout(Task):
search_task= lambda: SearchFor(VelocityTSearch(forward=2,stride = 3, rightFirst=PIPE_RIGHT_FIRST),
lambda: shm.pipe_results.heuristic_score.get() > 0,
consistent_frames=(10, 10))
pitch_search_task = lambda: SearchFor(PitchSearch(30),
lambda: shm.pipe_results.heuristic_score.get() > 0,
consistent_frames=(6, 6))
pipe_test = lambda: Sequential(Depth(PIPE_SEARCH_DEPTH),
search_task(), center(), align(),
Depth(PIPE_FOLLOW_DEPTH))
pitch_pipe_test = lambda: Sequential(Depth(PIPE_SEARCH_DEPTH),
pitch_search_task(), Zero(),
Concurrent(center(), Pitch(0)), Zero(),
center(), align(), Depth(PIPE_FOLLOW_DEPTH))
def check_seen():
visible = shm.pipe_results.heuristic_score.get()
#print(visible)
if visible > 0:
return True
else:
#print('Lost Pipe!')
return False
class Timeout(Task):
def on_first_run(self, time, task, *args, **kwargs):
self.timer = Timer(time)
