RobotContainer.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import static frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator.visionMeasurementStdDevs2;

import java.io.File;

import choreo.auto.AutoFactory;
import choreo.auto.AutoRoutine;
import choreo.auto.AutoTrajectory;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.apriltags.ApriltagReading;
import frc.robot.commands.ResetAll;
import frc.robot.autochooser.AutoChooser;
import frc.robot.commands.AddApriltagReading;
import frc.robot.commands.AddGarbageReading;
import frc.robot.commands.AddTunableApriltagReading;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.DefaultAnglerControl;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.drive.DriveDirectionTime;
import frc.robot.commands.drive.DriveSwerve;
import frc.robot.commands.drive.FakeVision;
import frc.robot.commands.feeder.DefaultSpinFeeder;
import frc.robot.commands.feeder.SpinFeeder;
import frc.robot.commands.hopper.DefaultSpinHopper;
import frc.robot.commands.hopper.SpinHopper;
import frc.robot.commands.intake.SpinIntake;
import frc.robot.commands.intake.StopIntake;
import frc.robot.commands.intakeDeployment.SetDeploymentState;
import frc.robot.commands.intakeDeployment.ToggleDeployment;
import frc.robot.commands.parallels.RunHopperAndFeeder;
import frc.robot.commands.shooter.DefaultShooterControl;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.shooter.SpinShooter;
import frc.robot.commands.testing.RunDashboardShotTest;
import frc.robot.commands.turret.DefaultTurretControl;
import frc.robot.commands.turret.RunTurretToFwdLimit;
import frc.robot.commands.turret.RunTurretToRevLimit;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.commands.turret.TestSetTurretAngle;
import frc.robot.constants.Constants;
import frc.robot.constants.enums.DeploymentState;
import frc.robot.constants.enums.DriveDirection;
import frc.robot.constants.enums.ShootingState;
import frc.robot.constants.enums.ShootingState.ShootState;
import frc.robot.subsystems.AnglerSubsystem;
import frc.robot.subsystems.ApriltagSubsystem;
import frc.robot.subsystems.CameraThread;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.subsystems.ControllerSubsystem;
import frc.robot.subsystems.FeederSubsystem;
import frc.robot.subsystems.GyroSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeDeployerSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.LightStripSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.TurretSubsystem;
//import frc.robot.subsystems.RollerSubsystem;
//import frc.robot.subsystems.TiltSubsystem;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster;
import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster;
import frc.robot.utils.logging.io.gyro.RealGyroIo;
import frc.robot.utils.logging.io.gyro.ThreadedGyro;
import frc.robot.utils.logging.io.gyro.ThreadedGyroSwerveIMU;
import frc.robot.utils.simulation.RobotVisualizer;
import swervelib.SwerveInputStream;
import swervelib.imu.SwerveIMU;

/**
 * This class is where the bulk of the robot should be declared. Since
 * Command-based is a
 * "declarative" paradigm, very little robot logic should actually be handled in
 * the {@link Robot}
 * periodic methods (other than the scheduler calls). Instead, the structure of
 * the robot (including
 * subsystems, commands, and trigger mappings) should be declared here.
 */
public class RobotContainer {
        private static final String DRIVER_CAM_PREFIX = CameraThread.LOGGING_PREFIX;
        private static final String DRIVER_CAM_SERVER_NAME = "serve_" + DRIVER_CAM_PREFIX;
        private static final int DRIVER_CAM_PORT = 1182;
        private static final int DRIVER_CAM_FPS = 30;

        // Instantiate the autochooser.
        private final AutoChooser autoChooser;
        // The robot's subsystems and commands are defined here...
        private final CommandXboxController controller =
            new CommandXboxController(Constants.XBOX_CONTROLLER_PORT);
        private final ClimberSubsystem climberSubsystem;
        private final AnglerSubsystem anglerSubsystem;
        private final IntakeSubsystem intakeSubsystem;
        private final FeederSubsystem feederSubsystem;
        private final ApriltagSubsystem apriltagSubsystem;
        private final ShooterSubsystem shooterSubsystem;
        private final ControllerSubsystem controllerSubsystem;
        private RobotVisualizer robotVisualizer = null;
        private final HopperSubsystem hopperSubsystem;
        private final LightStripSubsystem lightStripSubsystem;

