From c77a4a0556a99c14166a3ea09c2a49a295397682 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Tue, 27 Aug 2024 17:02:17 -0700 Subject: [PATCH 1/9] =?UTF-8?q?=E2=9C=A8=20Base=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../robot/subsystems/turret/EncoderIO.java | 26 +++++ .../subsystems/turret/EncoderIOCancoder.java | 53 ++++++++++ .../frc/robot/subsystems/turret/Turret.java | 77 +++++++++++++++ .../subsystems/turret/TurretConstants.java | 64 +++++++++++++ .../frc/robot/subsystems/turret/TurretIO.java | 34 +++++++ .../subsystems/turret/TurretIOTalonFX.java | 96 +++++++++++++++++++ 6 files changed, 350 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/turret/EncoderIO.java create mode 100644 src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java create mode 100644 src/main/java/frc/robot/subsystems/turret/Turret.java create mode 100644 src/main/java/frc/robot/subsystems/turret/TurretConstants.java create mode 100644 src/main/java/frc/robot/subsystems/turret/TurretIO.java create mode 100644 src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java diff --git a/src/main/java/frc/robot/subsystems/turret/EncoderIO.java b/src/main/java/frc/robot/subsystems/turret/EncoderIO.java new file mode 100644 index 0000000..21b96e5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/EncoderIO.java @@ -0,0 +1,26 @@ +// Copyright (c) 2024 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface EncoderIO { + + @AutoLog + public static class EncoderIOInputs { + public double encoderPositionDegrees = 0.0; + public double encoderVelocity = 0.0; + + public Rotation2d turretMotorAbsolutePosition = new Rotation2d(); + } + + public default void updateInputs(EncoderIOInputs inputs) {} + + public default void zero() {} +} diff --git a/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java b/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java new file mode 100644 index 0000000..55e3129 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java @@ -0,0 +1,53 @@ +// Copyright (c) 2024 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.subsystems.turret; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; +import edu.wpi.first.math.util.Units; + +public class EncoderIOCancoder implements EncoderIO { + + private final CANcoder encoder; + + private final StatusSignal encoderPosition; + private final StatusSignal encoderVelocity; + + public EncoderIOCancoder() { + encoder = new CANcoder(TurretConstants.kCanCoderID, "mani"); + var response = encoder.getConfigurator().apply(TurretConstants.canCoderConfig); + + if (!response.isOK()) { + System.out.println( + "CANcoder ID " + + TurretConstants.kCanCoderID + + " failed config with error " + + response.getDescription()); + } + + encoderPosition = encoder.getAbsolutePosition(); + encoderVelocity = encoder.getVelocity(); + + BaseStatusSignal.setUpdateFrequencyForAll( + TurretConstants.updateFrequency, encoderPosition, encoderVelocity); + encoder.optimizeBusUtilization(); + } + + @Override + public void updateInputs(EncoderIOInputs inputs) { + BaseStatusSignal.refreshAll(encoderPosition, encoderVelocity); + inputs.encoderPositionDegrees = Units.rotationsToDegrees(encoderPosition.getValueAsDouble()); + inputs.encoderVelocity = encoderVelocity.getValueAsDouble(); + } + + @Override + public void zero() { + encoder.setPosition(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java new file mode 100644 index 0000000..9d96ff4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -0,0 +1,77 @@ +// Copyright (c) 2024 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Turret extends SubsystemBase { + + private final TurretIO turretIO; + private final TurretIOInputsAutoLogged turretIOInputs = new TurretIOInputsAutoLogged(); + + private final EncoderIO encoderIO1; + private final EncoderIOInputsAutoLogged encoderIOInputs1 = new EncoderIOInputsAutoLogged(); + + private final EncoderIO encoderIO2; + private final EncoderIOInputsAutoLogged encoderIOInputs2 = new EncoderIOInputsAutoLogged(); + + public Turret(TurretIO turretIO, EncoderIO encoderIO1, EncoderIO encoderIO2) { + this.turretIO = turretIO; + this.encoderIO1 = encoderIO1; + this.encoderIO2 = encoderIO2; + this.setDefaultCommand(reset()); + } + + @Override + public void periodic() { + turretIO.updateInputs(turretIOInputs); + Logger.processInputs(this.getClass().getSimpleName(), turretIOInputs); + encoderIO1.updateInputs(encoderIOInputs1); + Logger.processInputs(this.getClass().getSimpleName() + "/encoder1", encoderIOInputs1); + encoderIO2.updateInputs(encoderIOInputs2); + Logger.processInputs(this.getClass().getSimpleName() + "/encoder2", encoderIOInputs2); + } + + public Command setPositionRelativeToSwerve(Rotation2d position, Rotation2d swerveAngle) { + return new StartEndCommand( + () -> + turretIO.setPosition( + Units.degreesToRotations(position.getDegrees() - swerveAngle.getDegrees())), + () -> {}, + this); + } + + public Command setPosition(Rotation2d position) { + return new StartEndCommand( + () -> turretIO.setPosition(Units.degreesToRotations(position.getDegrees())), + () -> {}, + this); + } + + public Command zero() { + return new StartEndCommand(() -> turretIO.zero(), () -> {}, this); + } + + public Command reset() { + return new StartEndCommand( + () -> { + if (turretIOInputs.turretMotorPosition > TurretConstants.kForwardLimit) { + turretIO.setPosition(0); + } else if (turretIOInputs.turretMotorPosition < TurretConstants.kReverseLimit) { + turretIO.setPosition(0); + } + }, + () -> {}, + this); + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java new file mode 100644 index 0000000..608e769 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -0,0 +1,64 @@ +// Copyright (c) 2024 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.subsystems.turret; + +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public final class TurretConstants { + + public static int kCanCoderID = 0; // TODO: set id + + public static final CANcoderConfiguration canCoderConfig = + new CANcoderConfiguration() + .withMagnetSensor(new MagnetSensorConfigs().withMagnetOffset(0)); // TODO: set config + public static double updateFrequency = 50.0; + + public static int kTurretMotorID = 0; // TODO: Set ID + + public static final TalonFXConfiguration motorConfigs = // TODO: Set configs + new TalonFXConfiguration() + .withSlot0( + new Slot0Configs() + .withKS(0) + .withKV(0.05) + .withKP(25) + .withKI(0) + .withKD(0) // Original 0.145 + ) + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.Clockwise_Positive)) + .withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicAcceleration(100) + .withMotionMagicCruiseVelocity(100) + .withMotionMagicJerk(420)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(60)) + .withFeedback( + new FeedbackConfigs() + .withFeedbackRemoteSensorID(kCanCoderID) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) + .withSensorToMechanismRatio(1) // TODO: TUNE IMPORTANT + ); + + public static final int gearToothCountE1 = 1; // TODO: Set gear ratio + public static final int gearToothCountE2 = 1; // TODO: Set gear ratio + + public static final boolean kUseMotionMagic = false; + + public static final double kForwardLimit = 69; // TODO: Set limit + public static final double kReverseLimit = -69; // TODO: Set limit + public static int flashConfigRetries = 5; +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java new file mode 100644 index 0000000..1a530f5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -0,0 +1,34 @@ +// Copyright (c) 2024 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.subsystems.turret; + +import org.littletonrobotics.junction.AutoLog; + +public interface TurretIO { + @AutoLog + public static class TurretIOInputs { + public double turretMotorVoltage = 0.0; + public double turretMotorPosition = 0.0; + public double turretMotorStatorCurrent = 0.0; + public double turretMotorSupplyCurrent = 0.0; + public double turretMotorTemperature = 0.0; + public double turretMotorReferenceSlope = 0.0; + } + + public default void updateInputs(TurretIOInputs inputs) {} + + public default void setVoltage(double voltage) {} + + public default void setPosition(double position) {} + + public default void setSensorPosition(double position) {} + + public default void zero() {} + + public default void off() {} +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java new file mode 100644 index 0000000..496a90f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -0,0 +1,96 @@ +// Copyright (c) 2024 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.subsystems.turret; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import frc.robot.utils.PhoenixUtil; + +public class TurretIOTalonFX implements TurretIO { + private final TalonFX turretMotor = new TalonFX(TurretConstants.kTurretMotorID); + private final PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0); + private final MotionMagicExpoVoltage motionMagicRequest = + new MotionMagicExpoVoltage(0).withSlot(0); + + private final StatusSignal turretMotorVoltage = turretMotor.getMotorVoltage(); + private final StatusSignal turretMotorPosition = turretMotor.getPosition(); + private final StatusSignal turretMotorStatorCurrent = turretMotor.getStatorCurrent(); + private final StatusSignal turretMotorSupplyCurrent = turretMotor.getSupplyCurrent(); + private final StatusSignal turretMotorTemperature = turretMotor.getDeviceTemp(); + private final StatusSignal turretMotorReferenceSlope = + turretMotor.getClosedLoopReferenceSlope(); + + public TurretIOTalonFX() { + var motorConfig = TurretConstants.motorConfigs; + ClosedLoopGeneralConfigs closedLoopGeneralConfigs = new ClosedLoopGeneralConfigs(); + closedLoopGeneralConfigs.ContinuousWrap = true; + motorConfig.ClosedLoopGeneral = closedLoopGeneralConfigs; + PhoenixUtil.applyMotorConfigs(turretMotor, motorConfig, TurretConstants.flashConfigRetries); + + BaseStatusSignal.setUpdateFrequencyForAll( + TurretConstants.updateFrequency, + turretMotorVoltage, + turretMotorPosition, + turretMotorStatorCurrent, + turretMotorSupplyCurrent, + turretMotorTemperature, + turretMotorReferenceSlope); + turretMotor.optimizeBusUtilization(); + } + + @Override + public void updateInputs(TurretIOInputs inputs) { + BaseStatusSignal.refreshAll( + turretMotorVoltage, + turretMotorPosition, + turretMotorStatorCurrent, + turretMotorSupplyCurrent, + turretMotorTemperature, + turretMotorReferenceSlope); + inputs.turretMotorVoltage = turretMotorVoltage.getValueAsDouble(); + inputs.turretMotorPosition = turretMotorPosition.getValueAsDouble(); + inputs.turretMotorStatorCurrent = turretMotorStatorCurrent.getValueAsDouble(); + inputs.turretMotorSupplyCurrent = turretMotorSupplyCurrent.getValueAsDouble(); + inputs.turretMotorTemperature = turretMotorTemperature.getValueAsDouble(); + inputs.turretMotorReferenceSlope = turretMotorReferenceSlope.getValueAsDouble(); + } + + @Override + public void setVoltage(double voltage) { + turretMotor.setVoltage(voltage); + } + + @Override + public void setPosition(double position) { + if (TurretConstants.kUseMotionMagic) { + turretMotor.setControl(motionMagicRequest.withPosition(position)); + } else { + turretMotor.setControl(positionRequest.withPosition(position)); + } + } + + @Override + public void off() { + turretMotor.setControl(new NeutralOut()); + } + + @Override + public void setSensorPosition(double position) { + turretMotor.setPosition(position); + } + + @Override + public void zero() { + turretMotor.setPosition(0); + } +} From f7d9254b6958a3a62cd88854d0a67499a09442e9 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Tue, 27 Aug 2024 18:24:37 -0700 Subject: [PATCH 2/9] =?UTF-8?q?=F0=9F=94=A5=20Removed=20unused=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../frc/robot/subsystems/turret/TurretConstants.java | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 608e769..600877a 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -8,7 +8,6 @@ package frc.robot.subsystems.turret; import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -45,13 +44,7 @@ public final class TurretConstants { .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(60)) - .withFeedback( - new FeedbackConfigs() - .withFeedbackRemoteSensorID(kCanCoderID) - .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) - .withSensorToMechanismRatio(1) // TODO: TUNE IMPORTANT - ); + .withStatorCurrentLimit(60)); public static final int gearToothCountE1 = 1; // TODO: Set gear ratio public static final int gearToothCountE2 = 1; // TODO: Set gear ratio From a82290a63ea7c4b4543e1b31834ae37fefa46535 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Thu, 29 Aug 2024 17:38:22 -0700 Subject: [PATCH 3/9] =?UTF-8?q?=F0=9F=90=9B=20Fix=20turret=20calc=20bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- networktables.json | 1 + simgui-ds.json | 92 +++++++++++++++++++ simgui.json | 5 + src/main/java/frc/robot/Robot.java | 17 ++-- src/main/java/frc/robot/RobotContainer.java | 6 ++ .../subsystems/turret/EncoderIOCancoder.java | 6 +- .../frc/robot/subsystems/turret/Turret.java | 15 +++ .../subsystems/turret/TurretConstants.java | 13 ++- 8 files changed, 144 insertions(+), 11 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..e512fa2 --- /dev/null +++ b/simgui.json @@ -0,0 +1,5 @@ +{ + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index eac33cf..c51810e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,6 +18,7 @@ import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; @@ -47,13 +48,15 @@ public void robotInit() { 1, ModuleType.kRev); // Ignore this "resource leak"; it was the example code from docs } else { setUseTiming(false); // Run as fast as possible - String logPath = - LogFileUtil - .findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) - Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log - Logger.addDataReceiver( - new WPILOGWriter( - LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log +// String logPath = +// LogFileUtil +// .findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) +// Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log +// Logger.addDataReceiver( +// new WPILOGWriter( +// LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log + Logger.addDataReceiver(new WPILOGWriter("")); + Logger.addDataReceiver(new NT4Publisher()); } // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 01283fd..da80d45 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,10 @@ import frc.robot.commands.Autos; import frc.robot.commands.ExampleCommand; import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.turret.*; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -25,6 +29,8 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + private final Turret turret = new Turret(new TurretIOTalonFX(), new EncoderIOCancoder(TurretConstants.kCanCoderID1), new EncoderIOCancoder(TurretConstants.kCanCoderID2)); + // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); diff --git a/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java b/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java index 55e3129..0170690 100644 --- a/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java +++ b/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java @@ -19,14 +19,14 @@ public class EncoderIOCancoder implements EncoderIO { private final StatusSignal encoderPosition; private final StatusSignal encoderVelocity; - public EncoderIOCancoder() { - encoder = new CANcoder(TurretConstants.kCanCoderID, "mani"); + public EncoderIOCancoder(int canCoderID) { + encoder = new CANcoder(canCoderID, "mani"); var response = encoder.getConfigurator().apply(TurretConstants.canCoderConfig); if (!response.isOK()) { System.out.println( "CANcoder ID " - + TurretConstants.kCanCoderID + + canCoderID + " failed config with error " + response.getDescription()); } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 9d96ff4..19c850e 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -40,6 +40,21 @@ public void periodic() { Logger.processInputs(this.getClass().getSimpleName() + "/encoder1", encoderIOInputs1); encoderIO2.updateInputs(encoderIOInputs2); Logger.processInputs(this.getClass().getSimpleName() + "/encoder2", encoderIOInputs2); + + } + + public static Rotation2d getTurretPosition(Rotation2d cancoder1, Rotation2d cancoder2) { + Rotation2d diff = cancoder2.minus(cancoder1); + double drivingRotRaw = diff.getDegrees() / TurretConstants.differenceDegrees; + double expectedG1 = (drivingRotRaw * TurretConstants.ratio1) % 1; + double g1Diff = expectedG1 - (cancoder1.getRotations() % 1); + if (g1Diff > 0.5) { + g1Diff -= 1; + } else if (g1Diff < -0.5) { + g1Diff += 1; + } + double drivingRem = g1Diff / TurretConstants.ratio1; + return Rotation2d.fromRotations(drivingRotRaw - drivingRem); } public Command setPositionRelativeToSwerve(Rotation2d position, Rotation2d swerveAngle) { diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 600877a..835bc6b 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -13,7 +13,8 @@ public final class TurretConstants { - public static int kCanCoderID = 0; // TODO: set id + public static int kCanCoderID1 = 0; // TODO: set id + public static int kCanCoderID2 = 0; // TODO: set id public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration() @@ -54,4 +55,14 @@ public final class TurretConstants { public static final double kForwardLimit = 69; // TODO: Set limit public static final double kReverseLimit = -69; // TODO: Set limit public static int flashConfigRetries = 5; + + // CRT constants + public static final int drivingGear1 = 29; + public static final int drivenGear1 = 29; + public static final int drivingGear2 = 29; + public static final int drivenGear2 = 28; + + public static final double ratio1 = (double) drivingGear1 / drivenGear1; + public static final double ratio2 = (double) drivingGear2 / drivenGear2; + public static final double differenceDegrees = (ratio2 - ratio1) * 360; } From f5398922f3c4e7db0040b159007396828a4d9761 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Thu, 29 Aug 2024 17:40:22 -0700 Subject: [PATCH 4/9] =?UTF-8?q?=F0=9F=8E=A8=20Spotless?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Robot.java | 17 ++++++++--------- src/main/java/frc/robot/RobotContainer.java | 9 +++++---- .../subsystems/turret/EncoderIOCancoder.java | 5 +---- .../frc/robot/subsystems/turret/Turret.java | 1 - .../subsystems/turret/TurretConstants.java | 2 +- 5 files changed, 15 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c51810e..5fe8bed 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,11 +15,9 @@ import frc.robot.utils.NT4PublisherNoFMS; import monologue.Logged; import monologue.Monologue; -import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; /** @@ -48,13 +46,14 @@ public void robotInit() { 1, ModuleType.kRev); // Ignore this "resource leak"; it was the example code from docs } else { setUseTiming(false); // Run as fast as possible -// String logPath = -// LogFileUtil -// .findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) -// Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log -// Logger.addDataReceiver( -// new WPILOGWriter( -// LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log + // String logPath = + // LogFileUtil + // .findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the + // user) + // Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log + // Logger.addDataReceiver( + // new WPILOGWriter( + // LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log Logger.addDataReceiver(new WPILOGWriter("")); Logger.addDataReceiver(new NT4Publisher()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index da80d45..a2e8d87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,9 +14,6 @@ import frc.robot.commands.Autos; import frc.robot.commands.ExampleCommand; import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.turret.*; /** @@ -29,7 +26,11 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); - private final Turret turret = new Turret(new TurretIOTalonFX(), new EncoderIOCancoder(TurretConstants.kCanCoderID1), new EncoderIOCancoder(TurretConstants.kCanCoderID2)); + private final Turret turret = + new Turret( + new TurretIOTalonFX(), + new EncoderIOCancoder(TurretConstants.kCanCoderID1), + new EncoderIOCancoder(TurretConstants.kCanCoderID2)); // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandXboxController m_driverController = diff --git a/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java b/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java index 0170690..078596f 100644 --- a/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java +++ b/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java @@ -25,10 +25,7 @@ public EncoderIOCancoder(int canCoderID) { if (!response.isOK()) { System.out.println( - "CANcoder ID " - + canCoderID - + " failed config with error " - + response.getDescription()); + "CANcoder ID " + canCoderID + " failed config with error " + response.getDescription()); } encoderPosition = encoder.getAbsolutePosition(); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 19c850e..21cb52e 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -40,7 +40,6 @@ public void periodic() { Logger.processInputs(this.getClass().getSimpleName() + "/encoder1", encoderIOInputs1); encoderIO2.updateInputs(encoderIOInputs2); Logger.processInputs(this.getClass().getSimpleName() + "/encoder2", encoderIOInputs2); - } public static Rotation2d getTurretPosition(Rotation2d cancoder1, Rotation2d cancoder2) { diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 835bc6b..506fe84 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -14,7 +14,7 @@ public final class TurretConstants { public static int kCanCoderID1 = 0; // TODO: set id - public static int kCanCoderID2 = 0; // TODO: set id + public static int kCanCoderID2 = 0; // TODO: set id public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration() From ad6e64daa48bba3e03545f06684db6e1505cd0fd Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Thu, 29 Aug 2024 18:05:34 -0700 Subject: [PATCH 5/9] =?UTF-8?q?=E2=9C=A8=20Lock=20onto=20speaker=20tag=20v?= =?UTF-8?q?ia=20PID=20or=20pose?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 5 ++ src/main/java/frc/robot/RobotContainer.java | 1 + .../frc/robot/subsystems/turret/Turret.java | 60 +++++++++++++------ .../subsystems/turret/TurretConstants.java | 9 ++- 4 files changed, 56 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f584cb6..ebea7e9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -38,6 +38,11 @@ public static class FeatureFlags { public static final boolean shooterRegenBraking = true; public static final boolean kSwerveEnabled = true; + public static final boolean kClimbEnabled = true; + public static final boolean kIntakeEnabled = true; + public static final boolean kShooterEnabled = true; + public static final boolean kPivotShooterEnabled = true; + public static final boolean kTurretEnabled = true; } // Defaults from Monologue docs diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a2e8d87..108c36d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ public class RobotContainer { private final Turret turret = new Turret( + Constants.FeatureFlags.kTurretEnabled, new TurretIOTalonFX(), new EncoderIOCancoder(TurretConstants.kCanCoderID1), new EncoderIOCancoder(TurretConstants.kCanCoderID2)); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 21cb52e..84c1442 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -7,14 +7,18 @@ package frc.robot.subsystems.turret; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.StartEndCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.PIDCommand; +import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; +import frc.robot.subsystems.vision.Vision; +import frc.robot.utils.DisableSubsystem; import org.littletonrobotics.junction.Logger; -public class Turret extends SubsystemBase { +public class Turret extends DisableSubsystem { private final TurretIO turretIO; private final TurretIOInputsAutoLogged turretIOInputs = new TurretIOInputsAutoLogged(); @@ -25,7 +29,8 @@ public class Turret extends SubsystemBase { private final EncoderIO encoderIO2; private final EncoderIOInputsAutoLogged encoderIOInputs2 = new EncoderIOInputsAutoLogged(); - public Turret(TurretIO turretIO, EncoderIO encoderIO1, EncoderIO encoderIO2) { + public Turret(boolean disabled, TurretIO turretIO, EncoderIO encoderIO1, EncoderIO encoderIO2) { + super(disabled); this.turretIO = turretIO; this.encoderIO1 = encoderIO1; this.encoderIO2 = encoderIO2; @@ -57,35 +62,56 @@ public static Rotation2d getTurretPosition(Rotation2d cancoder1, Rotation2d canc } public Command setPositionRelativeToSwerve(Rotation2d position, Rotation2d swerveAngle) { - return new StartEndCommand( + return this.runOnce( () -> turretIO.setPosition( - Units.degreesToRotations(position.getDegrees() - swerveAngle.getDegrees())), - () -> {}, - this); + Units.degreesToRotations(position.getDegrees() - swerveAngle.getDegrees()))); } public Command setPosition(Rotation2d position) { - return new StartEndCommand( - () -> turretIO.setPosition(Units.degreesToRotations(position.getDegrees())), - () -> {}, - this); + return this.runOnce( + () -> + turretIO.setPosition( + Units.degreesToRotations(position.getDegrees()) / TurretConstants.gearRatio)); } public Command zero() { - return new StartEndCommand(() -> turretIO.zero(), () -> {}, this); + return this.runOnce(() -> turretIO.zero()); + } + + public Command lockToSpeakerTag(Vision vision) { + return new PIDCommand( + new PIDController( + TurretConstants.followTagP, TurretConstants.followTagI, TurretConstants.followTagD), + vision::getCompensatedCenterLimelightX, + 0, + output -> turretIO.setVoltage(output), + this); + } + + public Command lockToSpeakerPose(CommandSwerveDrivetrain swerve, Pose2d speakerPose) { + return this.run( + () -> + this.setPosition( + swerve + .getState() + .Pose + .rotateBy( + getTurretPosition( + Rotation2d.fromDegrees(encoderIOInputs1.encoderPositionDegrees), + Rotation2d.fromDegrees(encoderIOInputs2.encoderPositionDegrees))) + .minus(speakerPose) + .getRotation())); } public Command reset() { - return new StartEndCommand( + return this.runOnce( () -> { if (turretIOInputs.turretMotorPosition > TurretConstants.kForwardLimit) { turretIO.setPosition(0); } else if (turretIOInputs.turretMotorPosition < TurretConstants.kReverseLimit) { turretIO.setPosition(0); } - }, - () -> {}, - this); + }); } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 506fe84..fbde0ea 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -16,12 +16,17 @@ public final class TurretConstants { public static int kCanCoderID1 = 0; // TODO: set id public static int kCanCoderID2 = 0; // TODO: set id + public static final double gearRatio = 1; // TODO: Set gear ratio public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration() .withMagnetSensor(new MagnetSensorConfigs().withMagnetOffset(0)); // TODO: set config - public static double updateFrequency = 50.0; + public static final double updateFrequency = 50.0; - public static int kTurretMotorID = 0; // TODO: Set ID + public static final int kTurretMotorID = 0; // TODO: Set ID + + public static final double followTagP = 1; + public static final double followTagI = 0; + public static final double followTagD = 0; public static final TalonFXConfiguration motorConfigs = // TODO: Set configs new TalonFXConfiguration() From 3f477ad747b2dcb79ef8c71dcfe0c4e2fec4e91e Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Thu, 29 Aug 2024 18:12:42 -0700 Subject: [PATCH 6/9] =?UTF-8?q?=F0=9F=90=9B=20Fix=20lock=20speaker=20posit?= =?UTF-8?q?ioning=20bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/subsystems/turret/Turret.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 84c1442..15947a4 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -101,7 +101,9 @@ public Command lockToSpeakerPose(CommandSwerveDrivetrain swerve, Pose2d speakerP Rotation2d.fromDegrees(encoderIOInputs1.encoderPositionDegrees), Rotation2d.fromDegrees(encoderIOInputs2.encoderPositionDegrees))) .minus(speakerPose) - .getRotation())); + .getRotation().plus(getTurretPosition( + Rotation2d.fromDegrees(encoderIOInputs1.encoderPositionDegrees), + Rotation2d.fromDegrees(encoderIOInputs2.encoderPositionDegrees))))); } public Command reset() { From 239c9832a6fe5e6e84210bbf9f0a5fb11fce4d64 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Thu, 29 Aug 2024 18:16:22 -0700 Subject: [PATCH 7/9] =?UTF-8?q?=F0=9F=94=8A=20Logs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/robot/subsystems/turret/Turret.java | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 15947a4..7cd39b3 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -45,6 +45,11 @@ public void periodic() { Logger.processInputs(this.getClass().getSimpleName() + "/encoder1", encoderIOInputs1); encoderIO2.updateInputs(encoderIOInputs2); Logger.processInputs(this.getClass().getSimpleName() + "/encoder2", encoderIOInputs2); + Logger.recordOutput( + this.getClass().getSimpleName() + "/CRTposition", + getTurretPosition( + Rotation2d.fromDegrees(encoderIOInputs1.encoderPositionDegrees), + Rotation2d.fromDegrees(encoderIOInputs2.encoderPositionDegrees))); } public static Rotation2d getTurretPosition(Rotation2d cancoder1, Rotation2d cancoder2) { @@ -101,9 +106,11 @@ public Command lockToSpeakerPose(CommandSwerveDrivetrain swerve, Pose2d speakerP Rotation2d.fromDegrees(encoderIOInputs1.encoderPositionDegrees), Rotation2d.fromDegrees(encoderIOInputs2.encoderPositionDegrees))) .minus(speakerPose) - .getRotation().plus(getTurretPosition( - Rotation2d.fromDegrees(encoderIOInputs1.encoderPositionDegrees), - Rotation2d.fromDegrees(encoderIOInputs2.encoderPositionDegrees))))); + .getRotation() + .plus( + getTurretPosition( + Rotation2d.fromDegrees(encoderIOInputs1.encoderPositionDegrees), + Rotation2d.fromDegrees(encoderIOInputs2.encoderPositionDegrees))))); } public Command reset() { From b0a7fdd38f333612c4a3dba2d6530cfae757f8cf Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Thu, 29 Aug 2024 19:00:34 -0700 Subject: [PATCH 8/9] =?UTF-8?q?=F0=9F=93=9D=20Javadocs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../frc/robot/subsystems/turret/Turret.java | 44 ++++++++++++++++--- 1 file changed, 37 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 7cd39b3..030459b 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -52,6 +52,13 @@ public void periodic() { Rotation2d.fromDegrees(encoderIOInputs2.encoderPositionDegrees))); } + /** + * @see Chinese remainder theorem sheet calculator + * Uses the calculations from the sheet to calculate the turret position from two encoders + * @param cancoder1 + * @param cancoder2 + * @return rotation2d of the turret position + */ public static Rotation2d getTurretPosition(Rotation2d cancoder1, Rotation2d cancoder2) { Rotation2d diff = cancoder2.minus(cancoder1); double drivingRotRaw = diff.getDegrees() / TurretConstants.differenceDegrees; @@ -66,24 +73,39 @@ public static Rotation2d getTurretPosition(Rotation2d cancoder1, Rotation2d canc return Rotation2d.fromRotations(drivingRotRaw - drivingRem); } - public Command setPositionRelativeToSwerve(Rotation2d position, Rotation2d swerveAngle) { - return this.runOnce( - () -> - turretIO.setPosition( - Units.degreesToRotations(position.getDegrees() - swerveAngle.getDegrees()))); + /** + * @param position Desired position of the turret motor + * @param swerveAngle Current angle of the swerve drivetrain + * @return command to set the position of the turret absolute to the swerve angle + */ + public Command setPositionRelativeToSwerve(Rotation2d position, CommandSwerveDrivetrain swerve) { + return this.run( + () ->turretIO.setPosition(position.minus(swerve.getState().Pose.getRotation()).getRotations() / TurretConstants.gearRatio)); + } + /** + * @param position Desired position of the turret motor + * @return Command to set the turret motor to the desired position / by the gear ratio + */ public Command setPosition(Rotation2d position) { return this.runOnce( () -> - turretIO.setPosition( - Units.degreesToRotations(position.getDegrees()) / TurretConstants.gearRatio)); + turretIO.setPosition(position.getRotations()/ TurretConstants.gearRatio)); } + /** + * @return Command to zero the turret motor's encoder position + */ public Command zero() { return this.runOnce(() -> turretIO.zero()); } + /** + * Will run a PID loop that tracks the tx of the speaker tag and attempts to get it to 0 by moving the turret so the LL crosshair is centered + * @param vision Reference to the vision subsystem + * @return Command to run a PID loop to lock the turret to the speaker tag + */ public Command lockToSpeakerTag(Vision vision) { return new PIDCommand( new PIDController( @@ -94,6 +116,11 @@ public Command lockToSpeakerTag(Vision vision) { this); } + /** + * @param swerve Reference to the swerve drivetrain + * @param speakerPose Pose of the speaker + * @return Command to turn turret to face the speaker + */ public Command lockToSpeakerPose(CommandSwerveDrivetrain swerve, Pose2d speakerPose) { return this.run( () -> @@ -113,6 +140,9 @@ public Command lockToSpeakerPose(CommandSwerveDrivetrain swerve, Pose2d speakerP Rotation2d.fromDegrees(encoderIOInputs2.encoderPositionDegrees))))); } + /** + * @return Command to reset the turret to the forward limit if it is past the forward limit, or to the reverse limit if it is past the reverse limit + */ public Command reset() { return this.runOnce( () -> { From 0d210b0df29dfdbf16839ec2eed49650534b80ce Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Thu, 29 Aug 2024 19:03:22 -0700 Subject: [PATCH 9/9] =?UTF-8?q?=F0=9F=8E=A8=20Spotless=20:/?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../frc/robot/subsystems/turret/Turret.java | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 030459b..45c7099 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PIDCommand; import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -53,8 +52,10 @@ public void periodic() { } /** - * @see Chinese remainder theorem sheet calculator - * Uses the calculations from the sheet to calculate the turret position from two encoders + * @see Chinese + * remainder theorem sheet calculator Uses the calculations from the sheet to calculate + * the turret position from two encoders * @param cancoder1 * @param cancoder2 * @return rotation2d of the turret position @@ -80,8 +81,10 @@ public static Rotation2d getTurretPosition(Rotation2d cancoder1, Rotation2d canc */ public Command setPositionRelativeToSwerve(Rotation2d position, CommandSwerveDrivetrain swerve) { return this.run( - () ->turretIO.setPosition(position.minus(swerve.getState().Pose.getRotation()).getRotations() / TurretConstants.gearRatio)); - + () -> + turretIO.setPosition( + position.minus(swerve.getState().Pose.getRotation()).getRotations() + / TurretConstants.gearRatio)); } /** @@ -90,8 +93,7 @@ public Command setPositionRelativeToSwerve(Rotation2d position, CommandSwerveDri */ public Command setPosition(Rotation2d position) { return this.runOnce( - () -> - turretIO.setPosition(position.getRotations()/ TurretConstants.gearRatio)); + () -> turretIO.setPosition(position.getRotations() / TurretConstants.gearRatio)); } /** @@ -102,7 +104,9 @@ public Command zero() { } /** - * Will run a PID loop that tracks the tx of the speaker tag and attempts to get it to 0 by moving the turret so the LL crosshair is centered + * Will run a PID loop that tracks the tx of the speaker tag and attempts to get it to 0 by moving + * the turret so the LL crosshair is centered + * * @param vision Reference to the vision subsystem * @return Command to run a PID loop to lock the turret to the speaker tag */ @@ -141,7 +145,8 @@ public Command lockToSpeakerPose(CommandSwerveDrivetrain swerve, Pose2d speakerP } /** - * @return Command to reset the turret to the forward limit if it is past the forward limit, or to the reverse limit if it is past the reverse limit + * @return Command to reset the turret to the forward limit if it is past the forward limit, or to + * the reverse limit if it is past the reverse limit */ public Command reset() { return this.runOnce(