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)