20211108~20211114 程式組

package frc.robot;

// Default Imports
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

// Phoenix Tuner Related Imports
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;


public class Robot extends TimedRobot {
  private static final String kDefaultAuto = "Default";
  private static final String kCustomAuto = "My Auto";
  private String m_autoSelected;
  private final SendableChooser<String> m_chooser = new SendableChooser<>();

  // Change Spark to be the name of the motor that is being used
  WPI_TalonFX m_frontRight = new WPI_TalonFX(3);
  WPI_TalonFX m_rearRight = new WPI_TalonFX(4);
  SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight);

  WPI_TalonFX m_frontLeft = new WPI_TalonFX(1);
  WPI_TalonFX m_rearLeft = new WPI_TalonFX(2);
  SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);

  MecanumDrive m_robotDrive;
  Joystick m_stick;
  Joystick m_zstick;
  Timer m_timer;

  // Following are declarations for the buttons on the controller
  public final int leftStick_X = 0;
  public final int leftStick_Y = 1;
  public final int rightStick_X = 4;
  public final int rightStick_Y = 5;
  public final int trigger_L = 2;
  public final int trigger_R = 3;
  public final int Btn_A = 1;
  public final int Btn_B = 2;
  public final int Btn_X = 3;
  public final int Btn_Y = 4;
  public final int Btn_LB = 5;
  public final int Btn_RB = 6;
  public final int Btn_LS = 9;  
  public final int Btn_RS = 10;
  
  @Override
  public void robotInit() {
    // If left is inverted, set the following setting
    // m_left.setInverted(true);
    m_robotDrive = new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearLeft);

    // Make sure that the channels are matched accordingly
    m_stick = new Joystick(0);
    m_zstick = new Joystick(1);
    m_timer = new Timer();

    m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
    m_chooser.addOption("My Auto", kCustomAuto);
    SmartDashboard.putData("Auto choices", m_chooser);
  }

  /**
   * This function is called every robot packet, no matter the mode. Use this for items like
   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
   *
   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
   * SmartDashboard integrated updating.
   */
  @Override
  public void robotPeriodic() {}

  /**
   * This autonomous (along with the chooser code above) shows how to select between different
   * autonomous modes using the dashboard. The sendable chooser code works with the Java
   * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
   * uncomment the getString line to get the auto name from the text box below the Gyro
   *
   * <p>You can add additional auto modes by adding additional comparisons to the switch structure
   * below with additional strings. If using the SendableChooser make sure to add them to the
   * chooser code above as well.
   */
  @Override
  public void autonomousInit() {
    m_autoSelected = m_chooser.getSelected();
    // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
    System.out.println("Auto selected: " + m_autoSelected);

    m_timer.reset();
    m_timer.start();
  }

  /** This function is called periodically during autonomous. */
  @Override
  public void autonomousPeriodic() {
    switch (m_autoSelected) {
      case kCustomAuto:
        // Put custom auto code here
        if (m_timer.get() < 2.0) {
          
        }
        break;
      case kDefaultAuto:
      default:
        // Put default auto code here
        break;
    }
  }

  /** This function is called once when teleop is enabled. */
  @Override
  public void teleopInit() {
    System.out.println("Teleop Mode Initiated, MecanumMode Working");
  }

  /** This function is called periodically during operator control. */
  @Override
  public void teleopPeriodic() {
    m_robotDrive.driveCartesian(m_stick.getX(), -m_stick.getY(), m_zstick.getX());
  }

  /** This function is called once when the robot is disabled. */
  @Override
  public void disabledInit() {}

  /** This function is called periodically when disabled. */
  @Override
  public void disabledPeriodic() {}

  /** This function is called once when test mode is enabled. */
  @Override
  public void testInit() {}

  /** This function is called periodically during test mode. */
  @Override
  public void testPeriodic() {}
}