2022NoWayHome/src/main/java/frc4388/robot/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 frc4388.robot;

import com.revrobotics.CANSparkMax;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFX;

import edu.wpi.first.wpilibj.Joystick;
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.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.AimToCenter;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
//import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.LEDPatterns;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;

/**
 * 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 button mappings) should be declared here.
 */
public class RobotContainer {
  /* RobotMap */
  private final RobotMap m_robotMap = new RobotMap();
  private final TalonFX m_testMotor = new TalonFX(2);

  /*
   * Subsystems
   * private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
   * m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
   * m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
   * m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor,
   * m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor,
   * m_robotMap.leftFrontEncoder,
   * m_robotMap.rightFrontEncoder,
   * m_robotMap.leftBackEncoder,
   * m_robotMap.rightBackEncoder
   * );
   */
  private final LED m_robotLED = new LED(m_robotMap.LEDController);
  private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
  private final Hood m_robotHood = new Hood();
  private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
  private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
  private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFrontSteerMotor, 
                                                                  m_robotMap.leftFrontWheelMotor, 
                                                                  m_robotMap.rightFrontSteerMotor, 
                                                                  m_robotMap.rightFrontWheelMotor, 
                                                                  m_robotMap.leftBackSteerMotor, 
                                                                  m_robotMap.leftBackWheelMotor, 
                                                                  m_robotMap.rightBackSteerMotor, 
                                                                  m_robotMap.rightBackWheelMotor,
                                                                  m_robotMap.leftFrontEncoder,
                                                                  m_robotMap.rightFrontEncoder,
                                                                  m_robotMap.leftBackEncoder,
                                                                  m_robotMap.rightBackEncoder);
  /* Controllers */
  private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
  private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);

  /**
   * The container for the robot. Contains subsystems, OI devices, and commands.
   */
  public RobotContainer() {
    configureButtonBindings();

    /* Default Commands */
    // drives the swerve drive with a two-axis input from the driver controller
    /*m_robotSwerveDrive.setDefaultCommand(
        new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
            getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));

    // continually sends updates to the Blinkin LED controller to keep the lights on
    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
    

    */
    //Turret default command

    m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive));
      // m_robotTurret.setDefaultCommand(
      //     new RunCommand(() -> m_robotTurret.aimToCenter()));
  }

  /**
   * Use this method to define your button->command mappings. Buttons can be
   * created by instantiating a {@link GenericHID} or one of its subclasses
   * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
   * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
   */
  private void configureButtonBindings() {
    /* Driver Buttons */

    /* Operator Buttons */
    // activates "Lit Mode"

    new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
        .whileHeld(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.5));

    new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
        .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
        .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
    // activates "BoomBoom"
    new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
        .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1))
        .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0));
    /* Driver Buttons */
    // activates intake
    new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON);
    // .whenPressed() -> m_robot
    /* operator button */
    // activates hood
    new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
        .whenPressed(() -> m_robotHood.runHood(0.5d))
        .whenReleased(() -> m_robotHood.runHood(0.d));
    // new JoystickButton(getOperatorJoystick());
  }

  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {
    // no auto
    return new InstantCommand();
  }

  /**
   * Add your docs here.
   */
  public IHandController getDriverController() {
    return m_driverXbox;
  }

  /**
   * Add your docs here.
   */
  public IHandController getOperatorController() {
    return m_operatorXbox;
  }

  /**
   * Add your docs here.
   */
  public Joystick getOperatorJoystick() {
    return m_operatorXbox.getJoyStick();
  }

  /**
   * Add your docs here.
   */
  public Joystick getDriverJoystick() {
    return m_driverXbox.getJoyStick();
  }
}