diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b9adf0e..dd60c42 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -232,6 +232,7 @@ public void robotPeriodic() { } /** This function is called once when the robot is disabled. */ + @Override public void disabledInit() {} /** This function is called periodically when disabled. */ diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index c00e959..ad9b4d3 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -79,7 +79,7 @@ public void periodic() { Logger.processInputs("Flywheel", flywheelInputs); Logger.processInputs("Hood", hoodInputs); - turretDisconnectedAlert.set(!turretInputs.connected); + turretDisconnectedAlert.set(!turretInputs.motorControllerConnected); flywheelDisconnectedAlert.set(!flywheelInputs.connected); hoodDisconnectedAlert.set(!hoodInputs.connected); @@ -141,7 +141,8 @@ public void aim(Translation3d target) { // Get actual hood & turret position Rotation2d hoodPosition = hoodInputs.position; - Rotation2d turretPosition = turretInputs.position.plus(turretBasePose.toPose2d().getRotation()); + Rotation2d turretPosition = + turretInputs.relativePosition.plus(turretBasePose.toPose2d().getRotation()); // Build actual velocities Translation3d v0_actual = @@ -180,7 +181,7 @@ public void aim(Translation3d target) { @AutoLogOutput(key = "Launcher/TurretPose") public Pose2d getTurretPose() { - return turretBasePose.toPose2d().plus(new Transform2d(0, 0, turretInputs.position)); + return turretBasePose.toPose2d().plus(new Transform2d(0, 0, turretInputs.relativePosition)); } // @AutoLogOutput(key = "Turret/IsOnTarget") diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 54b1ac5..7ec5330 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -30,19 +30,21 @@ public static final class TurretConstants { // Geometry public static final Transform3d chassisToTurretBase = new Transform3d(Inches.of(0), Inches.of(10), Inches.of(22), Rotation3d.kZero); - public static final Rotation2d rotationOffset = new Rotation2d(0.44); + public static final Rotation2d absEncoderOffset = new Rotation2d(0.5); + public static final Rotation2d mechanismOffset = Rotation2d.kCCW_Pi_2; // Absolute encoder - public static final double encoderPositionFactor = 2 * Math.PI; // Radians - public static final double encoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec + public static final double encoderPositionFactor = (2 * Math.PI) / 5.0; // Radians + public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * 5.0); // Rad/sec // Position controller public static final double minInput = 0.0; public static final double maxInput = 2 * Math.PI; - public static final double kPReal = 0.35; + public static final double kPReal = 0.5; // Motor controller public static final int port = 12; + public static final int DIOPort = 5; public static final double motorReduction = 5.0; public static final AngularVelocity maxAngularVelocity = NEO550Constants.kFreeSpeed.div(motorReduction); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java index 32dfe86..14802c9 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java @@ -7,11 +7,14 @@ public interface TurretIO { @AutoLog public static class TurretIOInputs { - public boolean connected = false; - public Rotation2d position = Rotation2d.kZero; + public boolean motorControllerConnected = false; + public Rotation2d relativePosition = Rotation2d.kZero; public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; public double currentAmps = 0.0; + + public boolean absoluteEncoderConnected = false; + public Rotation2d absolutePosition = Rotation2d.kZero; } public default void updateInputs(TurretIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java index d39e36c..1a8278a 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java @@ -45,8 +45,8 @@ public void updateInputs(TurretIOInputs inputs) { turnSim.update(Robot.defaultPeriodSecs); // Update turn inputs - inputs.connected = true; - inputs.position = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.motorControllerConnected = true; + inputs.relativePosition = new Rotation2d(turnSim.getAngularPositionRad()); inputs.velocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.appliedVolts = appliedVolts; inputs.currentAmps = Math.abs(turnSim.getCurrentDrawAmps()); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index 8273d08..07e5198 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -4,22 +4,22 @@ import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import static frc.robot.util.SparkUtil.*; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import frc.robot.Constants.MotorConstants.NEO550Constants; import frc.robot.Constants.RobotConstants; import java.util.function.DoubleSupplier; @@ -27,15 +27,26 @@ public class TurretIOSpark implements TurretIO { private final SparkBase turnSpark; - private final AbsoluteEncoder turnEncoder; - private final SparkClosedLoopController turnController; + private final RelativeEncoder turnSparkEncoder; + private final PIDController controller; + private final DutyCycleEncoder absoluteEncoder; private final Debouncer turnConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private boolean closedLoop = false; + private double appliedVolts = 0.0; + private double feedforwardVolts = 0.0; + private boolean seeded = false; + public TurretIOSpark() { turnSpark = new SparkMax(port, MotorType.kBrushless); - turnEncoder = turnSpark.getAbsoluteEncoder(); - turnController = turnSpark.getClosedLoopController(); + turnSparkEncoder = turnSpark.getEncoder(); + controller = new PIDController(kPReal, 0.0, 0.0); + absoluteEncoder = + new DutyCycleEncoder( + new DigitalInput(DIOPort), + 2 * Math.PI, + absEncoderOffset.getRadians() + mechanismOffset.getRadians()); var turnConfig = new SparkMaxConfig(); @@ -46,37 +57,13 @@ public TurretIOSpark() { .voltageCompensation(RobotConstants.kNominalVoltage); turnConfig - .absoluteEncoder - .inverted(false) - .zeroCentered(false) - .zeroOffset(rotationOffset.getRotations()) + .encoder .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor) - .averageDepth(2); + .velocityConversionFactor(encoderVelocityFactor); - turnConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) - .positionWrappingEnabled(true) - .positionWrappingInputRange(0, 2 * Math.PI) - .pid(kPReal, 0.0, 0.0); - - // turnConfig - // .softLimit - // .forwardSoftLimitEnabled(false) - // .forwardSoftLimit(maxAngle) - // .reverseSoftLimitEnabled(true) - // .reverseSoftLimit(minAngle); + turnConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPReal, 0.0, 0.0); - turnConfig - .signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs(20) - .absoluteEncoderVelocityAlwaysOn(true) - .absoluteEncoderVelocityPeriodMs(20) - .appliedOutputPeriodMs(20) - .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + turnConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); tryUntilOk( turnSpark, @@ -88,33 +75,56 @@ public TurretIOSpark() { @Override public void updateInputs(TurretIOInputs inputs) { + if (!seeded && absoluteEncoder.isConnected()) { + turnSparkEncoder.setPosition(absoluteEncoder.get()); + seeded = true; + } + + // Run closed-loop control + if (closedLoop) { + appliedVolts = controller.calculate(turnSparkEncoder.getPosition()) + feedforwardVolts; + } else { + controller.reset(); + } + + // Update state + turnSpark.setVoltage( + MathUtil.clamp( + appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + sparkStickyFault = false; - ifOk(turnSpark, turnEncoder::getPosition, (value) -> inputs.position = new Rotation2d(value)); - ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); + ifOk( + turnSpark, + turnSparkEncoder::getPosition, + (value) -> inputs.relativePosition = new Rotation2d(value).plus(mechanismOffset)); + ifOk(turnSpark, turnSparkEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); ifOk( turnSpark, new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, (values) -> inputs.appliedVolts = values[0] * values[1]); ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.currentAmps = value); - inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault); + inputs.motorControllerConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + + inputs.absoluteEncoderConnected = absoluteEncoder.isConnected(); + inputs.absolutePosition = new Rotation2d(absoluteEncoder.get()); } @Override public void setOpenLoop(double output) { - turnSpark.setVoltage(output); + closedLoop = false; + appliedVolts = output; } @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { + closedLoop = true; double setpoint = - // Math.max(minAngle, Math.min(maxAngle, rotation.getRadians())) + - // rotationOffset.getRadians(); - MathUtil.inputModulus(rotation.getRadians(), minInput, maxInput); - double feedforwardVolts = + MathUtil.inputModulus( + rotation.getRadians() - mechanismOffset.getRadians(), minInput, maxInput); + this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); - turnController.setSetpoint( - setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); + controller.setSetpoint(setpoint); } }