changes

src/main/java/frc/robot/Constants.java

@@ -60,7 +60,8 @@ public static final class CANConstants {
 
   // Shooter Angles
   public static final double ARMENCODEROFFSET = 2.8378526;
-  public static final double ARMSTARTINGANGLE = 22.5; // WHY MaTH HURT
+  public static final double ARMSTARTINGANGLEOFFSET = -2.2;
+  public static final double ARMSTARTINGANGLE = 22.5 + ARMSTARTINGANGLEOFFSET; // WHY MaTH HURT
   public static final double ARMMINRELATVESTART = 0.0;
   public static final double ARMLOADANGLE = 65 - ARMSTARTINGANGLE;
   public static final double ARMSPEAKERANGLE =

src/main/java/frc/robot/RobotContainer.java

@@ -107,7 +107,7 @@ public class RobotContainer {
   // Init For Autonomous
   // private RamseteAutoBuilder autoBuilder;
   private SendableChooser<String> autoDashboardChooser = new SendableChooser<String>();
-  public final boolean enableAutoProfiling = false;
+  public final boolean enableAutoProfiling = true;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {

src/main/java/frc/robot/commands/ArmCommand.java

@@ -42,10 +42,12 @@ public void initialize() {}
   @Override
   public void execute() {
     if (!m_shooterState.shooting) {
-      if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))
-          && m_shooterState.axisEnabled) {
-        m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput);
-      } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) {
+      if (m_shooterState.axisEnabled) {
+          if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) {
+            m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput);
+          }
+        }
+      else if (m_shooterState.isLoaded & !m_shooterState.isLowered) {
         m_ArmSubsystem.lowerArm();
         m_shooterState.setLowered();
       } else if (!m_shooterState.isLoaded & !m_shooterState.isLowered) {

src/main/java/frc/robot/subsystems/ArmSubsystem.java

@@ -32,10 +32,10 @@ public class ArmSubsystem extends SubsystemBase {
   private final AbsoluteEncoder m_AbsoluteEncoder;
   private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput;
   private static double kDt = 0.02; // 20ms (update rate for wpilib)
-  private final double ksArmVolts = 0.08521;
-  private final double kgArmGravityGain = 0.1711;
-  private final double kvArmVoltSecondsPerMeter = 0.0022383;
-  private final double kaArmVoltSecondsSquaredPerMeter = 0.00041162;
+  private final double ksArmVolts = 0.62208;
+  private final double kgArmGravityGain = 0.23678;
+  private final double kvArmVoltSecondsPerMeter = 1.4195;
+  private final double kaArmVoltSecondsSquaredPerMeter = 0.44504;
   private final double kMinArmAngleRadians = Units.degreesToRadians(Constants.ARMMINRELATVESTART);
   private final double kMaxArmAngleRadians = Units.degreesToRadians(Constants.ARMMAXRELATIVE);
   private final double kArmLoadAngleRadians =
@@ -44,8 +44,8 @@ public class ArmSubsystem extends SubsystemBase {
       Units.degreesToRadians(Constants.ARMSPEAKERANGLE); // angle to be when shooting into speaker
   private final double kArmAmpAngleRadians =
       Units.degreesToRadians(Constants.ARMAMPANGLE); // angle to be when shooting into amp
-  private final double karmMaxVelocity = 1.0; // m/s
-  private final double karmMaxAcceleration = 0.5; // m/s^2
+  private final double karmMaxVelocity = 0.25; // rad/s
+  private final double karmMaxAcceleration = 0.10; // rad/s^2
   private boolean kPIDEnabled = true;
   // general drive constants
   // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2
@@ -94,9 +94,9 @@ public ArmSubsystem(ShooterState shooterState) {
     m_MainEncoder.setVelocityConversionFactor(kVelocityConversionRatio);
     m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMMINRELATVESTART));
     // PID coefficients
-    kP = 1.7496;
+    kP = 2.3142;
     kI = 0;
-    kD = 0.0068645;
+    kD = 0.0;
     kIz = 0;
     kMaxOutput = 0.4;
     kMinOutput = -0.4;

src/main/java/frc/robot/subsystems/CameraSubsystem.java

@@ -26,15 +26,15 @@ public class CameraSubsystem extends SubsystemBase {
   private final PhotonCamera frontCamera;
   public PhotonPipelineResult frontCameraResult;
   // Physical location of camera relative to center
-  private final double CameraLocationXMeters = 0.5;
-  private final double CameraLocationYMeters = 0.0;
-  private final double CameraLocationZMeters = 0.5;
+  private final double CameraLocationXMeters = Units.inchesToMeters(6);
+  private final double CameraLocationYMeters = Units.inchesToMeters(9.3);
+  private final double CameraLocationZMeters = Units.inchesToMeters(10.5);
   // angle of camera / orientation
 
   // Cam mounted facing forward, half a meter forward of center, half a meter up from center.
-  private final double CameraRollRadians = Units.degreesToRadians(0.5);
+  private final double CameraRollRadians = Units.degreesToRadians(90);
   private final double CameraPitchRadians = Units.degreesToRadians(0.0);
-  private final double CameraYawRadians = Units.degreesToRadians(0.5);
+  private final double CameraYawRadians = Units.degreesToRadians(0);
 
   private final Transform3d frontCameraLocation =
       new Transform3d(

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -36,9 +36,9 @@ public class ShooterSubsystem extends SubsystemBase {
   private final double kVelocityConversionRatio = kPositionConversionRatio / 60;
 
   // setup feedforward
-  private final double ksShooterVolts = 0.17875;
-  private final double kvDriveVoltSecondsPerMeter = 1.5442;
-  private final double kaDriveVoltSecondsSquaredPerMeter = 0.019079;
+  private final double ksShooterVolts = 0.2063;
+  private final double kvDriveVoltSecondsPerMeter = 1.5611;
+  private final double kaDriveVoltSecondsSquaredPerMeter = 0.1396;
 
   SimpleMotorFeedforward m_shooterFeedForward =
       new SimpleMotorFeedforward(
@@ -67,7 +67,7 @@ public ShooterSubsystem() {
     m_ShooterMainEncoder.setPositionConversionFactor(kPositionConversionRatio);
     m_ShooterMainEncoder.setVelocityConversionFactor(kVelocityConversionRatio);
     // PID coefficients
-    kP = 0.00032423;
+    kP = 0.00013373;
     kI = 0;
     kD = 0;
     kIz = 0;