fixed logic issue and added d to arm pid

Author: ShmayaR

src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java

@@ -144,7 +144,7 @@ private static void configureArmMasterMotor() {
 
         config.Slot0.kP = RobotHardwareStats.isSimulation() ? 100 : 0;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
-        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
+        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 1 : 0;
         config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0;
         config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0;
         config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0;

src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java

@@ -98,7 +98,7 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) {
     }
 
     void setTargetPositionRotations(double targetPositionRotations) {
-        double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() ? 0 : Math.cos(RobotContainer.ARM.getAngle().getDegrees()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
+        double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() ? 0 : Math.cos(RobotContainer.ARM.getAngle().getRadians()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
         double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters);
         masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations)));
     }