From cc8f4912065acda1ae559eab92025300282ab808 Mon Sep 17 00:00:00 2001
From: SR1899 <programming@saintsrobotics.com>
Date: Sat, 9 Mar 2024 10:45:45 -0800
Subject: [PATCH] Fixed arm encoder offset

---
 src/main/java/frc/robot/Constants.java        |  2 +-
 src/main/java/frc/robot/RobotContainer.java   | 20 ++++++++++++-------
 .../frc/robot/subsystems/IntakeSubsystem.java |  1 +
 3 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 309d83a..8fb314f 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -106,7 +106,7 @@ public static final class IntakeConstants {
     public static final double kIntakeAmpScoringAngle = -93; // 193 - 100 (previous angle)
 
     /** Encoder offset in rotations */
-    public static final double kArmEncoderOffset = 0.2252;
+    public static final double kArmEncoderOffset = 0.3415;
 
     public static final double kIntakeSpeed = 0.5;
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cf4085b..26fee18 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -69,7 +69,7 @@ public RobotContainer() {
     NamedCommands.registerCommand("Shoot",
         new SequentialCommandGroup(
             new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting),
-            new ParallelDeadlineGroup(new WaitCommand(2), new NoteOuttakeCommand(m_intakeSubsystem)),
+            new NoteOuttakeCommand(m_intakeSubsystem),
             new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off)));
 
     NamedCommands.registerCommand("Intake",
@@ -93,12 +93,18 @@ public RobotContainer() {
                                                                                // from robot center to
                                                                                // furthest module.
             new ReplanningConfig(false, false)),
-        () ->  
-        {var alliance = DriverStation.getAlliance();
-        if (alliance.isPresent()) {
-          return alliance.get() == DriverStation.Alliance.Red;
-        }
-        return false;}, m_robotDrive);
+        () -> {
+          // Boolean supplier that controls when the path will be mirrored for the red
+          // alliance
+          // This will flip the path being followed to the red side of the field.
+          // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+          var alliance = DriverStation.getAlliance();
+          if (alliance.isPresent()) {
+            return alliance.get() == DriverStation.Alliance.Red;
+          }
+          return false;
+        }, m_robotDrive);
 
     // new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem,
     // ShootSpeed.Shooting),
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index e630d6d..80e1453 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -126,6 +126,7 @@ public void periodic() {
     SmartDashboard.putNumber("intakespeed", m_intakeSpeed);
 
     SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance());
+    SmartDashboard.putNumber("Arm Absolute Angle", m_armEncoder.getAbsolutePosition());
     SmartDashboard.putBoolean("Have Note?", haveNote);
     SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches) : -1);
     SmartDashboard.putNumber("pid output", armMotorSpeed);