this is what happens when you only have 1 programmer for an entire competition weekend

src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto

@@ -11,6 +11,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake in"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto

@@ -11,6 +11,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake in"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/autos/4-Center-Stays.auto

@@ -11,6 +11,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake in"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/paths/Middle5thNote.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 8.0,
-        "y": 0.75
+        "x": 7.96240792481585,
+        "y": 0.5130810261620535
       },
       "prevControl": {
-        "x": 7.0,
-        "y": 0.75
+        "x": 6.96240792481585,
+        "y": 0.5130810261620535
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/MoveOut-Left.path

@@ -16,28 +16,28 @@
     },
     {
       "anchor": {
-        "x": 3.45,
-        "y": 7.5
+        "x": 2.157262600239487,
+        "y": 8.004064008128015
       },
       "prevControl": {
-        "x": 1.9500000000000002,
-        "y": 7.5
+        "x": 0.6572626002394868,
+        "y": 8.004064008128015
       },
       "nextControl": {
-        "x": 4.950000000000003,
-        "y": 7.5
+        "x": 3.6572626002394935,
+        "y": 8.004064008128015
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.0,
-        "y": 7.5
+        "x": 7.1268188250662226,
+        "y": 7.81349105555354
       },
       "prevControl": {
-        "x": 6.0,
-        "y": 7.5
+        "x": 6.1268188250662226,
+        "y": 7.81349105555354
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -41,7 +41,7 @@ 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 kFrontRightDriveMotorPort = 36;
     public static final int kRearRightDriveMotorPort = 34;
 
     public static final int kFrontLeftTurningMotorPort = 28;
@@ -98,15 +98,15 @@ public static final class DriveConstants {
   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 int kArmEncoderChannel = 1;
 
     // In degrees
-    public static final double kIntakeLoweredAngle = -193;
+    public static final double kIntakeLoweredAngle = -188;
     public static final double kIntakeRaisedAngle = 0;
     public static final double kIntakeAmpScoringAngle = -71; // 193 - 100 (previous angle)
 
     /** Encoder offset in rotations */
-    public static final double kArmEncoderOffset = 0.715;
+    public static double kArmEncoderOffset = 0.259;
 
     public static final double kIntakeSpeed = 0.5;
 

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

@@ -12,6 +12,7 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -37,6 +38,7 @@
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
+import frc.robot.subsystems.ServoSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.ArmPosition;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShootSpeed;
@@ -57,15 +59,25 @@ public class RobotContainer {
   private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
   private final LEDSubsystem m_ledSubsystem = new LEDSubsystem();
 
+//   private final ServoSubsystem m_servoSubsystem = new ServoSubsystem();
+
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
   private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
 
   private final SendableChooser<Command> autoChooser;
 
+  private final Servo m_ampServo = new Servo(1);
+
   /**
    * The container for the robot. Contains subsystems, IO devices, and commands.
    */
   public RobotContainer() {
+
+    SmartDashboard.putBoolean("fast", false);
+    SmartDashboard.putBoolean("shoot fast", false);
+
+
+
     NamedCommands.registerCommand("Shoot",
         new SequentialCommandGroup(
             new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime),
@@ -145,24 +157,21 @@ public RobotContainer() {
                     scaleJoysticks(-m_driverController.getLeftY(), -m_driverController.getLeftX()),
                     IOConstants.kControllerDeadband)
                     * DriveConstants.kMaxSpeedMetersPerSecond
-                    * (1 - m_driverController
-                        .getRightTriggerAxis()
+                    * (1 - (SmartDashboard.getBoolean("fast", false) ? m_driverController.getRightTriggerAxis() : 1)
                         * IOConstants.kSlowModeScalar),
                 // * 0.8,
                 MathUtil.applyDeadband(
                     scaleJoysticks(-m_driverController.getLeftX(), -m_driverController.getLeftY()),
                     IOConstants.kControllerDeadband)
                     * DriveConstants.kMaxSpeedMetersPerSecond
-                    * (1 - m_driverController
-                        .getRightTriggerAxis()
+                    * (1 - (SmartDashboard.getBoolean("fast", false) ? m_driverController.getRightTriggerAxis() : 1)
                         * IOConstants.kSlowModeScalar),
                 // * 0.8,
                 MathUtil.applyDeadband(
                     -m_driverController.getRightX(),
                     IOConstants.kControllerDeadband)
                     * DriveConstants.kMaxAngularSpeedRadiansPerSecond
-                    * (1 - m_driverController
-                        .getRightTriggerAxis()
+                    * (1 - (SmartDashboard.getBoolean("fast", false) ? m_driverController.getRightTriggerAxis() : 1)
                         * IOConstants.kSlowModeScalar)
                     * 0.7,
                 !m_driverController.getLeftBumper()),
@@ -196,10 +205,54 @@ private void configureBindings() {
 
     new JoystickButton(m_driverController, Button.kX.value)
         .onTrue(new SequentialCommandGroup(
-            new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime),
+            new ShooterSetSpeedCommand(m_shooterSubsystem, SmartDashboard.getBoolean("shoot fast", false) ? ShootSpeed.Shooting : ShootSpeed.Prep, ShooterConstants.kShooterOnTime),
             new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem))))
         .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime));
 
+    // new JoystickButton(m_driverController, Button.kA.value)
+    //     .onTrue(new InstantCommand(() -> m_ampServo.setAngle(135)));
+
+    // i need some more sleep
+    new JoystickButton(m_driverController, Button.kY.value)
+        .onTrue(new InstantCommand(() -> {
+          m_intakeSubsystem.getCurrentCommand().cancel();
+          m_intakeSubsystem.resetArmEncoder();
+          DriverStation.reportError("reset intake in", false);
+        }));
+
+    // soft reset, should stop driving
+    // new JoystickButton(m_driverController, Button.kA.value)
+    //     .onTrue(new InstantCommand(() -> m_intakeSubsystem.reset()));
+
+    new JoystickButton(m_driverController, Button.kA.value)
+        .onTrue(new InstantCommand(() -> m_intakeSubsystem.resetEvenHarder()));
+
+    // slightly harder reset
+    new JoystickButton(m_driverController, Button.kB.value)
+        .onTrue(new InstantCommand(() -> m_intakeSubsystem.resetHard()));
+
+    // i better run
+    // new JoystickButton(m_driverController, Button.kX.value)
+    //     .onTrue(new InstantCommand(() -> m_intakeSubsystem.daveImSorry()));
+
+    new JoystickButton(m_driverController, Button.kBack.value)
+        .onTrue(new InstantCommand(() -> m_intakeSubsystem.heCantKillMeTwice()));
+
+    // new JoystickButton(m_driverController, Button.kX.value)
+    //     .onTrue(new InstantCommand(() -> {
+    //       // m_intakeSubsystem.getCurrentCommand().cancel();
+    //       DriverStation.reportError("reset intake out", false);
+    //     }));
+
+    // new JoystickButton(m_driverController, Button.kA.value)
+    //     .onTrue(new InstantCommand(() -> m_servoSubsystem.setPulseWidth(2500)));
+
+    // new JoystickButton(m_driverController, Button.kB.value)
+    //     .onTrue(new InstantCommand(() -> m_ampServo.setAngle(-20)));
+
+    // new JoystickButton(m_driverController, Button.kB.value)
+    //     .onTrue(new InstantCommand(() -> m_servoSubsystem.setPulseWidth(1500)));
+
     new JoystickButton(m_driverController, Button.kRightBumper.value)
         .onTrue(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp))
         .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
@@ -219,6 +272,10 @@ private void configureBindings() {
       return m_operatorController.getRightTriggerAxis() > 0.5;
     }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem));
 
+    new Trigger(() -> {
+      return m_operatorController.getAButton() && !m_operatorController.getRightBumper();
+    }).onTrue(new InstantCommand(() -> m_shooterSubsystem.setShootingSpeed(ShootSpeed.Amp))).onFalse(new InstantCommand(() -> m_shooterSubsystem.setShootingSpeed(ShootSpeed.Off)));
+
     // Amp Outtake, Operator Controller X Button
     new JoystickButton(m_operatorController, Button.kX.value)
         .whileTrue(new AmpOuttakeCommand(m_intakeSubsystem));
@@ -231,7 +288,7 @@ private void configureBindings() {
     // Spin up Shooter, Operator Controller Left Trigger
     new Trigger(() -> {
       return m_operatorController.getLeftTriggerAxis() > 0.5;
-    }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime))
+    }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, SmartDashboard.getBoolean("shoot fast", false) ? ShootSpeed.Shooting : ShootSpeed.SlowShoot, ShooterConstants.kShooterOnTime))
         .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime));
 
     // Climber Up, Operator Controller Right Bumper + A Button

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

