diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 2461f16..cc03486 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -1,15 +1,27 @@ package frc.robot.constants; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.math.util.Units; public class ArmConstants { public static final int kArmMotorID = 0; public static final int kCANcoderID = 0; - public static final double kEncoderConversionFactor = 0; - public static final double kArmMaxVelocity = 0; + public static final double kPositionalP = 0; + public static final double kPositionalI = 0; + public static final double kPositionalD = 0; + public static final double kPositionalTolerance = Units.degreesToRadians(1); + + public static final double kVelocityP = 0; + public static final double kVelocityI = 0; + public static final double kVelocityD = 0; + // TODO Is this reasonable? + public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60; + public static final double kArmCoralIntakePosition = 0; public static final double kArmL1Position = 0; public static final double kArmL2Position = 0; @@ -21,5 +33,9 @@ public class ArmConstants { public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); - + static { + canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + canCoderConfig.MagnetSensor.MagnetOffset = 0.0; + canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index d6bcba7..a5d1b86 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -29,7 +29,26 @@ public class Arm extends SubsystemBase { MotorType.kBrushless ); + positionController = new PIDController( + ArmConstants.kPositionalP, + ArmConstants.kPositionalI, + ArmConstants.kPositionalD + ); + + // TODO: Generate constants for continuous input range based on CANcoder configuration? + positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179)); + positionController.setTolerance(ArmConstants.kPositionalTolerance); + + velocityController = new PIDController( + ArmConstants.kVelocityP, + ArmConstants.kVelocityI, + ArmConstants.kVelocityD + ); + + velocityController.setTolerance(ArmConstants.kVelocityTolerance); + canCoder = new CANcoder(ArmConstants.kCANcoderID); + canCoder.getConfigurator().apply(ArmConstants.canCoderConfig); } /** @@ -85,7 +104,7 @@ public class Arm extends SubsystemBase { } public double getEncoderPosition() { - return Units.rotationsToRadians(canCoder.getPosition().getValueAsDouble()); + return Units.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble()); } public double getEncoderVelocity() {