Amp Subsystem

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

@@ -46,6 +46,10 @@ public static class CAN {
     public final static int CAN_hopperright = 16;
     public final static int CAN_hopperleft = 17;
 
+    public final static int ampPCM = 18;
+    public final static int ampChannel = 19;
+    public final static int ampLimitSwitch = 20; //TODO
+    public final static int ampMotor = 21; //TODO
   }
 
   public static class AprilTags {

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

@@ -5,29 +5,24 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import edu.wpi.first.wpilibj2.command.button.POVButton;
 import frc.robot.commands.*;
-import frc.robot.commands.Chassis.FlipDriveOrientation;
-import frc.robot.commands.Chassis.TeleopDrive;
-import frc.robot.commands.Chassis.ZeroEverything;
-import frc.robot.commands.Chassis.ZeroWheels;
-import frc.robot.sensors.Camera;
-import frc.robot.subsystems.Chassis;
+import frc.robot.commands.Amp.AmpIntake;
+import frc.robot.commands.Amp.AmpOuttake;
+import frc.robot.commands.Amp.AmpPrime;
+import frc.robot.commands.Amp.AmpUnPrime;
+import frc.robot.subsystems.Amp;
 import frc.robot.subsystems.ExampleSubsystem;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.subsystems.Hopper;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -39,28 +34,22 @@
 // The robot's subsystems and commands are defined here...
 public class RobotContainer {
   private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
-  private final Camera limelight;
-  private final Chassis chassis;
-  private final Hopper hopper;
+  private final Amp amp;
   private final XboxController driverController = new XboxController(0);
   private final XboxController operatorController = new XboxController(1);
   private final SendableChooser<Command> autoChooser;
 
   // container for the robot containing subsystems, OI devices, and commands
   public RobotContainer() {
-    limelight = new Camera();
-    chassis = new Chassis(limelight);
-    hopper = new Hopper();
+    amp = new Amp();
 
     // Named commands must be registered before the creation of any PathPlanner Autos or Paths
     // Do this in RobotContainer, after subsystem initialization, but before the creation of any other commands.
-    NamedCommands.registerCommand("spinHopper", hopper.spinHopperAuto());
 
     configureBindings(); // configure button bindings
     exportShuffleBoardData(); // export ShuffleBoardData
 
     // Default commands running in the background when other commands not scheduled
-    chassis.setDefaultCommand(new TeleopDrive(chassis, driverController));
 
     // Build an auto chooser. This will use Commands.none() as the default option.
     // autoChooser = AutoBuilder.buildAutoChooser();
@@ -96,8 +85,7 @@ public void periodic() {
   public void exportShuffleBoardData() {
     if (Constants.debugMode) {
       ShuffleboardTab tab = Shuffleboard.getTab("Subsystems");
-      tab.add(chassis);
-      chassis.exportSwerveModData(Shuffleboard.getTab("Swerve Modules"));
+      tab.add(amp);
     }
   }
 
@@ -113,30 +101,9 @@ private void configureBindings() {
 
     // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, cancelling on release.
     // driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
-    new POVButton(driverController, Constants.Buttons.LST_POV_N).whileTrue(new ZeroEverything(chassis));
-    new POVButton(driverController, Constants.Buttons.LST_POV_W).whileTrue(new ZeroWheels(chassis));
-    new JoystickButton(driverController, Constants.Buttons.LST_BTN_B).whileTrue(new FlipDriveOrientation(chassis));
-
-    new JoystickButton(driverController, Constants.Buttons.LST_BTN_Y).whileTrue(new SpinHopper(hopper));
-
-    SmartDashboard.putData(new FlipDriveOrientation(chassis));
-  }
-
-  /*
-  Sendable Commands
-   */
-  public Command resetEverything() {
-    return new ZeroEverything(chassis);
-  }
-
-  /*
-  Odometry and Chassis methods
-   */
-  public void resetOdometryWithoutApril() {
-    chassis.resetOdometry(new Pose2d(0, 0, new Rotation2d()));
-  }
-
-  public void updateChassisPose() {
-    chassis.updateOdometery();
+    new JoystickButton(driverController, Constants.Buttons.LST_BTN_A).whileTrue(new AmpIntake(amp));
+    new JoystickButton(driverController, Constants.Buttons.LST_BTN_X).whileTrue(new AmpOuttake(amp));
+    new JoystickButton(driverController, Constants.Buttons.LST_BTN_X).whileTrue(new AmpPrime(amp));
+    new JoystickButton(driverController, Constants.Buttons.LST_BTN_X).whileTrue(new AmpUnPrime(amp));
   }
 }

src/main/java/frc/robot/commands/Amp/AmpIntake.java

@@ -0,0 +1,47 @@
+// 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.commands.Amp;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Amp;
+
+public class AmpIntake extends Command {
+  private final Amp amp;
+
+  /**
+   * @param amp The subsystem used by this command.
+   */
+  public AmpIntake(Amp amp) {
+    this.amp = amp;
+    addRequirements(amp);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    amp.intakeAmp();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    amp.ampMotorStop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (amp.getLimitSwitch()) {
+      return true;
+    }
+    return false;
+  }
+}

src/main/java/frc/robot/commands/SpinHopper.java → src/main/java/frc/robot/commands/Amp/AmpOuttake.java

@@ -2,56 +2,42 @@
 // 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.commands;
+package frc.robot.commands.Amp;
 
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Hopper;
+import frc.robot.subsystems.Amp;
 
 /** An example command that uses an example subsystem. */
-public class SpinHopper extends Command {
-  private final Hopper hopper;
-  private final Timer timer;
+public class AmpOuttake extends Command {
+  private final Amp amp;
 
   /**
-   * Creates a new ExampleCommand.
-   *
-   * @param hopper The subsystem used by this command.
+   * @param amp The subsystem used by this command.
    */
-  public SpinHopper(Hopper hopper) {
-    timer = new Timer();
-    this.hopper = hopper;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(hopper);
+  public AmpOuttake(Amp amp) {
+    this.amp = amp;
+    addRequirements(amp);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    hopper.spinHopper();
-    timer.reset();
-    timer.start();
+    amp.outtakeAmp();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-  }
+  public void execute() {}
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    hopper.stopHopper();
+    amp.ampMotorStop();
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (timer.hasElapsed(2.0)) {
-      timer.stop();
-      return true;
-    } else {
-      return false;
-    }
+    return false;
   }
 }

src/main/java/frc/robot/commands/Chassis/FlipDriveOrientation.java → src/main/java/frc/robot/commands/Amp/AmpPrime.java

@@ -2,30 +2,27 @@
 // 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.commands.Chassis;
+package frc.robot.commands.Amp;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Chassis;
-import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.Amp;
 
 /** An example command that uses an example subsystem. */
-public class FlipDriveOrientation extends Command {
-  private final Chassis chassis;
+public class AmpPrime extends Command {
+  private final Amp amp;
 
   /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
+   * @param amp The subsystem used by this command.
    */
-  public FlipDriveOrientation(Chassis subsystem) {
-    this.chassis = subsystem;
-    addRequirements(subsystem);
+  public AmpPrime(Amp amp) {
+    this.amp = amp;
+    addRequirements(amp);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    chassis.flipFieldRelative();
+    amp.primeAmp();
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc/robot/commands/Amp/AmpUnPrime.java

@@ -0,0 +1,41 @@
+// 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.commands.Amp;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Amp;
+
+/** An example command that uses an example subsystem. */
+public class AmpUnPrime extends Command {
+  private final Amp amp;
+
+  /**
+   * @param amp The subsystem used by this command.
+   */
+  public AmpUnPrime(Amp amp) {
+    this.amp = amp;
+    addRequirements(amp);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    amp.unPrimeAmp();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}