Revert "Intake" (#13)

SaintsRobotics · Feb 21, 2024 · 4e18f68 · 4e18f68
1 parent 497b170
commit 4e18f68
Show file tree

Hide file tree

Showing 5 changed files with 131 additions and 234 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -25,121 +25,102 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    /**
-     * Input/Output constants
-     */
-    public static final class IOConstants {
-        public static final int kDriverControllerPort = 0;
-
-        public static final double kControllerDeadband = 0.2;
-        public static final double kSlowModeScalar = 0.8;
-    }
-
-    public static final class DriveConstants {
-        // TODO: set motor and encoder constants
-        public static final int kFrontLeftDriveMotorPort = 32;
-        public static final int kRearLeftDriveMotorPort = 29;
-        public static final int kFrontRightDriveMotorPort = 38;
-        public static final int kRearRightDriveMotorPort = 34;
-
-        public static final int kFrontLeftTurningMotorPort = 28;
-        public static final int kRearLeftTurningMotorPort = 22;
-        public static final int kFrontRightTurningMotorPort = 37;
-        public static final int kRearRightTurningMotorPort = 26;
-
-        public static final int kFrontLeftTurningEncoderPort = 19;
-        public static final int kRearLeftTurningEncoderPort = 20;
-        public static final int kFrontRightTurningEncoderPort = 18;
-        public static final int kRearRightTurningEncoderPort = 17;
-
-        public static final double kFrontLeftTurningEncoderOffset = 0;
-        public static final double kRearLeftTurningEncoderOffset = 0;
-        public static final double kFrontRightTurningEncoderOffset = 0;
-        public static final double kRearRightTurningEncoderOffset = 0;
-
-        // TODO: Test motor orientations before driving on an actual robot
-        public static final boolean kFrontLeftDriveMotorReversed = false;
-        public static final boolean kRearLeftDriveMotorReversed = false;
-        public static final boolean kFrontRightDriveMotorReversed = true;
-        public static final boolean kRearRightDriveMotorReversed = true;
-
-        /** Distance between centers of right and left wheels on robot (in meters). */
-        public static final double kTrackWidth = 0.57785;
-
-        /** Distance between front and back wheels on robot (in meters). */
-        public static final double kWheelBase = 0.57785;
-
-        /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */
-        public static final double kWheelDiameterMeters = 0.1;
-
-        /** Gear ratio between the motor and the wheel. */
-        public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration
-        // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2
-        // configuration
-
-        // TODO: Tune this PID before running on a robot on the ground
-        public static final double kPModuleTurningController = -0.3;
-
-        public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
-                new Translation2d(kWheelBase / 2, kTrackWidth / 2),
-                new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
-                new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
-                new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
-
-        /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
-        public static final double kMaxSpeedMetersPerSecond = 4.4196;
-        /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
-        public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164;
-        // ^^ Calculated using the method taken from the old SDS github example
-
-        /** Heading Correction */
-        public static final double kHeadingCorrectionTurningStopTime = 0.2;
-        // TODO: Tune this PID before running on a robot on the ground
-        public static final double kPHeadingCorrectionController = 5;
-    }
-
-    // Intake PID and Encoder Constants
-    public static class IntakeConstants {
-        public static final double kIntakeLoweredAngle = 9.0;
-        public static final double kIntakeRaisedAngle = 9.0;
-        public static final int kIntakeMotorID = 0;
-        public static final int kArmMotorID = 0;
-        public static final double kIntakeP = 0;
-        public static final double kIntakeI = 0;
-        public static final double kIntakeD = 0;
-        public static final double kArmP = 0;
-        public static final double kArmI = 0;
-        public static final double kArmD = 0;
-        public static final int kArmEncoderCh = 0;
-        public static double kIntakeSpeed;
-    }
-
-    // Shooter subsystem speed constants
-    public static class ShooterConstants {
-        public static final double kSpinSpeedTrue = 0.75;
-        public static final double kSpinSpeedFalse = 0;
-        public static int kBottomShooterMotorPort;
-        public static int kTopShooterMotorPort;
-
-    }
-
-    public static final class VisionConstants {
-        // TODO: Update cam pose relative to center of bot
-        public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
-        public static final double[] kLimelightCamPose = {
-                kCamPose.getX(),
-                kCamPose.getY(),
-                kCamPose.getZ(),
-                kCamPose.getRotation().getX(),
-                kCamPose.getRotation().getY(),
-                kCamPose.getRotation().getZ() };
-
-        // TODO: Experiment with different std devs in the pose estimator
-        public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
-        public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9);
-
-        // Field size in meters
-        public static final double kFieldWidth = 8.21055;
-        public static final double kFieldLength = 16.54175;
-    }
+
+  /**
+   * Input/Output constants
+   */
+  public static final class IOConstants {
+    public static final int kDriverControllerPort = 0;
+
+    public static final double kControllerDeadband = 0.2;
+    public static final double kSlowModeScalar = 0.8;
+  }
+
+  public static final class DriveConstants {
+    // TODO: set motor and encoder constants
+    public static final int kFrontLeftDriveMotorPort = 32;
+    public static final int kRearLeftDriveMotorPort = 29;
+    public static final int kFrontRightDriveMotorPort = 38;
+    public static final int kRearRightDriveMotorPort = 34;
+
+    public static final int kFrontLeftTurningMotorPort = 28;
+    public static final int kRearLeftTurningMotorPort = 22;
+    public static final int kFrontRightTurningMotorPort = 37;
+    public static final int kRearRightTurningMotorPort = 26;
+
+    public static final int kFrontLeftTurningEncoderPort = 19;
+    public static final int kRearLeftTurningEncoderPort = 20;
+    public static final int kFrontRightTurningEncoderPort = 18;
+    public static final int kRearRightTurningEncoderPort = 17;
+
+    public static final double kFrontLeftTurningEncoderOffset = 0;
+    public static final double kRearLeftTurningEncoderOffset = 0;
+    public static final double kFrontRightTurningEncoderOffset = 0;
+    public static final double kRearRightTurningEncoderOffset = 0;
+
+    // TODO: Test motor orientations before driving on an actual robot
+    public static final boolean kFrontLeftDriveMotorReversed = false;
+    public static final boolean kRearLeftDriveMotorReversed = false;
+    public static final boolean kFrontRightDriveMotorReversed = true;
+    public static final boolean kRearRightDriveMotorReversed = true;
+
+    /** Distance between centers of right and left wheels on robot (in meters). */
+    public static final double kTrackWidth = 0.57785;
+
+    /** Distance between front and back wheels on robot (in meters). */
+    public static final double kWheelBase = 0.57785;
+
+    /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */
+    public static final double kWheelDiameterMeters = 0.1;
+
+    /** Gear ratio between the motor and the wheel. */
+    public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration
+    // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration
+
+    // TODO: Tune this PID before running on a robot on the ground
+    public static final double kPModuleTurningController = -0.3;
+
+    public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
+        new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+    /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
+    public static final double kMaxSpeedMetersPerSecond = 4.4196;
+    /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
+    public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164;
+    // ^^ Calculated using the method taken from the old SDS github example
+
+    /** Heading Correction */
+    public static final double kHeadingCorrectionTurningStopTime = 0.2;
+    // TODO: Tune this PID before running on a robot on the ground
+    public static final double kPHeadingCorrectionController = 5;
+  }
+
+  public static final class ShooterConstants {
+    public static final int kTopShooterMotorPort = 35;
+    public static final int kBottomShooterMotorPort = 20;
+  }
+
+  public static final class VisionConstants {
+    // TODO: Update cam pose relative to center of bot
+    public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
+    public static final double[] kLimelightCamPose = {
+        kCamPose.getX(),
+        kCamPose.getY(),
+        kCamPose.getZ(),
+        kCamPose.getRotation().getX(),
+        kCamPose.getRotation().getY(),
+        kCamPose.getRotation().getZ() };
+
+    // TODO: Experiment with different std devs in the pose estimator
+    public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
+    public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9);
+
+    // Field size in meters
+    public static final double kFieldWidth = 8.21055;
+    public static final double kFieldLength = 16.54175;
+  }
+
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -12,11 +12,9 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IOConstants;
 import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
 /*
@@ -26,22 +24,18 @@
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-
+  // The robot's subsystems and commands are defined here
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
   public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
-  public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
-
-  private boolean IntakeDropped = false;
-  private boolean lastAButton = false;
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
 
+  /**
+   * The container for the robot. Contains subsystems, OI devices, and commands.
+   */
   public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
