fix: merged well enough this time

SaintsRobotics · Feb 20, 2024 · b35629d · b35629d
1 parent 30d92d6
commit b35629d
Show file tree

Hide file tree

Showing 3 changed files with 137 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -4,9 +4,77 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.numbers.N3;
+
 public final class Constants {
 
+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 kIntakeDroppedAngle = 9.0;
@@ -20,14 +88,38 @@ public static class IntakeConstants {
         public static final double kArmI = 0;
         public static final double kArmD = 0;
         public static final int kArmEncoderCh = 0;
+        public static final double IntakeDroppedAngle = 0;
+        public static final double IntakeRaisedAngle = 0;
     }
 
     //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;
+  }
 }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,6 +13,7 @@
 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.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
@@ -29,30 +30,33 @@ public class RobotContainer {
   private boolean lastAButton = false;
 
   private XboxController controller = new XboxController(0);
+
   public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
-    m_intakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle),m_IntakeSubsystem));
+    m_intakeSubsystem.setDefaultCommand(new InstantCommand(
+        () -> m_intakeSubsystem.load(controller.getRightTriggerAxis() - controller.getLeftTriggerAxis(),
+            IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle),
+        m_intakeSubsystem));
   }
 
-  public void periodic(){
-    if(controller.getAButton()){
+  public void periodic() {
+    if (controller.getAButton()) {
       lastAButton = true;
       if (!lastAButton)
         IntakeDropped = !IntakeDropped;
-    }
-    else {
+    } else {
       lastAButton = false;
     }
   }
 
   private void configureBindings() {
     new JoystickButton(controller, Button.kB.value)
-      .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(0.75), m_ShooterSubsystem))
-      .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem));
+        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
+        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
     new JoystickButton(controller, Button.kA.value)
-      .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(-0.75), m_ShooterSubsystem))
-      .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem));
+        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
+        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
   }
 
   /**

diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -0,0 +1,32 @@
+// 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.subsystems;
+
+import com.revrobotics.CANSparkFlex;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.ShooterConstants;
+
+public class ShooterSubsystem extends SubsystemBase {
+  /** Creates a new ShooterSubsystem. */
+  CANSparkFlex m_bottom = new CANSparkFlex(ShooterConstants.kBottomShooterMotorPort, MotorType.kBrushless);
+  CANSparkFlex m_top = new CANSparkFlex(ShooterConstants.kTopShooterMotorPort, MotorType.kBrushless);
+
+  public ShooterSubsystem() {
+
+  }
+
+  public void spin(double speed) {
+    m_bottom.set(speed);
+    m_top.set(speed);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    // SmartDashboard.putNumber("Speed", m_bottom.);
+  }
+}