@@ -68,6 +68,7 @@ public void reverse() {
 
   public Value getState(){
     return m_state;
+    // return Value.kOff;
   }
 
   /**

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

@@ -58,6 +58,40 @@ public void reset() {
     m_armSetpoint = getArmPosition();
   }
 
+  /**
+   * what time is it!
+   * 
+   * 
+   * 
+   * bed time
+   */
+  public void resetHard() {
+    reset();
+
+    m_armEncoder.reset();
+  }
+
+  /** CS swears the problem isnt their fault yet here I am */
+  public void resetEvenHarder() {
+    reset();
+
+    // m_armEncoder.reset();
+    m_armEncoder.setPositionOffset(0.7);
+    // m_armEncoder.reset();
+  }
+
+  /** just play some hans zimmer and hope he gets distracted */
+  public void daveImSorry() {
+    IntakeConstants.kArmEncoderOffset = m_armEncoder.getAbsolutePosition();
+  }
+
+  /** i wish i was at the hans zimmer concert rn */
+  public void heCantKillMeTwice() {
+    SmartDashboard.putNumber("yolo", m_armEncoder.getAbsolutePosition() - IntakeConstants.kIntakeLoweredAngle);
+    IntakeConstants.kArmEncoderOffset = m_armEncoder.getAbsolutePosition() - IntakeConstants.kIntakeLoweredAngle;
+    // m_armSetpoint = m_armEncoder.get() - 60;
+  }
+
   public void setArmPosition(ArmPosition position) {
     m_armPosition = position;
     switch (position) {
@@ -155,4 +189,18 @@ public static enum ArmPosition {
   public void colorSensorToggle() {
     m_colorSensorToggle = !m_colorSensorToggle;
   }
+
+  /** honestly its been a rough day i dont care enough 
+   * 
+   * yolo 
+   */
+  public void resetArmEncoder() {
+    // Double initalizing the encoder crashes the code
+    // m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel);
+
+    // m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset);
+    // m_armEncoder.setDistancePerRotation(360);
+
+    // m_armSetpoint = m_armEncoder.getDistance();
+  }
 }

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

@@ -0,0 +1,54 @@
+// 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 edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * Currently this subsystem is just in case anything goes wrong with the normal, default Servo
+ * implementation in WPILib because the servo we have might not work with that pulse length
+ * 
+ * current stats of MG995 servo:
+ * 4.8 to 7.2 V power and signal
+ * 20ms (50 Hz) PWM period
+ * 5 microsecond dead band width
+ * Rotate about 120 degrees (60 in each direction)
+ * 
+ * 
+ * different source claims that it "provides a running angle of about 180 degrees over a servo
+ * pulse range of 600 us to 2400 us"
+ * 
+ * studica claims pulse width range of 500 to 250 us with a neutral position of 1500 us and a 
+ * dead band width of 4 us
+ * 
+ * another source: https://components101.com/motors/mg995-servo-motor
+ * 
+ * WATCH OUT FOR MS VS US
+ * a cycle for the servo has to be 20ms (50 Hz) so a pulse width of 1500 us would be 1.5 ms
+ */
+public class ServoSubsystem extends SubsystemBase {
+
+  private PWM m_iloveservos = new PWM(1);
+
+  private int m_pulseWidth = 1500;
+
+  /** Creates a new ServoSubsystem. */
+  public ServoSubsystem() {
+    m_iloveservos.setBoundsMicroseconds(20000, 2500, 1500, 500, 20000);
+    // m_iloveservos.setB
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    m_iloveservos.setPulseTimeMicroseconds(m_pulseWidth);
+  }
+
+  /** Set pulse width in MICROSECONDS (us) */
+  public void setPulseWidth(int pulseWidth) {
+    m_pulseWidth = pulseWidth;
+  }
+}