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 edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.robot.subsystems.*;
import frc.robot.commands.*;
import frc.robot.commands.auto.*;
import frc.robot.commands.instant.*;

public class RobotContainer {

  // Constants
  private static final double BEVEL_IN_CORRECTION = 0.25;

    /* init subsystems */

    public SwerveModule leftFrontSwerve = new SwerveModule(RobotMap.leftFrontDrive, RobotMap.leftFrontRotate, RobotMap.leftFrontEncoder, 0.4675 + BEVEL_IN_CORRECTION, 0.0, 0.0, false); 
    public SwerveModule rightFrontSwerve = new SwerveModule(RobotMap.rightFrontDrive, RobotMap.rightFrontRotate, RobotMap.rightFrontEncoder, 0.7705 - BEVEL_IN_CORRECTION, 0.0, 0.0, true); 
    public SwerveModule leftBackSwerve = new SwerveModule(RobotMap.leftBackDrive, RobotMap.leftBackRotate, RobotMap.leftBackEncoder, 0.1280 - BEVEL_IN_CORRECTION, 0.0, 0.0, false); 
    public SwerveModule rightBackSwerve = new SwerveModule(RobotMap.rightBackDrive, RobotMap.rightBackRotate, RobotMap.rightBackEncoder, 0.4992 + BEVEL_IN_CORRECTION, 0.0, 0.0, true); 

    public Gyro gyro = new Gyro(RobotMap.gyro);
    public Elevator elevator = new Elevator();
    public Wrist wrist = new Wrist(RobotMap.wristSolenoid);
    public CoralClaw coralClaw = new CoralClaw(RobotMap.coralClawPrimaryMotor, RobotMap.coralClawSecondaryMotor, RobotMap.coralLimitSwitch);

    public Camera camera = new Camera();
    public Drivetrain drivetrain = new Drivetrain(leftBackSwerve, rightBackSwerve, leftFrontSwerve, rightFrontSwerve, gyro);
    
  /**
   * The RobotContainer 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 Robot periodic methods (other than the 
   * scheduler calls). Instead, the structure of the robot (including subsystems, 
   * commands, and button mappings) should be declared here.
   *
   * The constructor initializes the RobotContainer, sets up the default command 
   * for the drivetrain subsystem, and configures the button bindings.
   */
  public RobotContainer() {
    // Initialize OI with a reference to this RobotContainer instance
    configureBindings();
    drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier));
    RobotMap.compressor.enableAnalog(70, 120);
    // set the default drive command to use the left and right stick values from the Xbox controller
    // The left stick controls the forward and backward movement of the robot, while the right stick
    // controls the rotation of the robot.
  }

  /**
   * Configures the bindings for the robot controls.
   * This method sets up the action to manually reset the robot's position
   * when the drive controller's button A is pressed, along with operator controls.
   */
  private void configureBindings() {
      // Drive controller bindings
      OI.driveControllerA.onTrue(Commands.runOnce(() -> {
		  drivetrain.resetPosition();
	  }));

      OI.driveRightTrigger.onTrue(new InstantCommand(() -> drivetrain.setSlow(true)));
      OI.driveRightTrigger.onFalse(new InstantCommand(() -> drivetrain.setSlow(false)));

      // Operator controller bindings
      // Elevator level controls
      // OperatorBindings.operatorControllerA.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.LOW)));
      // OperatorBindings.operatorControllerB.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.HIGH)));
      // OperatorBindings.operatorControllerX.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.MED)));
      // OperatorBindings.operatorControllerY.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.EXTRA_HIGH)));

      // OPERATOR B : INTAKE
      OI.operatorControllerB.onTrue(new IntakeCommandGroup(elevator, wrist, coralClaw));
      // OPERATOR RB : TROUGH(L1)
      OI.operatorControllerRightBumper.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 1)); 
      // OPERATOR A : L2 (lowest pole)
      OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 2));
      // OPERATOR X : L3 (2nd pole)
      OI.operatorControllerX.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 3));
      // OPERATOR Y : L4 (last pole)
      OI.operatorControllerY.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 4));

      // OPERATOR LB: WRIST TOGGLE 
      OI.operatorControllerLeftBumper.onTrue(new InstantCommand(() -> wrist.toggle()));

      OI.operatorControllerStart.onTrue(new InstantCommand(() -> elevator.trimTarget(0.0175)));
      OI.operatorControllerBack.onTrue(new InstantCommand(() -> elevator.trimTarget(-0.0175)));
      
      
      
      // Coral Claw controls with triggers
      // OI.operatorRightTrigger.onTrue(new InstantCommand(() -> coralClaw.intake()));
      // OI.operatorRightTrigger.onFalse(new InstantCommand(() -> coralClaw.stop()));
      OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, 1));
      OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, 0));
      // OI.operatorLeftTrigger.onTrue(new InstantCommand(() -> coralClaw.setOutput(coralClaw.OUTTAKE_SPEED)));
      // OI.operatorLeftTrigger.onFalse(new InstantCommand(() -> coralClaw.stop()));
      
      // Reset encoder with Start button
      // OI.operatorControllerStart.onTrue(new InstantCommand(() -> elevator.resetEncoder()));
      
      // Manual control with right stick for testing in simulation
      OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> 
          elevator.setSpeed(OI.processElevatorInput(OI.operatorController.getRightY())), elevator));
  }

  public Command getAutonomousCommand() {
      return new ScoreTest(drivetrain, gyro, camera);
      // return new DriveDistance(drivetrain, gyro, 7.0, 0.0, 0.0).andThen(new DriveDistance(drivetrain, gyro, 0.0, 3.0, 0.0));
    // return new DriveWithHeadingCommand(drivetrain, gyro, OI.xboxLeftStickXSupplier, OI.xboxRightStickYSupplier, new Rotation2d(0.0));
    // return new DriveMeters(drivetrain, 0.0, 0.0, 0.0);
  }
  
}