fix

Author: ShmayaR

src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java

@@ -23,13 +23,13 @@ public class IntakeAssistCommand extends ParallelCommandGroup {
     static final ProfiledPIDController
             X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
             new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
-            new ProfiledPIDController(0.6, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)),
+            new ProfiledPIDController(0.7, 0, 0, new TrapezoidProfile.Constraints(2.65, 4.5)),
             Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
                     new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
                     new ProfiledPIDController(0.25, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)),
             THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
                     new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
-                    new ProfiledPIDController(0.2, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5));
+                    new ProfiledPIDController(0.6, 0, 0, new TrapezoidProfile.Constraints(2.8, 4.5));
     private Translation2d distanceFromTrackedGamePiece;
 
     /**
@@ -161,6 +161,7 @@ private static double calculateNormalAssistPower(double pidOutput, double joysti
 
     private static void resetPIDControllers(Translation2d distanceFromTrackedGamePiece) {
         X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond);
+        X_PID_CONTROLLER.setGoal(-0.15);
         Y_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().vyMetersPerSecond);
         THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond);
     }

src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java

@@ -51,7 +51,7 @@ public static Command getFindCoralCommand(boolean isRight) {
                 SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
                         () -> 0,
                         () -> 0,
-                        () -> AutonomousConstants.AUTO_FIND_CORAL_ROTATION_POWER
+                        () -> 0
                 )
         );
     }

src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java

@@ -6,6 +6,8 @@
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
 import frc.trigon.robot.constants.OperatorConstants;
+import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
+import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import lib.commands.ExecuteEndCommand;
 import lib.commands.GearRatioCalculationCommand;
@@ -45,10 +47,11 @@ public static Command resetElevatorPositionCommand() {
                         RobotContainer.ARM_ELEVATOR
                 ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)),
                 new ExecuteEndCommand(
-                        () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2),
+                        () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2),
                         () -> {},
                         RobotContainer.ARM_ELEVATOR
-                )).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)).finallyDo(RobotContainer.ARM_ELEVATOR::resetElevatorPosition);
+                )
+        ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT)).finallyDo(RobotContainer.ARM_ELEVATOR::resetElevatorPosition);
     }
 
     public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) {

src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java

@@ -48,8 +48,8 @@ public class ArmElevatorConstants {
     private static final double
             ARM_MOTOR_CURRENT_LIMIT = 50,
             ELEVATOR_MOTOR_CURRENT_LIMIT = 50;
-    private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0.184814453125 - 0.25;
-    static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : 0.184814453125 - 0.25 - ANGLE_ENCODER_GRAVITY_OFFSET;
+    private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0.347412109375 - 0.25;
+    static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : 0.347412109375 - 0.25 - ANGLE_ENCODER_GRAVITY_OFFSET;
     private static final boolean
             SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false,
             SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false;
@@ -186,11 +186,11 @@ private static void configureArmMasterMotor() {
         config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID();
         config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
 
-        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 60;
+        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 55;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
         config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 1.2;
         config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.06;
-        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 5.6;
+        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 4.3;
         config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0;
         config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.358;
 
@@ -252,7 +252,7 @@ private static void configureElevatorMasterMotor() {
         config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
         config.Feedback.SensorToMechanismRatio = ELEVATOR_GEAR_RATIO;
 
-        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 12;
+        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 17;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
         config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 1;
         config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0.0546875;
@@ -341,11 +341,11 @@ public enum ArmElevatorState {
         SCORE_L3(Rotation2d.fromDegrees(25), PREPARE_SCORE_L3.targetPositionMeters, PREPARE_SCORE_L3, false, 0.8),
         SCORE_L4(Rotation2d.fromDegrees(-10), 1.5, PREPARE_SCORE_L4, false, 0.8),
         SCORE_NET(Rotation2d.fromDegrees(70), 1.61, null, false, 0.3),
-        SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7),
+        SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.1, null, false, 0.6),
         COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1),
-        COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.2, null, false, 1),
+        COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.15, null, false, 1),
         PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.3, null, false, 1),
-        COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.13, PREPARE_COLLECT_ALGAE_FLOOR, true, 1),
+        COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.16, PREPARE_COLLECT_ALGAE_FLOOR, true, 1),
         COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.15, null, false, 1);
 
 

src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java

@@ -24,7 +24,7 @@ public static Command getDebuggingCommand() {
 
     public static Command resetClimberPositionCommand() {
         return new ExecuteEndCommand(
-                () -> RobotContainer.CLIMBER.setTargetVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 3),
+                () -> RobotContainer.CLIMBER.setTargetVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 3),
                 RobotContainer.CLIMBER::resetClimberPosition,
                 RobotContainer.CLIMBER
         ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0));

src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java

@@ -50,7 +50,7 @@ public class EndEffectorConstants {
     private static final double
             DISTANCE_SENSOR_SCALING_SLOPE = 0.0002,
             DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200;
-    private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 5;
+    private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 4;
     static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent(
             CommandScheduler.getInstance().getActiveButtonLoop(),
             () -> DISTANCE_SENSOR.getScaledValue() < COLLECTION_DETECTION_DISTANCE_CENTIMETRES

src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java

@@ -26,7 +26,7 @@ public static Command getDebuggingCommand() {
 
     public static Command resetIntakePositionCommand() {
         return new ExecuteEndCommand(
-                () -> RobotContainer.INTAKE.setAngleMotorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2),
+                () -> RobotContainer.INTAKE.setAngleMotorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2),
                 RobotContainer.INTAKE::resetIntakePosition,
                 RobotContainer.INTAKE
         ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0));