Merge branch 'main' into auton

SaintsRobotics · Feb 24, 2024 · 905328d · 905328d
2 parents dd7bca8 + ae7dc15
commit 905328d
Show file tree

Hide file tree

Showing 14 changed files with 369 additions and 253 deletions.
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.1"
 }
 
 java {
@@ -69,6 +69,8 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     //testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    implementation files('vendordeps/DistanceSensor-java/DistanceSensor-java-0.4.0.jar')
 }
 
 test {

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -25,123 +25,117 @@
  * 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 final double kTimeIntake = 5;
-        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 final double kTimeShoot = 5;
-        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 int kOperatorControllerPort = 1;
+
+    public static final double kControllerDeadband = 0.05;
+    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 = 5;
+    public static final int kRearLeftTurningEncoderPort = 6;
+    public static final int kFrontRightTurningEncoderPort = 3;
+    public static final int kRearRightTurningEncoderPort = 4;
+
+    // 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 IntakeConstants {
+    public static final int kIntakeMotorID = 25;
+    public static final int kArmMotorID = 39;
+    public static final int kArmEncoderChannel = 0;
+
+    public static final double kIntakeLoweredAngle = 0;
+    public static final double kIntakeRaisedAngle = 120;
+
+    public static final double kArmEncoderOffset = 0;
+
+    public static final double kIntakeSpeed = 3.0;
+    public static final double kTimeIntake = 5;
+
+    // TODO: Tune distance sensor threshold for detecting note
+    public static final double kDistanceSensorThreshold = 10;
+  }
+
+  public static final class ShooterConstants {
+    public static final int kTopShooterMotorPort = 35;
+    public static final int kBottomShooterMotorPort = 20;
+    public static final double kTimeShoot = 5;
+  }
+
+  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.01);
+    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;
+  }
+
 }