Merge branch 'main' of github.com:StuyPulse/BigWang into se/climber

src/main/java/com/stuypulse/robot/Robot.java

@@ -5,6 +5,7 @@
 
 package com.stuypulse.robot;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -36,7 +37,19 @@ public void robotPeriodic() {
     public void disabledInit() {}
 
     @Override
-    public void disabledPeriodic() {}
+    public void disabledPeriodic() {
+        if (robot.intake.hasNote()) {
+            DriverStation.reportWarning("Intake IR sensor reporting note while disabled!", false);
+        }
+
+        if (robot.amper.hasNote()) {
+            DriverStation.reportWarning("Amper IR sensor reporting note while disabled!", false);
+        }
+
+        if (robot.conveyor.isNoteAtShooter()) {
+            DriverStation.reportWarning("Shooter IR sensor reporting note while disabled!", false);
+        }
+    }
 
     /***********************/
     /*** AUTONOMOUS MODE ***/

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -9,8 +9,17 @@
 import com.stuypulse.robot.commands.ClimberToBottom;
 import com.stuypulse.robot.commands.ClimberToTop;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
+import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.subsystems.amper.Amper;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.subsystems.vision.AprilTagVision;
+import com.stuypulse.robot.subsystems.vision.NoteVision;
+import com.stuypulse.robot.subsystems.intake.Intake;
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+import com.stuypulse.robot.subsystems.conveyor.Conveyor;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
@@ -29,7 +38,15 @@ public class RobotContainer {
 
     // Subsystem
     public final Climber climber = Climber.getInstance();
-
+    public final Amper amper = Amper.getInstance();
+    public final SwerveDrive swerve = SwerveDrive.getInstance();
+    public final Odometry odometry = Odometry.getInstance();
+    public final AprilTagVision vision = AprilTagVision.getInstance();
+    public final NoteVision noteVision = NoteVision.getInstance();
+    public final Intake intake = Intake.getInstance();
+    public final Shooter shooter = Shooter.getInstance();
+    public final Conveyor conveyor = Conveyor.getInstance();
+
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 
@@ -45,7 +62,9 @@ public RobotContainer() {
     /*** DEFAULTS ***/
     /****************/
 
-    private void configureDefaultCommands() {}
+    private void configureDefaultCommands() {
+        swerve.setDefaultCommand(new SwerveDriveDrive(driver));
+    }
 
     /***************/
     /*** BUTTONS ***/

src/main/java/com/stuypulse/robot/commands/amper/AmperIntake.java

@@ -0,0 +1,25 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AmperIntake extends Command {
+
+    private final Amper amper;
+
+    public AmperIntake() {
+        amper = Amper.getInstance();
+        addRequirements(amper);
+    } 
+
+    @Override
+    public void initialize() {
+        amper.intake();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        amper.stopRoller();
+    }
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperOuttake.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AmperOuttake extends Command {
+
+    private final Amper amper;
+
+    public AmperOuttake() {
+        amper = Amper.getInstance();
+        addRequirements(amper);
+    } 
+
+    @Override
+    public void initialize() {
+        amper.score();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return !amper.hasNote();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        amper.stopRoller();
+    }
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreAmp.java

@@ -0,0 +1,17 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.constants.Settings.Amper.Score;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AmperScoreAmp extends SequentialCommandGroup {
+
+    public AmperScoreAmp() {
+        addCommands(
+            new AmperToHeight(Score.AMP_SCORE_HEIGHT.get()),
+            new AmperWaitToHeight(Score.AMP_SCORE_HEIGHT.get()),
+            new AmperOuttake()
+        );
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreForever.java

@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class AmperScoreForever extends InstantCommand {
+
+    private final Amper amper;
+
+    public AmperScoreForever() {
+        amper = Amper.getInstance();
+        addRequirements(amper);
+    } 
+
+    @Override
+    public void initialize() {
+        amper.score();
+    }
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreTrap.java

@@ -0,0 +1,17 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.constants.Settings.Amper.Score;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AmperScoreTrap extends SequentialCommandGroup {
+
+    public AmperScoreTrap() {
+        addCommands(
+            new AmperToHeight(Score.TRAP_SCORE_HEIGHT.get()),
+            new AmperWaitToHeight(Score.TRAP_SCORE_HEIGHT.get()),
+            new AmperOuttake()
+        ); 
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java

@@ -0,0 +1,22 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class AmperToHeight extends InstantCommand {
+    private final Amper amper;
+    private final double height;
+
+    public AmperToHeight(double height) {
+        amper = Amper.getInstance();
+        this.height = height;
+
+        addRequirements(amper);
+    }
+
+    @Override
+    public void initialize() {
+        amper.setTargetHeight(height);
+    }
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperWaitToHeight.java

@@ -0,0 +1,24 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.constants.Settings.Amper.Lift;
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AmperWaitToHeight extends Command {
+
+    private final Amper amper;
+    private final double height;
+
+    public AmperWaitToHeight(double height) {
+        amper = Amper.getInstance();
+        this.height = height;
+
+        addRequirements(amper);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(amper.getLiftHeight() - height) < Lift.MAX_HEIGHT_ERROR;
+    }
+}

src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class IntakeAcquire extends Command{
+
+    private Intake intake;
+
+    public IntakeAcquire() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.acquire();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intake.stop();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return intake.hasNote();
+    }
+}

src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java

@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class IntakeAcquireForever extends InstantCommand {
+
+    private Intake intake;
+
+    public IntakeAcquireForever() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.acquire();
+    }
+}

src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java

@@ -0,0 +1,19 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class IntakeDeacquire extends InstantCommand {
+    private Intake intake;
+
+    public IntakeDeacquire() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.deacquire();
+    }
+}

src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java

@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+public class IntakeStop extends InstantCommand {
+
+    private Intake intake;
+
+    public IntakeStop() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.stop();
+    }
+}

src/main/java/com/stuypulse/robot/commands/shooter/ShooterPodiumShot.java

@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class ShooterPodiumShot extends ShooterSetRPM{
+
+    public ShooterPodiumShot() {
+        super(Settings.Shooter.PODIUM_SHOT_LEFT_RPM, Settings.Shooter.PODIUM_SHOT_RIGHT_RPM);
+    }
+}

src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java

@@ -0,0 +1,27 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ShooterSetRPM extends InstantCommand {
+    private final Shooter shooter;
+    private final Number leftTargetRPM;
+    private final Number rightTargetRPM;
+
+    public ShooterSetRPM(Number leftTargetRPM, Number rightTargetRPM) {
+        shooter = Shooter.getInstance();
+        this.leftTargetRPM = leftTargetRPM;
+        this.rightTargetRPM = rightTargetRPM;
+
+        addRequirements(shooter);
+    }
+
+    @Override
+    public void initialize() {
+        shooter.setLeftTargetRPM(leftTargetRPM);
+        shooter.setRightTargetRPM(rightTargetRPM);
+    }
+
+}
+

src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java

@@ -0,0 +1,8 @@
+package com.stuypulse.robot.commands.shooter;
+
+public class ShooterStop extends ShooterSetRPM {    
+
+    public ShooterStop(){
+        super(0, 0);
+    }
+}