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 maybe_add_roll(self, original, roll_direction):
tasks = [original]
if self.roll_extension:
roll = -roll_direction * 45
tasks.extend([Roll(roll, error=10), Timer(1.0), Roll(0, error=3)])
return Sequential(*tasks)
self.finished = False
task()
pv = shm.settings_roll.kP.get()
rolly_roll = lambda:\
Sequential(
FunctionTask(lambda: shm.settings_roll.kP.set(.6)),
MasterConcurrent(
RollDegrees(360 * 2 - 180),
RelativeToCurrentRoll(90),
VelocityX(0)
),
Timer(1),
FunctionTask(lambda: shm.settings_roll.kP.set(pv)),
Roll(0, error=10)
)
def focus_elem(elem_x, offset=0):
return HeadingTarget(
point=[lambda: elem_x().get(), 0],
target=lambda: [shm.gate.img_width.get() / 2 + offset, 0],
px=0.3,
deadband=(20, 1))
focus_left = lambda: focus_elem(lambda: shm.gate.leftmost_x)
focus_middle = lambda: focus_elem(lambda: shm.gate.middle_x)
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
),
),
)
self.progress = 0
def on_run(self, N):
new_angle = shm.kalman.roll.get()
angle_diff = new_angle - self.current_r
self.current_r = new_angle
Ad = ((angle_diff + 180) % 360) - 180
self.progress += Ad if N > 0 else -Ad
if self.progress > abs(N): self.finish()
pv = shm.settings_roll.kP.get()
roll = lambda: Sequential(
FunctionTask(lambda: shm.settings_roll.kP.set(.6)),
#*([RelativeToInitialRoll(90)] * 8),
#Timed(RelativeToCurrentRoll(90), 6),
MasterConcurrent(RollDegrees(720 - 45), RelativeToCurrentRoll(90),
VelocityX(.35)),
#MasterConcurrent(RollDegrees(360), RelativeToCurrentRoll(90), VelocityX(.35)),
#Timer(.5),
#MasterConcurrent(RollDegrees(360 - 45), RelativeToCurrentRoll(90), VelocityX(.35)),
#MasterConcurrent(RollDegrees(-720 + 45), RelativeToCurrentRoll(-90)),
FunctionTask(lambda: shm.settings_roll.kP.set(pv)),
Concurrent(
Roll(0),
VelocityX(0),
),
Timer(2),
)
t2 = Timed(RelativeToCurrentRoll(-90), 3)