-    m_intakeSubsystem.setDefaultCommand(m_driverController.getLeftTriggerAxis() > 0.5
-        ? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem)
-        : new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem));
 
     m_robotDrive.setDefaultCommand(
         new RunCommand(
@@ -72,32 +66,16 @@ public RobotContainer() {
                     / 2,
                 !m_driverController.getRightBumper()),
             m_robotDrive));
-
-  }
-
-  public void periodic() {
-    if (m_driverController.getAButton()) {
-      lastAButton = true;
-      if (!lastAButton)
-        IntakeDropped = !IntakeDropped;
-    } else {
-      lastAButton = false;
-    }
   }
 
   /**
    * Use this method to define your button->command mappings.
    */
   private void configureBindings() {
+    new JoystickButton(m_driverController, Button.kStart.value)
+        .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));
+
     // TODO: Move shoot commands to operator controller
-    new JoystickButton(m_driverController, Button.kY.value)
-        .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle),
-            m_intakeSubsystem))
-        .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
-    new JoystickButton(m_driverController, Button.kX.value)
-        .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle),
-            m_intakeSubsystem))
-        .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
     new JoystickButton(m_driverController, Button.kB.value)
         .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
         .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
@@ -114,4 +92,4 @@ private void configureBindings() {
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }
-}
+}
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -29,4 +29,4 @@ public void periodic() {
     // This method will be called once per scheduler run
     // SmartDashboard.putNumber("Speed", m_bottom.);
   }
-}
+}
-Original file line number
+Diff line change
@@ Expand Up / @@ -29,4 +29,4 @@ public void periodic() { @@
         // This method will be called once per scheduler run
         // SmartDashboard.putNumber("Speed", m_bottom.);
       }
-    }
+    }