        private final TurretSubsystem turretSubsystem;
        private final IntakeDeployerSubsystem intakeDeployer;
        private SwerveSubsystem drivebase = null;
        private GyroSubsystem gyroSubsystem = null;
        private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT);
        private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT);
        private ShootingState shootState = new ShootingState(ShootState.STOPPED);
        private Drive drive;
        private AutoFactory autoFactory;
        private static AutoRoutine straightRoutine;
        private static AutoTrajectory straightTrajectory;
        private final VisionTruster truster = new ConstantVisionTruster(visionMeasurementStdDevs2);

    // Replace with CommandPS4Controller or CommandJoystick if needed
        // new CommandXboxController(OperatorConstants.kDriverControllerPort);private
        // final CommandXboxController controller = new
        // CommandXboxController(Constants.XBOX_CONTROLLER_PORT);

        /**
         * The container for the robot. Contains subsystems, OI devices, and commands.
         */
        public RobotContainer() {
                // Configure the trigger bindings
                switch (Constants.currentMode) {
                        case REAL -> {
                                anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createRealIo());
                                intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createRealIo());
                                hopperSubsystem = new HopperSubsystem(HopperSubsystem.createRealIo());
                                intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createRealIo());
                                turretSubsystem = new TurretSubsystem(TurretSubsystem.createRealIo());
                                climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo());
                                feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo());
                                shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo());
                                RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo();
                                ThreadedGyro threadedGyro = gyroIo.getThreadedGyro();
                                gyroSubsystem = new GyroSubsystem(gyroIo);
                                SwerveIMU swerveIMU = new ThreadedGyroSwerveIMU(threadedGyro);
                                drivebase = !Constants.TESTBED ? new SwerveSubsystem(
                                                new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null;
                            apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null;
                            controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
                                 lightStripSubsystem = new LightStripSubsystem(drivebase, shootState, intakeSubsystem::getIntakeStalled);
            }
                        case REPLAY -> {
                                anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createMockIo());
                                intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo());
                                hopperSubsystem = new HopperSubsystem(HopperSubsystem.createMockIo());
                                climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createMockIo());
                                feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo());
                                turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo());
                                shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo());
                                intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo());

                                // No GyroSubsystem in REPLAY for now
                // create the drive subsystem with null gyro (use default json)
                drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null;
                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
                                lightStripSubsystem = new LightStripSubsystem(drivebase, shootState, intakeSubsystem::getIntakeStalled);

            }
            case SIM -> {
                robotVisualizer = new RobotVisualizer();
                                anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createSimIo(robotVisualizer));
                                intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer));
                                hopperSubsystem = new HopperSubsystem(HopperSubsystem.createSimIo(robotVisualizer));
                                climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createSimIo(robotVisualizer));
                                feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer));
                                turretSubsystem = new TurretSubsystem(TurretSubsystem.createSimIo(robotVisualizer));
                                shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer));
                                intakeDeployer = new IntakeDeployerSubsystem(
                                                IntakeDeployerSubsystem.createSimIo(robotVisualizer));// No GyroSubsystem in REPLAY for now

                                                // create the drive subsystem with null gyro (use default json)
                drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null;
                                lightStripSubsystem = new LightStripSubsystem(drivebase, shootState, intakeSubsystem::getIntakeStalled);

        }

                        default -> {
                                throw new RuntimeException("Did not specify Robot Mode");
                        }
                }

                setUpAutoFactory();
                autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem, feederSubsystem, hopperSubsystem, turretSubsystem, anglerSubsystem, controllerSubsystem);
                configureBindings();
                putShuffleboardCommands();
        }


        /**
         * Use this method to define your trigger->command mappings. Triggers can be
         * created via the
         * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
         * an arbitrary
         * predicate, or via the named factories in {@link
         * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for
         * {@link
         * CommandXboxController
         * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
         * PS4} controllers or
         * {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
         * joysticks}.
         */
        private void setUpAutoFactory() {
                if(!Constants.TESTBED){
                drive = new Drive(drivebase);

                // Sets up Choreo with pose, odometry, drivebase, and a follow trajectory
                // command
                autoFactory = new AutoFactory(drivebase::getPose,
                                drivebase::resetOdometry,
                                drive::followTrajectory,
                                true,
                                drivebase);

                // example implementation of autoRoutine
                if (false) {
                        // Uses autofactory to create a new routine
                        straightRoutine = autoFactory.newRoutine("StraightRoutine");

                        /*
                         * Loads a trajectory created in Choreo given the name
                         * Can load multiple trajectories from the same routine
                         *
                         * i.e.
                         * AutoRoutine routine = autoFactory.newRoutine("grabAndScore");
                         * AutoTrajectory grabTraj = routine.trajectory("grabPiece");
                         * AutoTrajectory scoreTraj = routine.trajectory("scorePiece");
                         */
                        straightTrajectory = straightRoutine.trajectory("StraightPath");

                        /*
                         * .active() is a trigger that becomes true when the routine is running
                         * .onTrue() starts a command when the trigger becomes true (i.e. when the
                         * routine starts)
                         *
                         * Use commands.sequence() to sequence multiple commands (i.e. reset odometry,
                         * then follow trajectory)
                         */
                        straightRoutine.active().onTrue(
                                        straightTrajectory.resetOdometry()
                                                        .andThen(straightTrajectory.cmd()));

                        /*
                         * -----------------------------------------------------------------------------
                         * -------------------
                         * Trajectory Triggers (read more on docs page
                         * https://choreo.autos/choreolib/auto-factory/):
                         * -----------------------------------------------------------------------------
                         * -------------------
                         *
                         * trajectory.atTime(String)
                         * trajectory.atTime(double time)
                         * trajectory.done()
                         * trajectory.active()
                         * trajectory.inactive()
                         * trajectory.atPose(String, double, double)
                         * trajectory.atPose(Pose2d, double, double)
                         * trajectory.doneDelayed(int)
                         * trajectory.doneFor(int)
                         * trajectory.recentlyDone()
                         */
                }}
        }

        private void configureBindings() {
                controller.a().onTrue(new ToggleDeployment(intakeDeployer));
                controller.b().onTrue(new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED));
                controller.y().onTrue(new ClimberUp(climberSubsystem));
                controller.x().onTrue(new ClimberDown(climberSubsystem));
                controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED));
                controller.povRight().onTrue(new SetShootingState(shootState, ShootState.STOPPED));
                controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB));
                controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING));
                controller.leftTrigger().onTrue((new ResetAll(anglerSubsystem, climberSubsystem, 
                        intakeDeployer, intakeSubsystem, shooterSubsystem, turretSubsystem, shootState)));
                driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED)));
                if (controllerSubsystem != null) {
                        steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem));
                }

                // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
                // new Trigger(m_exampleSubsystem::exampleCondition)
                // .onTrue(new ExampleCommand(m_exampleSubsystem));

                // Schedule `exampleMethodCommand` when the Xbox controller's B button is
                // pressed,
                // cancelling on release.
                // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());

                intakeSubsystem.setDefaultCommand(new SpinIntake(intakeSubsystem, intakeDeployer));
            if (controllerSubsystem != null) {
                anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem));
                shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem));
                turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem));
                hopperSubsystem.setDefaultCommand(new DefaultSpinHopper(hopperSubsystem, controllerSubsystem));
                feederSubsystem.setDefaultCommand(new DefaultSpinFeeder(feederSubsystem, controllerSubsystem));
            }

            if (!Constants.TESTBED) {
                        SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
                                        () -> driveJoystick.getY() * -1,
                                        () -> driveJoystick.getX() * -1)
                                        .withControllerRotationAxis(() -> {return steerJoystick.getX() * -1;})
                                        .deadband(Constants.DEADBAND)
                                        .scaleTranslation(0.8)
                                        .allianceRelativeControl(true);
                        Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
                        drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
                }
        }

        public void putShuffleboardCommands() {

                if (Constants.DEBUG) {

                        /*
                         * SmartDashboard.putData(
                         * "Spin Roller",
                         * new SpinRoller(rollerSubsystem));
                         *
                         * SmartDashboard.putData(
                         * "Tilt Up",
                         * new TiltUp(tiltSubsystem));
                         *
                         * SmartDashboard.putData(
                         * "Tilt Down",
                         * new TiltDown(tiltSubsystem));
                         */



            // TODO: These commands do not REQUIRE the subsystem therefore cannot be used in// production
                                SmartDashboard.putData(
                                        "Drive/Forward",
                                        new DriveSwerve(drivebase, DriveDirection.FORWARD, 0.5, 0.2));
                                SmartDashboard.putData(
                                        "Drive/Backward",
                                        new DriveSwerve(drivebase, DriveDirection.BACKWARD, 0.5, 0.2));
                                SmartDashboard.putData(
                                        "Drive/Left",
                                        new DriveSwerve(drivebase, DriveDirection.LEFT, 0.5, 0.2));
                                SmartDashboard.putData(
                                        "Drive/Right",
                                        new DriveSwerve(drivebase, DriveDirection.RIGHT, 0.5, 0.2));
                        SmartDashboard.putData(
                                        "Intake/Spin Forward",
                                        new InstantCommand(() -> intakeSubsystem.setSpeed(1.0)));

                        SmartDashboard.putData(
                                        "Intake/Spin Backward",
                                        new InstantCommand(() -> intakeSubsystem.setSpeed(-1.0)));

                        SmartDashboard.putData(
                                        "Intake/Stop",
                                        new InstantCommand(intakeSubsystem::stopMotors));

                        SmartDashboard.putNumber(RunDashboardShotTest.ANGLER_TARGET_POSITION_KEY, 0.0);
                        SmartDashboard.putNumber(RunDashboardShotTest.TURRET_TARGET_POSITION_KEY, Constants.TURRET_HOME_ANGLE);
                        SmartDashboard.putNumber(RunDashboardShotTest.SHOOTER_TARGET_RPM_KEY, Constants.SHOOTER_SPEED);
                        SmartDashboard.putNumber("Target Angle",0);
                        SmartDashboard.putData("RunHoppperAndFeeder",
                                        new RunHopperAndFeeder(hopperSubsystem, feederSubsystem));
                        SmartDashboard.putData("RunTurret",
                                        new TestSetTurretAngle(turretSubsystem));

                        SmartDashboard.putData(
                                        "test/Run Dashboard Shot Test (30s)",
                                        new RunDashboardShotTest(anglerSubsystem, turretSubsystem, shooterSubsystem));

                        SmartDashboard.putData(
                                        "angler/Go Home",
                                        new InstantCommand(() -> anglerSubsystem
                                                        .setPosition(Constants.ANGLER_HOME_ROTATIONS)));

                        SmartDashboard.putData(
                                        "angler/Go Low",
                                        new InstantCommand(() -> anglerSubsystem
                                                        .setPosition(Constants.ANGLER_ENCODER_LOW)));

                        SmartDashboard.putData(
                                        "angler/Go High",
                                        new InstantCommand(() -> anglerSubsystem
                                                        .setPosition(Constants.ANGLER_ENCODER_HIGH)));

                        SmartDashboard.putData(
                                        "angler/Run To Fwd Limit",
                                        new RunCommand(anglerSubsystem::runForward, anglerSubsystem)
                                                        .until(anglerSubsystem::isAtForwardLimit));

                        SmartDashboard.putData(
                                        "angler/Run To Rev Limit",
                                        new RunCommand(anglerSubsystem::runReverse, anglerSubsystem)
                                                        .until(anglerSubsystem::isAtReverseLimit));

                        SmartDashboard.putData(
                                        "angler/Reset Encoder",
                                        new InstantCommand(anglerSubsystem::resetEncoderToZero));

                        SmartDashboard.putData(
                                        "angler/Home Rev (Reset)",
                                        new RunAnglerToReverseLimit(anglerSubsystem));
                        SmartDashboard.putData(
                                        "reset All",
                                        new ResetAll(anglerSubsystem, climberSubsystem, intakeDeployer, intakeSubsystem, 
                                                shooterSubsystem, turretSubsystem, shootState));

            SmartDashboard.putData(
                    "turret/TurretTest/Turret Go 45",
                    new SetTurretAngle(turretSubsystem, 45));

            SmartDashboard.putData(
                    "turret/TurretTest/Turret Go 0",
                    new SetTurretAngle(turretSubsystem, 0));

            SmartDashboard.putData(
                    "turret/TurretTest/Turret Go -45",
                    new SetTurretAngle(turretSubsystem, -45));
                    SmartDashboard.putData(
                    "turret/TurretTest/Turret Go 40",
                    new SetTurretAngle(turretSubsystem, 40));
                SmartDashboard.putData(
                    "turret/TurretTest/Turret Go -40",
                    new SetTurretAngle(turretSubsystem, -40));
                SmartDashboard.putData(
                    "turret/TurretTest/Turret Go 35",
                    new SetTurretAngle(turretSubsystem, 35));
                SmartDashboard.putData(
                    "turret/TurretTest/Turret Go -35",
                    new SetTurretAngle(turretSubsystem, -35));

            SmartDashboard.putData(
                    "turret/Run Turret to Rev Limit",
                    new RunTurretToRevLimit(turretSubsystem));

            SmartDashboard.putData(
                    "turret/Run Turret to Fwd Limit",
                    new RunTurretToFwdLimit(turretSubsystem));

                        SmartDashboard.putData(
                                        "intakedeployer/InitlizeDeployer",
                                        new ToggleDeployment(intakeDeployer));
                        SmartDashboard.putData(
                                        "Spin Intake",
                                        new SpinIntake(intakeSubsystem, intakeDeployer));
                        SmartDashboard.putData(
                                        "Stop Intake",
                                        new StopIntake(intakeSubsystem));

                        SmartDashboard.putData(
                                        "Start Hopper",
                                        new SpinHopper(hopperSubsystem));

                        SmartDashboard.putData(
                                        "Climber Up",
                                        new ClimberUp(climberSubsystem));

                        SmartDashboard.putData(
                                        "Climber Down",
                                        new ClimberDown(climberSubsystem));

                        SmartDashboard.putData(
                                        "Spin Feeder",
                                        new SpinFeeder(feederSubsystem));

                        SmartDashboard.putData(
                                        "Spins Shooter",
                                        new SpinShooter(shooterSubsystem, Constants.SHOOTER_SPEED));

                        SmartDashboard.putData(
                                        "Shooting State: Stopped",
                                        new SetShootingState(shootState, ShootState.STOPPED));

                        SmartDashboard.putData(
                                        "Shooting State: Fixed",
                                        new SetShootingState(shootState, ShootState.FIXED));

                        SmartDashboard.putData(
                                        "Shooting State: Fixed 2",
                                        new SetShootingState(shootState, ShootState.FIXED_2));

                        SmartDashboard.putData(
                                        "Shooting State: Into Hub",
                                        new SetShootingState(shootState, ShootState.SHOOTING_HUB));

                        SmartDashboard.putData(
                                        "Shooting State: Shuttling",
                                        new SetShootingState(shootState, ShootState.SHUTTLING));
                        SmartDashboard.putData(
                                        "intakedeployer/Deployment State: UP",
                                        new SetDeploymentState(intakeDeployer, DeploymentState.UP));
                        SmartDashboard.putData(
                                        "intakedeployer/Deployment State: DOWN",
                                        new SetDeploymentState(intakeDeployer, DeploymentState.DOWN));
                        SmartDashboard.putData(
                                        "intakedeployer/Deployment State: STOPPED",
                                        new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED));

                }

        //basic drive command
        if (!Constants.TESTBED) {
            Command driveDirectionTime = new DriveDirectionTime(drivebase, 0.1, 0.1, true, 1);
            SmartDashboard.putData("Drive Command", driveDirectionTime);
            SmartDashboard.putData("Fake vision near trench", new FakeVision(drivebase, 4, 1));
            SmartDashboard.putData("Fake vision away from trench", new FakeVision(drivebase, 1, 1));
            SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem, new ApriltagReading(0, 0, 0, 0, 0, 0, 0, 0, 0)));
            SmartDashboard.putData("AddGarbageReading", new AddGarbageReading(apriltagSubsystem));
            SmartDashboard.putData("AddTunedApriltagReading", new AddTunableApriltagReading(apriltagSubsystem));

        }

        }

        /**
         * Use this to pass the autonomous command to the main {@link Robot} class.
         *
         * @return the command to run in autonomous
         */
        public Command getAutonomousCommand() {
                return autoChooser.getSelectedCommand();
    // return straightRoutine.cmd(straightTrajectory.done());

            //    return new ExampleAuto(drivebase, autoFactory);
        }

        public ClimberSubsystem getClimberSubsystem() {
                return climberSubsystem;
        }

    public RobotVisualizer getRobotVisualizer() {
        return robotVisualizer;
    }

    public AutoChooser getAutoChooser() {
        return autoChooser;
    }

    public IntakeSubsystem getIntakeSubsystem() {
        return intakeSubsystem;
    }
    
    public SwerveSubsystem getDriveBase() {
        return drivebase;
    }

    public ShootingState getShootingState() {
                return shootState;
        }
        public IntakeDeployerSubsystem getDeployer(){
                return intakeDeployer;
        }
}