From 9e92f408496178c0f4b20517173fd17ac1d3bb34 Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Fri, 5 Apr 2024 13:10:16 -0400
Subject: [PATCH] changed trim

---
 src/main/java/frc/robot/RobotContainer.java          | 12 ++++++++++++
 .../java/frc/robot/subsystems/IntakeSubsystem.java   | 10 +++++++---
 .../java/frc/robot/subsystems/ShooterSubsystem.java  |  2 +-
 3 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f2f9ffe..2f4adb6 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -175,6 +175,14 @@ private void configureBindings() {
     _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
     _operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.INTAKE));
 
+    _operatorController.povUp().whileTrue(
+      Commands.run(() -> {
+        _intakeSubsystem.actuate(-0.08);
+      }, _intakeSubsystem).handleInterrupt(
+        () -> { _intakeSubsystem.actuate(0); _intakeSubsystem.resetActuator(); } 
+      )
+    );
+
     // driver bindings
     _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem));
     _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem));
@@ -206,6 +214,10 @@ private void configureBindings() {
       }
     }, _swerveSubsystem));
 
+    _driveController.povDown().onTrue(Commands.runOnce(() -> {
+      _swerveSubsystem.resetGyro(180);
+    }, _swerveSubsystem));
+
     _driveController.R3().whileTrue(new NoteAlign(
       _swerveSubsystem,
       _visionSubsystem,
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 674fe3c..ed08078 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -48,7 +48,7 @@ public IntakeSubsystem() {
     _actuatorMotor = new CANSparkMax(Constants.CAN.INTAKE_ACTUATOR, MotorType.kBrushless);
 
     _actuatorEncoder = _actuatorMotor.getEncoder();
-    _actuatorEncoder.setPosition(0);
+    resetActuator();
 
     _actuatorController.setTolerance(0.5);
 
@@ -60,8 +60,8 @@ public IntakeSubsystem() {
     _actuatorMotor.setSoftLimit(SoftLimitDirection.kForward, Constants.Encoders.INTAKE_OUT);
     _actuatorMotor.setSoftLimit(SoftLimitDirection.kReverse, Constants.Encoders.INTAKE_STOWED);
 
-    _actuatorMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
-    _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+    _actuatorMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
+    _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
   }
 
   public boolean hasNote() {
@@ -86,6 +86,10 @@ public void setHasNoteAuton(boolean hasNoteAuton) {
     _hasNoteAuton = hasNoteAuton;
   }
 
+  public void resetActuator() {
+    _actuatorEncoder.setPosition(0);
+  }
+
   /**
    * Returns true if the actuator is at the last desired state.
    */
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 9cb9dea..5c0e45e 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -54,7 +54,7 @@ public class ShooterSubsystem extends SubsystemBase {
 
   private final Timer _revTimer = new Timer();
 
-  private double _shooterTrim = 0;
+  private double _shooterTrim = 5;
 
   private boolean _holdNote = false;