Robot.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 edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;

import frc.robot.commands.test.MoveElevatorToLevelCommand;
import frc.robot.commands.FetchCoralFromStationCommand;

import frc.robot.ElevatorLevel;

public class Robot extends TimedRobot {
  private Command m_autonomousCommand;

  private final RobotContainer m_robotContainer;
  
  // Enum to represent different test options
  private enum TestOption {
    ELEVATOR_LOW("Elevator to Low Position"),
    ELEVATOR_MID("Elevator to Mid Position"),
    ELEVATOR_HIGH("Elevator to High Position"),
    ELEVATOR_STATION("Elevator to Station Position"),
    CORAL_CLAW_INTAKE("Run Coral Claw Intake"),
    FETCH_CORAL_STATION("Fetch Coral From Station"),
    // Add more test options as needed
    NONE("No Test");
    
    private final String displayName;
    
    TestOption(String displayName) {
      this.displayName = displayName;
    }
    
    @Override
    public String toString() {
      return displayName;
    }
  }
  
  // Create SendableChooser for test options
  private final SendableChooser<TestOption> testChooser = new SendableChooser<>();

  public Robot() {
    m_robotContainer = new RobotContainer();
    
    // Initialize test chooser
    testChooser.setDefaultOption("No Test", TestOption.NONE);
    testChooser.addOption("Elevator to Low", TestOption.ELEVATOR_LOW);
    testChooser.addOption("Elevator to Mid", TestOption.ELEVATOR_MID);
    testChooser.addOption("Elevator to High", TestOption.ELEVATOR_HIGH);
    testChooser.addOption("Elevator to Station", TestOption.ELEVATOR_STATION);
    testChooser.addOption("Run Coral Claw Intake", TestOption.CORAL_CLAW_INTAKE);
    testChooser.addOption("Fetch Coral From Station", TestOption.FETCH_CORAL_STATION);
    // Add more options as needed
    
    // Add the chooser to the dashboard
    SmartDashboard.putData("Test Selector", testChooser);
  }

  @Override
  public void robotPeriodic() {
    CommandScheduler.getInstance().run();
  }

  @Override
  public void disabledInit() {}

  @Override
  public void disabledPeriodic() {
    SmartDashboard.putNumber("xbox left x", OI.xboxLeftStickXSupplier.getAsDouble());
    SmartDashboard.putNumber("xbox left y", OI.xboxLeftStickYSupplier.getAsDouble());

  }

  @Override
  public void disabledExit() {}

  @Override
  public void autonomousInit() {
    m_autonomousCommand = m_robotContainer.getAutonomousCommand();

    if (m_autonomousCommand != null) {
      m_autonomousCommand.schedule();
    }
  }

  @Override
  public void autonomousPeriodic() {}

  @Override
  public void autonomousExit() {}

  @Override
  public void teleopInit() {
    if (m_autonomousCommand != null) {
      m_autonomousCommand.cancel();
    }
  }

  @Override
  public void teleopPeriodic() {}

  @Override
  public void teleopExit() {}

  @Override
  public void testInit() {
    CommandScheduler.getInstance().cancelAll();
      // Get the selected test option
      TestOption selectedTest = testChooser.getSelected();
    
      // Run the selected test command
      switch(selectedTest) {
        case ELEVATOR_LOW:
          new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.LOW).schedule();
          break;
        case ELEVATOR_MID:
          new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.MED).schedule();
          break;
        case ELEVATOR_HIGH:
          new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.HIGH).schedule();
          break;
        case ELEVATOR_STATION:
          new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.STATION).schedule();
          break;
        case CORAL_CLAW_INTAKE:
          new InstantCommand(() -> m_robotContainer.coralClaw.intake()).schedule();
          break;
        case FETCH_CORAL_STATION:
          new FetchCoralFromStationCommand(m_robotContainer.elevator, m_robotContainer.coralClaw).schedule();
          break;
        case NONE:
        default:
          // Do nothing
          break;
      }
  }

  @Override
  public void testPeriodic() {
  
  }

  @Override
  public void testExit() {}

  @Override
  public void simulationInit() {
    /*
     *    FRONT
     * +x,+y | +x,-y
     * -x,+y | -x,-y
     *     BACK
     * 
     */
    
  }
}