def __init__(self, robot): CommandGroup.__init__(self, name='Command_Climb_Down') print("climber down command group init!!") # retract as soon as released maybe self.robot = robot self.addSequential(Do_Little_Climb_Down(robot)) self.addSequential(Do_Big_Climb_Down(robot))
def flowcontrolWHILE(func): parent = _getCommandGroup() source = _getSource(parent) try: parentLoop = source._currentLoop except AttributeError: parentLoop = None cg = CommandGroup(func.__name__) cg._source = source # Set the current loop for any BREAK statements source._currentLoop = cg func(cg) source._currentLoop = parentLoop def cancelLoop(self): self.getGroup().forceCancel = True end = Command("END WHILE") end.initialize = cancelLoop.__get__(end) cond = ConditionalCommand("flowcontrolWHILE", cg, end) cond.condition = condition cond.forceCancel = False cond.isFinished = _restartWhile.__get__(cond) cond._parentLoop = parentLoop parent.addSequential(cond) cg.conditionalCommand = cond
def __init__(self, robot): CommandGroup.__init__(self, name='Command_Bad_Auto') print("command_bad_auto init!!") self.requires(robot.drivetrain) self.drivetrain = robot.drivetrain self.addSequential(Do_Bad_Auto(robot)) self.addSequential(Command_Shoot(robot))
def __init__(self): CommandGroup.__init__(self, "SeqCommandGr") self.setInterruptible(True) self.addSequential(SampCommand()) self.addSequential(TestCommand()) self.addSequential(SampCommand())
def __init__(self): CommandGroup.__init__(self, "paraCommandGr") self.setInterruptible(False) self.addParallel(SampCommand()) self.addParallel(TestCommand()) self.addParallel(InstaCommand())
def __init__(self, robot, timeout=None): CommandGroup.__init__(self, name='auto_bounce') # bounce self.addSequential( AutonomousRamseteSimple( robot, path='bounce_pw1', relative=True, reset_telemetry=True)) # initial move forwards self.addSequential( AutonomousRamseteSimple( robot, path='bounce_pw2', relative=False, reset_telemetry=False)) # second move reverse self.addSequential( AutonomousRamseteSimple( robot, path='bounce_pw3', relative=False, reset_telemetry=False)) # third more forwards self.addSequential( AutonomousRamseteSimple( robot, path='bounce_pw4', relative=False, reset_telemetry=False)) # fourth move backwards
def _buildCommandGroup(func, parent): """Turns the given function into a full CommandGroup.""" source = _getSource(parent) cg = CommandGroup(func.__name__) cg._source = source func(cg) return cg
def __init__(self, robot): CommandGroup.__init__(self, name='Command_Intake_Pickup') print("command group intake pickup init!!") # retract as soon as released maybe self.robot = robot #XXX just combined do_four_bar into the Do_Intake command since # they're the same subsystem self.addSequential(Do_Four_Bar(robot)) self.addParallel(Do_Intake(robot)) self.addParallel(Do_Carrier(robot))
def isFinished(self): if CommandGroup.isFinished(self): if not self.whileCondition(): return True self.start() return False
def __init__(self, robot, timeout=None): CommandGroup.__init__(self, name='auto_barrel') # slalom relative_angles = False if relative_angles: self.addSequential( AutonomousDrivePID(robot, setpoint=0.8, timeout=3)) # initial move self.addSequential(AutonomousRotate(robot, setpoint=-55, timeout=3)) # first turn else: # use absolute heading # slalom with absolute angles E=0, S=90, W=180, N=-90 or 270 diagonal_distance = 1.8 self.addSequential(AutonomousDrivePID(robot, setpoint=6, timeout=3)) # initial move self.addSequential( AutonomousRotate(robot, setpoint=180, absolute=True, timeout=3)) # first turn
def getFollowCommand(self, armControl): commandGroup = CommandGroup() commandGroup.addSequential( PrintCommand('Following path ' + self.FilePath + '...')) for point in self.PathPoints: command = ArmCommand(armControl, point) commandGroup.addSequential(command) commandGroup.addSequential(PrintCommand('Path complete!')) return commandGroup
def _processCommand(self, cmd): if cmd is None: return [] with self.mutex: if self.locked: raise ValueError("Cannot add conditions to ConditionalCommand") for reqt in cmd.getRequirements(): self.requires(reqt) cmd.setParent(self) cmd = CommandGroup.Entry(cmd, CommandGroup.Entry.IN_SEQUENCE, None) return [cmd]
def autonomousInit(self): ahrs = AHRS.create_spi() #ahrs.getYaw() ahrs.getAngle() oneGroup = CommandGroup( ) # CHANGED (parentheses are needed to create a CommandGroup object) # CHANGED: oneGroup.addSequential( DriveForwardCommand(self.leftFront, self.rightFront, self.leftBack, self.rightBack, 1.0, 1.0)) # speed, seconds oneGroup.addSequential( RotateCommand(self.leftFront, self.leftBack, self.rightFront, self.rightBack, self.ahrs, math.radians(45))) scheduler = Scheduler.getInstance() scheduler.add(oneGroup)
def __init__(self, drive: Drivetrain, elevator: Elevator, intake: Intake): super().__init__("ScaleOnly command") close_waypoints = [ Vector2(0, -10), Vector2(16, -10), Vector2(23, -7.5) ] far_waypoints = [ Vector2(0, -10), Vector2(20, -10), Vector2(20, 7), Vector2(23, 7.5) ] cruise = 0.6 acc = 0.6 margin = 3 / 12 lookahead = 2 drive_path_far = PursuitDriveCommand(acc=acc, cruise_speed=cruise, waypoints=far_waypoints, drive=drive, dist_margin=margin, lookahead_base=lookahead) drive_path_close = PursuitDriveCommand(acc=acc, cruise_speed=cruise, waypoints=close_waypoints, drive=drive, dist_margin=margin, lookahead_base=lookahead) drive_path_far_flipped = PursuitDriveCommand( acc=acc, cruise_speed=cruise, waypoints=pursuit.flip_waypoints_y(far_waypoints), drive=drive, dist_margin=margin, lookahead_base=lookahead) drive_path_close_flipped = PursuitDriveCommand( acc=acc, cruise_speed=cruise, waypoints=pursuit.flip_waypoints_y(close_waypoints), drive=drive, dist_margin=margin, lookahead_base=lookahead) drive_path_chooser = ConditionalCommand( "StandardScaleOnlySideCondition") drive_path_chooser.onFalse = drive_path_far drive_path_chooser.onTrue = drive_path_close drive_path_chooser.condition = lambda: game_data.get_scale_side( ) == game_data.Side.RIGHT drive_path_flip_chooser = ConditionalCommand( "FlipScaleOnlySideCondition") drive_path_flip_chooser.onFalse = drive_path_close_flipped drive_path_flip_chooser.onTrue = drive_path_far_flipped drive_path_flip_chooser.condition = lambda: game_data.get_scale_side( ) == game_data.Side.RIGHT drive_flip_chooser = ConditionalCommand("DriveFlipCondition") drive_flip_chooser.condition = lambda: game_data.get_robot_side( ) == game_data.Side.RIGHT drive_flip_chooser.onTrue = drive_path_chooser drive_flip_chooser.onFalse = drive_path_flip_chooser elevator_condition = WaitUntilConditionCommand( lambda: pose_estimator.get_current_pose().x > 16) elevator_to_height = MoveElevatorCommand(elevator, ElevatorPositions.SWITCH) elev_group = CommandGroup() elev_group.addSequential(elevator_condition) if not hal.isSimulation(): elev_group.addSequential(elevator_to_height) else: elev_group.addSequential(PrintCommand("Elevator moving")) intake_out = MoveIntakeCommand(intake, ArmState.DOWN) drop_cube = TimedRunIntakeCommand(intake, time=0.5, power=-intake.power) drive_back = TimeDriveCommand(drive, time=0.2, power=-0.5) spin_chooser = ConditionalCommand("SpinChooser") spin_chooser.condition = lambda: game_data.get_scale_side( ) == game_data.Side.RIGHT spin_chooser.onTrue = TurnToAngle(drive, 180, delta=False) spin_chooser.onFalse = TurnToAngle(drive, -180, delta=False) switch_path_close = [Vector2(20, -10), Vector2(19, -8)] switch_path_far = [Vector2(20, -10), Vector2(18, 7)] switch_path_chooser = ConditionalCommand("SwitchPathChooser") switch_path_chooser.condition = lambda: game_data.get_own_switch_side( ) == game_data.get_robot_side() switch_path_chooser.onTrue = PursuitDriveCommand( drive, switch_path_close, cruise, acc) switch_path_chooser.onFalse = PursuitDriveCommand( drive, switch_path_far, cruise, acc) self.addParallel(elev_group) self.addSequential(drive_flip_chooser) self.addSequential(intake_out) self.addSequential(drop_cube) self.addSequential(drive_back) if not hal.isSimulation(): self.addParallel(MoveElevatorCommand(elevator, 0)) else: self.addParallel(PrintCommand("Elevator moving")) self.addSequential(spin_chooser) self.addSequential(switch_path_chooser)
def robotInit(self): # Start up continuous processes # In simulation, cd is code dir. On the robot, it's something else so we need to use abs dir basedir = get_basedir() dashboard2.run(basedir) DashboardUpdateCommand().start() OIUpdateCommand().start() # CheckFaults([self.elevator, self.forklift]).start() wpilib.CameraServer.launch('vision.py:main') dashboard2.add_graph("Speed", lambda: 0) dashboard2.add_graph("CTE", lambda: 0) dashboard2.add_graph("Lookahead", lambda: 0) dashboard2.add_graph("Goal Distance", lambda: 0) # Actions elev_manual_command = OpElevatorManualCommand(self.elevator) elev_move_to_top = MoveElevatorCommand(self.elevator, 62) elev_move_to_bottom = MoveElevatorCommand(self.elevator, 0) elev_zero = ElevatorZeroCommand(self.elevator) elev_intake_switch = CommandGroup() elev_intake_switch.addSequential(MoveElevatorCommand( self.elevator, 30)) elev_intake_switch.addSequential( MoveIntakeCommand(self.intake, ArmState.DOWN)) intake_open = OpenIntakeCommand(self.intake) intake_toggle = ToggleIntakeCommand(self.intake) intake_run = RunIntakeCommand(self.intake) drop_forks = DropForkliftCommand(self.forklift) climb_cmd = Climb(climber=self.forklift, elevator=self.elevator) OI.get().exec_while_condition( condition=OI.get().elevator_is_manual_control, cmd=elev_manual_command) OI.get().add_action_listener( condition=OI.get().elevator_move_to_bottom, action=elev_move_to_bottom.start) OI.get().add_action_listener(condition=OI.get().elevator_move_to_top, action=elev_move_to_top.start) OI.get().add_action_listener( condition=OI.get().elevator_move_to_switch, action=elev_intake_switch.start) OI.get().add_action_listener(condition=OI.get().elevator_zero, action=elev_zero.start) OI.get().add_action_listener(condition=OI.get().do_drop_forks, action=drop_forks.start) OI.get().exec_while_condition(condition=OI.get().do_climb, cmd=climb_cmd) OI.get().add_action_listener(condition=OI.get().toggle_arm, action=intake_toggle.start) OI.get().exec_while_condition(condition=OI.get().arm_is_open, cmd=intake_open) OI.get().exec_while_condition(condition=OI.get().intake_is_active, cmd=intake_run) self.side_chooser = dashboard2.add_chooser("Starting Position") self.side_chooser.add_option("Left", game_data.Side.LEFT) self.side_chooser.add_option("Center", game_data.Side.CENTER) self.side_chooser.add_option("Right", game_data.Side.RIGHT) self.side_chooser.set_default("Center") # Auto modes auto_switch_only = SwitchOnlyMonolith(drive=self.drivetrain, elevator=self.elevator, intake=self.intake) auto_scale_only = ScaleOnly(drive=self.drivetrain, elevator=self.elevator, intake=self.intake) auto_scale_double = DoubleScale(drive=self.drivetrain, elevator=self.elevator, intake=self.intake) auto_switch_scale = ScaleSwitchSameSide(drive=self.drivetrain, elevator=self.elevator, intake=self.intake) # auto_vault = VaultOnly(drive=self.drivetrain, intake=self.intake) auto_drive_simple = TimeDriveCommand(drive=self.drivetrain, power=0.3, time=3) self.auto_chooser = dashboard2.add_chooser("Autonomous") self.auto_chooser.add_option("Switch Only", auto_switch_only) self.auto_chooser.add_option("Scale Only", auto_scale_only) self.auto_chooser.add_option("2x Scale", auto_scale_double) self.auto_chooser.add_option("Scale -> Switch", auto_switch_scale) # self.auto_chooser.add_option("Switch and Scale", auto_switch_scale) # self.auto_chooser.add_option("Vault Only", auto_vault) self.auto_chooser.add_option( "Drive Forward", PursuitDriveCommand(acc=4, cruise_speed=8, waypoints=[Pose(1.5, 0, 0), Pose(10, 0, 0)], drive=self.drivetrain)) self.auto_chooser.set_default("Drive Forward")
def __init__(self, robot, timeout=None): CommandGroup.__init__(self, name='auto_slalom') # slalom relative_angles = False if relative_angles: self.addSequential( AutonomousDrivePID(robot, setpoint=0.8, timeout=3)) # initial move from start self.addSequential(AutonomousRotate(robot, setpoint=-55, timeout=3)) # first CCW turn self.addSequential( AutonomousDrivePID(robot, setpoint=2.25, timeout=3)) # first diagonal self.addSequential(AutonomousRotate( robot, setpoint=55, timeout=3)) # first CW turn on top self.addSequential( AutonomousDrivePID(robot, setpoint=3.0, timeout=3)) # cross the top self.addSequential(AutonomousRotate(robot, setpoint=55, timeout=3)) # aim down self.addSequential( AutonomousDrivePID(robot, setpoint=2.1, timeout=3)) # drive down self.addSequential(AutonomousRotate(robot, setpoint=-55, timeout=3)) # turn back self.addSequential(AutonomousDrivePID( robot, setpoint=1, timeout=3)) # drive along bottom self.addSequential(AutonomousRotate(robot, setpoint=-90, timeout=3)) # point up self.addSequential( AutonomousDrivePID(robot, setpoint=1.7, timeout=3)) # drive up self.addSequential(AutonomousRotate( robot, setpoint=-90, timeout=3)) # face back to start self.addSequential( AutonomousDrivePID(robot, setpoint=1.0, timeout=3)) # cross back over top self.addSequential(AutonomousRotate(robot, setpoint=-60, timeout=3)) # CCW point down self.addSequential( AutonomousDrivePID(robot, setpoint=1.9, timeout=3)) # drive down diagonal self.addSequential(AutonomousRotate( robot, setpoint=65, timeout=3)) # face back to home self.addSequential( AutonomousDrivePID(robot, setpoint=3.4, timeout=3)) # cross the bottom self.addSequential(AutonomousRotate(robot, setpoint=52, timeout=3)) # point back up self.addSequential( AutonomousDrivePID(robot, setpoint=1.9, timeout=3)) # drive up diagonal self.addSequential(AutonomousRotate( robot, setpoint=-60, timeout=3)) # CCW turn to finish self.addSequential( AutonomousDrivePID(robot, setpoint=1.0, timeout=3)) # drive on finish area else: # use absolute heading # slalom with absolute angles E=0, S=90, W=180, N=-90 or 270 diagonal_distance = 2.1 self.addSequential( AutonomousDrivePID(robot, setpoint=0.7, timeout=3)) # initial move from start self.addSequential( AutonomousRotate(robot, setpoint=-55, absolute=True, timeout=3)) # first CCW turn self.addSequential( AutonomousDrivePID(robot, setpoint=diagonal_distance, timeout=3)) # first diagonal self.addSequential( AutonomousRotate(robot, setpoint=0, absolute=True, timeout=3)) # first CW turn on top self.addSequential( AutonomousDrivePID(robot, setpoint=2.8, timeout=3)) # cross the top self.addSequential( AutonomousRotate(robot, setpoint=55, absolute=True, timeout=3)) # aim down self.addSequential( AutonomousDrivePID(robot, setpoint=0.9 * diagonal_distance, timeout=3)) # drive down self.addSequential( AutonomousRotate(robot, setpoint=0, absolute=True, timeout=3)) # turn E self.addSequential( AutonomousDrivePID(robot, setpoint=1.0, timeout=3)) # drive along bottom self.addSequential( AutonomousRotate(robot, setpoint=-90, absolute=True, timeout=3)) # point up self.addSequential( AutonomousDrivePID(robot, setpoint=1.5, timeout=3)) # drive up self.addSequential( AutonomousRotate(robot, setpoint=180, absolute=True, timeout=3)) # face back to start self.addSequential( AutonomousDrivePID(robot, setpoint=1.2, timeout=3)) # cross back over top self.addSequential( AutonomousRotate(robot, setpoint=115, absolute=True, timeout=3)) # SW point down self.addSequential( AutonomousDrivePID(robot, setpoint=0.8 * diagonal_distance, timeout=3)) # drive down diagonal self.addSequential( AutonomousRotate(robot, setpoint=180, absolute=True, timeout=3)) # face back to home self.addSequential( AutonomousDrivePID(robot, setpoint=3.3, timeout=3)) # cross the bottom self.addSequential( AutonomousRotate(robot, setpoint=-115, absolute=True, timeout=3)) # point back NW self.addSequential( AutonomousDrivePID(robot, setpoint=0.75 * diagonal_distance, timeout=3)) # drive up diagonal self.addSequential( AutonomousRotate(robot, setpoint=180, absolute=True, timeout=3)) # CCW turn to finish self.addSequential( AutonomousDrivePID(robot, setpoint=1.0, timeout=3)) # drive on finish area
def get_scale_only_group(drive, elevator, intake): group = CommandGroup() is_close = game_data.get_robot_side() == game_data.get_scale_side() if game_data.get_robot_side() == Side.LEFT: if is_close: drive_command = close_drive_flipped else: drive_command = far_drive_flipped else: if is_close: drive_command = close_drive else: drive_command = far_drive elev_wait = TimedCommand(name="Elev Timeout", timeoutInSeconds=(0.5 if is_close else 2)) elevator_to_height = MoveElevatorCommand(elevator, 60) elev_group = CommandGroup() elev_group.addSequential(elev_wait) if not hal.isSimulation(): elev_group.addSequential(elevator_to_height) else: elev_group.addSequential(PrintCommand("Elevator moving")) drop_cube = SetIntakeCommand(intake, GrabState.OUT) drive_back = DistanceDriveCommand(drive=drive, power=-0.2, distance=3) group.addParallel(elev_group) group.addSequential(drive_command) group.addSequential(drop_cube) group.addSequential(drive_back) return group
def __init__(self, robot): CommandGroup.__init__(self, name='Command_Shoot') print("Command_Shoot init!!") # Carrier is now moved to a manual button self.addParallel(Do_Shoot(robot)) self.addParallel(Do_Activate_Feeder(robot))
def __init__(self, name): CommandGroup.__init__(self, name) callingFlow = _getCommandFlow() self._source = getattr(callingFlow, "_source", self) self._ifStack = None
def setParent(self, parent): self._popIfStack() CommandGroup.setParent(self, parent)
def start(self): self._popIfStack() CommandGroup.start(self)
def addParallel(self, cmd, timeout=None): self._popIfStack() if timeout is None: CommandGroup.addParallel(self, cmd) else: CommandGroup.addParallel(self, cmd, timeout)
def addSequential(self, cmd, timeout=None): self._popIfStack() if timeout is None: CommandGroup.addSequential(self, cmd) else: CommandGroup.addSequential(self, cmd, timeout)