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