From 35dcab53458546ff40a22e99e9140c8ddb56869a Mon Sep 17 00:00:00 2001
From: SR1899 <programming@saintsrobotics.com>
Date: Sat, 2 Mar 2024 15:40:03 -0800
Subject: [PATCH] Testing

---
 src/main/java/frc/robot/Constants.java        |  7 +++---
 .../frc/robot/subsystems/DriveSubsystem.java  | 24 +++++++++----------
 .../frc/robot/subsystems/IntakeSubsystem.java |  2 +-
 .../robot/subsystems/ShooterSubsystem.java    |  4 ++--
 4 files changed, 19 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 59dcb39..4a0b94e 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -101,8 +101,8 @@ public static final class IntakeConstants {
     public static final int kArmEncoderChannel = 0;
 
     // In degrees
-    public static final double kIntakeLoweredAngle = -193;
-    public static final double kIntakeRaisedAngle = 0;
+    public static final double kIntakeLoweredAngle = -183;
+    public static final double kIntakeRaisedAngle = -6;
     public static final double kIntakeAmpScoringAngle = -93; // 193 - 100 (previous angle)
 
     /** Encoder offset in rotations */
@@ -117,7 +117,8 @@ public static final class IntakeConstants {
   public static final class ShooterConstants {
     public static final int kTopShooterMotorPort = 20;
     public static final int kBottomShooterMotorPort = 35;
-    public static final double kShooterSpeed = 0.7;
+    public static final double kShooterSpeedTop = 0.75;
+    public static final double kShooterSpeedBottom = 0.9;
     public static final double kShooterOff = 0;
   }    
   public static class ClimberConstants {
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index dc822ef..b8a87ac 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -94,18 +94,18 @@ public void periodic() {
 
     m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
 
-    SmartDashboard.putNumber("gyro angle", m_gyro.getAngle());
-    SmartDashboard.putNumber("odometryX", m_poseEstimator.getEstimatedPosition().getX());
-    SmartDashboard.putNumber("odometryY", m_poseEstimator.getEstimatedPosition().getY());
-
-    // AdvantageScope Logging
-    double[] logData = {
-        m_frontLeft.getPosition().angle.getDegrees(), m_frontLeft.driveOutput,
-        m_frontRight.getPosition().angle.getDegrees(), m_frontRight.driveOutput,
-        m_rearLeft.getPosition().angle.getDegrees(), m_rearLeft.driveOutput,
-        m_rearRight.getPosition().angle.getDegrees(), m_rearRight.driveOutput,
-    };
-    SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData);
+    // SmartDashboard.putNumber("gyro angle", m_gyro.getAngle());
+    // SmartDashboard.putNumber("odometryX", m_poseEstimator.getEstimatedPosition().getX());
+    // SmartDashboard.putNumber("odometryY", m_poseEstimator.getEstimatedPosition().getY());
+
+    // // AdvantageScope Logging
+    // double[] logData = {
+    //     m_frontLeft.getPosition().angle.getDegrees(), m_frontLeft.driveOutput,
+    //     m_frontRight.getPosition().angle.getDegrees(), m_frontRight.driveOutput,
+    //     m_rearLeft.getPosition().angle.getDegrees(), m_rearLeft.driveOutput,
+    //     m_rearRight.getPosition().angle.getDegrees(), m_rearRight.driveOutput,
+    // };
+    // SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData);
 
     setModuleStates(swerveModuleStates);
   }
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 70600a7..419be10 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -27,7 +27,7 @@ public class IntakeSubsystem extends SubsystemBase {
 
   private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel);
 
-  private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port;
+  private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kMXP); // onboard I2C port;
 
   private double m_intakeSpeed = 0;
   private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index c7ab4f5..2a97fb7 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -31,8 +31,8 @@ public ShooterSubsystem() {
   public void setShootingSpeed(ShootSpeed speed) {
     switch (speed){
       case Shooting:
-        m_topSpeed = ShooterConstants.kShooterSpeed;
-        m_bottomSpeed = ShooterConstants.kShooterSpeed;
+        m_topSpeed = ShooterConstants.kShooterSpeedTop;
+        m_bottomSpeed = ShooterConstants.kShooterSpeedBottom;
         // System.out.println("shoot speed: " + ShooterConstants.kShooterSpeed);
         break;
       case Off: