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 addParallel(self, cmd, timeout=None):
self._popIfStack()
if timeout is None:
CommandGroup.addParallel(self, cmd)
else:
CommandGroup.addParallel(self, cmd, timeout)