diff --git a/src/main/java/frc/lib/controller/PositionControllerIO.java b/src/main/java/frc/lib/controller/PositionControllerIO.java index fa544ed..b6787c8 100644 --- a/src/main/java/frc/lib/controller/PositionControllerIO.java +++ b/src/main/java/frc/lib/controller/PositionControllerIO.java @@ -14,7 +14,13 @@ public static class PositionControllerIOConstants { * Interpret counterclockwise rotation on the motor face as having positive velocity, if set to * true. */ - public boolean ccwPositive = true; + public boolean ccwPositiveMotor = true; + + /** + * Interpret counterclockwise rotation on the encoder as having positive velocity, if set to + * true. + */ + public boolean ccwPositiveAbsoluteEncoder = true; /** Use the motor to brake the controlled mechanism on neutral output, if set to true. */ public boolean neutralBrake = false; diff --git a/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java b/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java index 50fd9e3..6882cf8 100644 --- a/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java +++ b/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java @@ -83,7 +83,7 @@ public void configure(PositionControllerIOConstants constants) { TalonFXConfiguration motorConfig = new TalonFXConfiguration(); motorConfig.MotorOutput.Inverted = - constants.ccwPositive + constants.ccwPositiveMotor ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive; motorConfig.MotorOutput.NeutralMode = @@ -110,7 +110,7 @@ public void configure(PositionControllerIOConstants constants) { encoderConfig.MagnetSensor.MagnetOffset = constants.absoluteEncoderOffsetRotations; encoderConfig.MagnetSensor.SensorDirection = - constants.ccwPositive + constants.ccwPositiveAbsoluteEncoder ? SensorDirectionValue.CounterClockwise_Positive : SensorDirectionValue.Clockwise_Positive; diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index 18eb7f0..a384e1f 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -47,5 +47,5 @@ public enum Subsystem { EnumSet.of( Subsystem.ARM, Subsystem.INTAKE, Subsystem.ODOMETRY, Subsystem.SHOOTER, Subsystem.SWERVE); - public static final Set REAL_SUBSYSTEMS = ALL_SUBSYSTEMS; + public static final Set REAL_SUBSYSTEMS = EnumSet.of(Subsystem.ARM); } diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index 2c11e29..6b14bd5 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -25,9 +25,9 @@ public static class ShoulderConstants { static { PIDF.kS = 0.14; - PIDF.kG = 0.45; + PIDF.kG = 0.18; PIDF.kV = 4.0; - PIDF.kP = 20.0; + PIDF.kP = 8.0; } /** Shoulder's controller constants. */ @@ -35,15 +35,15 @@ public static class ShoulderConstants { new PositionControllerIOConstants(); static { - CONTROLLER_CONSTANTS.ccwPositive = false; + CONTROLLER_CONSTANTS.ccwPositiveMotor = true; + CONTROLLER_CONSTANTS.ccwPositiveAbsoluteEncoder = false; CONTROLLER_CONSTANTS.neutralBrake = true; CONTROLLER_CONSTANTS.sensorToMechanismRatio = 39.771428571; - CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations = - Units.degreesToRotations(-146.77) + Units.degreesToRotations(-27.07); + CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations = Units.degreesToRotations(-173.135); } /** Maximum speed of the shoulder in rotations per second. */ - public static final double MAXIMUM_SPEED = Units.degreesToRotations(60.0); + public static final double MAXIMUM_SPEED = Units.degreesToRotations(120.0); /** Maximum acceleration of the shoulder in rotations per second per second. */ public static final double MAXIMUM_ACCELERATION = diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index 641b4b3..cac1b47 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -8,9 +8,9 @@ public record ArmState(State shoulderRotations) { - public static final ArmState INITIAL = new ArmState(Rotation2d.fromDegrees(-26.45)); + public static final ArmState INITIAL = new ArmState(Rotation2d.fromDegrees(-25)); - public static final ArmState STOW = new ArmState(Rotation2d.fromDegrees(-26.45)); + public static final ArmState STOW = new ArmState(Rotation2d.fromDegrees(-25)); public static final ArmState SPEAKER = new ArmState(Rotation2d.fromDegrees(-15)); @@ -18,9 +18,9 @@ public record ArmState(State shoulderRotations) { public static final ArmState EJECT = new ArmState(Rotation2d.fromDegrees(0)); - public static final ArmState AMP = new ArmState(Rotation2d.fromDegrees(90)); + public static final ArmState AMP = new ArmState(Rotation2d.fromDegrees(80)); - public static final ArmState CLIMB = new ArmState(Rotation2d.fromDegrees(90)); + public static final ArmState CLIMB = new ArmState(Rotation2d.fromDegrees(80)); public ArmState { Objects.requireNonNull(shoulderRotations);