Merge pull request #1 from qwertycloudhub/dev

Dev

src/main/java/frc/robot/Constants.java

@@ -0,0 +1,30 @@
+package frc.robot;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+
+import java.util.Map;
+
+public class Constants {
+
+    public static final double fieldLength = Units.inchesToMeters(651.223);
+    public static final class InboundingBox {
+        public static final Translation3d inboundingBoxPosition = new Translation3d(0, 0, 0);
+    }
+    public static final class CenterTank {
+        public static final Translation3d centerTankPosition = new Translation3d(0,0,0);
+    }
+    public static final class DunkTank {
+        public static final Translation3d dunkTankPosition = new Translation3d(0,0,0);
+    }
+
+
+    public static final Map<Integer, ControllerType> CONTROLLERS = Map.of(
+            0, ControllerType.CUSTOM, 1, ControllerType.XBOX
+    );
+    public static final int CONTROLLER_COUNT = CONTROLLERS.size();
+    public enum ControllerType {
+        CUSTOM,
+        XBOX
+    }
+}

src/main/java/frc/robot/Main.java

@@ -1,25 +1,13 @@
-// 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.RobotBase;
 
-/**
- * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
- * you are doing, do not modify this file except to change the parameter class to the startRobot call.
- */
+
 public final class Main {
     private Main() {
     }
 
-    /**
-     * Main initialization method. Do not perform any initialization here.
-     * <p>
-     * If you change your main Robot class (name), change the parameter type.
-     */
+
     public static void main(String... args) {
         RobotBase.startRobot(Robot::new);
     }

src/main/java/frc/robot/Robot.java

@@ -1,59 +1,26 @@
-// 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.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
-
-/**
- * The VM is configured to automatically run this class, and to call the methods corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
- * project.
- */
 public class Robot extends TimedRobot {
     private Command autonomousCommand;
 
     private RobotContainer robotContainer;
 
 
-    /**
-     * This method is run when the robot is first started up and should be used for any
-     * initialization code.
-     */
     @Override
     public void robotInit() {
-        // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
-        // autonomous chooser on the dashboard.
         robotContainer = new RobotContainer();
     }
 
-
-    /**
-     * This method is called every 20 ms, 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 methods, but before LiveWindow and
-     * SmartDashboard integrated updating.
-     */
     @Override
     public void robotPeriodic() {
-        // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-        // commands, running already-scheduled commands, removing finished or interrupted commands,
-        // and running subsystem periodic() methods.  This must be called from the robot's periodic
-        // block in order for anything in the Command-based framework to work.
         CommandScheduler.getInstance().run();
     }
 
 
-    /**
-     * This method is called once each time the robot enters Disabled mode.
-     */
     @Override
     public void disabledInit() {
     }
@@ -64,74 +31,49 @@ public void disabledPeriodic() {
     }
 
 
-    /**
-     * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-     */
     @Override
     public void autonomousInit() {
         autonomousCommand = robotContainer.getAutonomousCommand();
 
-        // schedule the autonomous command (example)
         if (autonomousCommand != null) {
             autonomousCommand.schedule();
         }
     }
 
 
-    /**
-     * This method is called periodically during autonomous.
-     */
     @Override
     public void autonomousPeriodic() {
     }
 
 
     @Override
     public void teleopInit() {
-        // This makes sure that the autonomous stops running when
-        // teleop starts running. If you want the autonomous to
-        // continue until interrupted by another command, remove
-        // this line or comment it out.
         if (autonomousCommand != null) {
             autonomousCommand.cancel();
         }
     }
 
 
-    /**
-     * This method is called periodically during operator control.
-     */
     @Override
     public void teleopPeriodic() {
     }
 
 
     @Override
     public void testInit() {
-        // Cancels all running commands at the start of test mode.
         CommandScheduler.getInstance().cancelAll();
     }
 
-
-    /**
-     * This method is called periodically during test mode.
-     */
     @Override
     public void testPeriodic() {
     }
 
 
-    /**
-     * This method is called once when the robot is first started up.
-     */
     @Override
     public void simulationInit() {
     }
 
 
-    /**
-     * This method is called periodically whilst in simulation.
-     */
     @Override
     public void simulationPeriodic() {
     }

src/main/java/frc/robot/RobotContainer.java

@@ -5,50 +5,60 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.XboxController;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.constants.Constants;
+import frc.robot.commands.CenterDunkTankAlignCommand;
+import frc.robot.commands.DunkTankAlignCommand;
+import frc.robot.commands.InboundingBoxAlignCommand;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain;
+import frc.robot.subsystems.drivetrain.generated.TunerConstants;
+import frc.robot.util.controllerUtils.ButtonHelper;
+import frc.robot.util.controllerUtils.ControllerContainer;
+import frc.robot.subsystems.IntakeSubsystem;
 
-
-/**
- * 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 {
+    public ControllerContainer controllerContainer = new ControllerContainer();
+    public final CommandXboxController joystick = new CommandXboxController(1);
+    ButtonHelper buttonHelper = new ButtonHelper(controllerContainer.getControllers());
+
+    final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain;
+    public final IntakeSubsystem intake = new IntakeSubsystem();
+    public final ShooterSubsystem shooter = new ShooterSubsystem();
 
-    XboxController xboxController;
+    private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(CommandSwerveDrivetrain.MAX_VELOCITY_METERS_PER_SECOND * 0.1)
+            .withRotationalDeadband(CommandSwerveDrivetrain.MaFxAngularRate * 0.1)
+            .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
-        xboxController = new XboxController(Constants.XBOX_CONTROLLER_PORT);
         configureBindings();
     }
 
 
-    /**
-     * 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 configureBindings() {
+        drivetrain.setDefaultCommand(
+                drivetrain.applyRequest(
+                        () ->
+                                drive
+                                        .withVelocityX(-joystick.getLeftY() * TunerConstants.kSpeedAt12VoltsMps)
+                                        .withVelocityY(-joystick.getLeftX() * TunerConstants.kSpeedAt12VoltsMps)
+                                        .withRotationalRate(-joystick.getRightX() * CommandSwerveDrivetrain.MaFxAngularRate)
+                ));
 
+        joystick.leftBumper().whileTrue(new InboundingBoxAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX()));
+        joystick.rightBumper().whileTrue(new DunkTankAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX()));
+        joystick.a().whileTrue(new CenterDunkTankAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX()));
+        joystick.leftTrigger().whileTrue(intake.rollIn(0));
+        joystick.rightTrigger().whileTrue(shooter.shootElectrolyte(0));
     }
 
-    /**
-     * Use this to pass the autonomous command to the main {@link Robot} class.
-     *
-     * @return the command to run in autonomous
-     */
     public Command getAutonomousCommand() {
         return null;
     }

src/main/java/frc/robot/TalonFXConstants.java

@@ -0,0 +1,7 @@
+package frc.robot;
+
+public class TalonFXConstants {
+    public static final boolean TALON_FUTURE_PROOF = true;
+    public static final String CANIVORE_NAME = "canivoreBus";
+
+}

src/main/java/frc/robot/commands/AutoAimCommand.java

@@ -0,0 +1,31 @@
+package frc.robot.commands;
+
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public class AutoAimCommand extends SwerveRequest.FieldCentricFacingAngle {
+
+    private Translation2d pointToFace;
+
+    @Override
+    public StatusCode apply(
+            SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
+        this.TargetDirection = pointToFace.minus(parameters.currentPose.getTranslation()).getAngle();
+        if (ForwardReference == SwerveRequest.ForwardReference.OperatorPerspective) {
+            this.TargetDirection = this.TargetDirection.minus(parameters.operatorForwardDirection);
+        }
+        return super.apply(parameters, modulesToApply);
+    }
+
+    @Override
+    public FieldCentricFacingAngle withTargetDirection(Rotation2d targetDirection) {
+        return this;
+    }
+
+    public void setPointToFace(Translation2d point) {
+        pointToFace = point;
+    }
+}

src/main/java/frc/robot/commands/CenterDunkTankAlignCommand.java

@@ -0,0 +1,62 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.VisionSubsystem;
+import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain;
+import frc.robot.util.AllianceFlipUtil;
+import frc.robot.util.LimelightHelpers;
+
+import java.util.function.DoubleSupplier;
+
+
+public class CenterDunkTankAlignCommand extends Command {
+    private final CommandSwerveDrivetrain drivetrain;
+    private final DoubleSupplier xVelSupplier;
+    private final AutoAimCommand poseAimRequest;
+    private final DoubleSupplier yVelSupplier;
+    private double currentTag;
+
+    public CenterDunkTankAlignCommand(
+            CommandSwerveDrivetrain drivetrain,
+            DoubleSupplier xVelSupplier,
+            DoubleSupplier yVelSupplier) {
+
+        this.drivetrain = drivetrain;
+        this.xVelSupplier = xVelSupplier;
+        this.yVelSupplier = yVelSupplier;
+
+        addRequirements(drivetrain);
+
+        poseAimRequest = new AutoAimCommand();
+        poseAimRequest.HeadingController.setPID(5,0,0);
+        poseAimRequest.HeadingController.enableContinuousInput(-Math.PI,Math.PI);
+    }
+
+    @Override
+    public void initialize() {
+        currentTag = LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME);
+
+        Translation2d centerTank = AllianceFlipUtil.apply(
+                Constants.CenterTank.centerTankPosition.toTranslation2d()
+        );
+
+        poseAimRequest.setPointToFace(centerTank);
+    }
+
+    @Override
+    public void execute() {
+        if(LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME) == currentTag) {
+            drivetrain.setControl(
+                    poseAimRequest.withVelocityX(xVelSupplier.getAsDouble() * 1.5).withVelocityY(yVelSupplier.getAsDouble() * 1.5)
+            );
+        } else {
+            end(true);
+        }
+    }
+
+    @Override
+    public boolean isFinished() {return false;}
+
+}