From 90fb7db5bb752d5245b01c2d5b66b96f81821973 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 7 Feb 2026 11:35:50 -0800 Subject: [PATCH 01/17] make turret spinner --- .../robot/subsystems/shooter/TurretIO.java | 161 ++++++++++++++++++ .../robot/subsystems/shooter/TurretIOSim.java | 59 +++++++ 2 files changed, 220 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter/TurretIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java new file mode 100644 index 0000000..feade78 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -0,0 +1,161 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.components.cancoder.CANcoderIO; +import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; + +public class TurretIO { + public static double TURRET_GEAR_RATIO = (42.0 / 12.0) * (32.0 / 16.0) * (85.0 / 10.0); + public static double CANCODER_SHARED_GEAR_TO_TURRET_GEAR_RATIO = 85.0 / 10.0; + // dont think ts is right + // public static double CANCODER_ONE_GEAR_RATIO = (42.0/12)*(24/16); + // public static double CANCODER_TWO_GEAR_RATIO = (42.0/12)*(26/16); + + // shared gear roations per encoder rotations + public static double CANCODER_ONE_GEAR_RATIO = 24.0 / 32.0; + public static double CANCODER_TWO_GEAR_RATIO = 26.0 / 32.0; + + //todo ID? + protected final TalonFX motor = new TalonFX(40, "*"); + private final CANcoderIO cancoderOne; + private final CANcoderIO cancoderTwo; + private final CANcoderIOInputsAutoLogged cancoderOneInputs = new CANcoderIOInputsAutoLogged(); + private final CANcoderIOInputsAutoLogged cancoderTwoInputs = new CANcoderIOInputsAutoLogged(); + + @AutoLog + public static class TurretIOInputs { + public double angularVelocityRotationsPerSec = 0.0; + public Rotation2d positionRotations = new Rotation2d(); + public double statorCurrentAmps = 0.0; + public double supplyCurrentAmp = 0.0; + public double voltage = 0.0; + public double tempCelcius = 0.0; + } + + private final StatusSignal angularVelocityRotationsPerSec = motor.getVelocity(); + private final StatusSignal positionRotations = motor.getPosition(); + private final StatusSignal supplyCurrentAmps = motor.getSupplyCurrent(); + private final StatusSignal statorCurrentAmps = motor.getStatorCurrent(); + private final StatusSignal voltage = motor.getMotorVoltage(); + private final StatusSignal tempCelcius = motor.getDeviceTemp(); + + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); + private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); + + // todo + private Rotation2d turretSetpoint = Rotation2d.kZero; + private Rotation2d turretAbsolutePos = getAbsoluteTurretPosition(); + + public TurretIO(CANcoderIO cancoderOne, CANcoderIO cancoderTwo) { + this.cancoderOne = cancoderOne; + this.cancoderTwo = cancoderTwo; + + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.Feedback.SensorToMechanismRatio = TURRET_GEAR_RATIO; + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + + config.Slot0.kS = 0; + config.Slot0.kG = 0; + config.Slot0.kV = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + motor.getConfigurator().apply(config); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + angularVelocityRotationsPerSec, + positionRotations, + voltage, + statorCurrentAmps, + supplyCurrentAmps, + tempCelcius); + motor.optimizeBusUtilization(); + } + + public void setPositionAngle(Rotation2d positionAngle) { + turretSetpoint = positionAngle; + motor.setControl(positionVoltage.withPosition(positionAngle.getRotations())); + } + + public void updateInputs(TurretIOInputs inputs) { + BaseStatusSignal.refreshAll( + positionRotations, + angularVelocityRotationsPerSec, + voltage, + statorCurrentAmps, + supplyCurrentAmps, + tempCelcius); + + inputs.positionRotations = Rotation2d.fromRotations(positionRotations.getValueAsDouble()); + inputs.angularVelocityRotationsPerSec = angularVelocityRotationsPerSec.getValueAsDouble(); + inputs.voltage = voltage.getValueAsDouble(); + inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); + inputs.supplyCurrentAmp = supplyCurrentAmps.getValueAsDouble(); + inputs.tempCelcius = tempCelcius.getValueAsDouble(); + } + + @AutoLogOutput(key = "Shooter/Turret/Setpoint") + public Rotation2d getTurretSetpoint() { + return turretSetpoint; + } + + // todo + public void resetTurretPosition(Rotation2d turretRotation) { + // uhh is this right + motor.setPosition(turretRotation.getRotations()); + } + + public Rotation2d getAbsoluteTurretPosition() { + // does this account for the gearing before the cancoders ig it wouldnt rlly matter nvm + Rotation2d cancoder1 = cancoderOneInputs.cancoderPositionRotations; + Rotation2d cancoder2 = cancoderTwoInputs.cancoderPositionRotations; + + Rotation2d sharedGearFromCan1 = cancoder1.times(CANCODER_ONE_GEAR_RATIO); + Rotation2d sharedGearFromCan2 = cancoder2.times(CANCODER_TWO_GEAR_RATIO); + + // find difference and wrap to -0.5 and 0.5 + //bc diff wont exceed 1 and we want it to show like which way it is... + //this is the part im less sure about + double diffRotations = + MathUtil.inputModulus( + sharedGearFromCan2.getRotations() - sharedGearFromCan1.getRotations(), -0.5, 0.5); + + // 32/(26-24) = 16 so gearing difference repeats every 16 + // so a full rotation of the shared gear is the difference times 16 + // because after one rotation the difference is like 2t, then 4t etc + // round bc only want the full rotations + double fullRotations = Math.round(diffRotations * 32 / (26 - 24)); + + // total roations by adding full rotations to the position you are on that rotation + double absoluteRotations = fullRotations + sharedGearFromCan1.getRotations(); + + // get turret by using absolute roations times the cancoder shared gear + double turretRotations = absoluteRotations * CANCODER_SHARED_GEAR_TO_TURRET_GEAR_RATIO; + + return Rotation2d.fromRotations(turretRotations); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java new file mode 100644 index 0000000..ff0a935 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.components.cancoder.CANcoderIO; + +public class TurretIOSim extends TurretIO{ + TalonFXSimState motorSim; + + private final DCMotorSim physicsSim; + + private final double simLoopPeriod = 0.002; // 2 ms + private Notifier simNotifier = null; + private double lastSimTime = 0.0; + + + public TurretIOSim(CANcoderIO can1, CANcoderIO can2) { + super(can1, can2); + physicsSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 0.01, TurretIO.TURRET_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)); + + motorSim = motor.getSimState(); + motorSim.setMotorType(MotorType.KrakenX44); + motorSim.Orientation = ChassisReference.Clockwise_Positive; + + simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + final double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; + + motorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); + + physicsSim.setInputVoltage(motorSim.getMotorVoltage()); + physicsSim.update(deltaTime); + + motorSim.setRawRotorPosition( + physicsSim.getAngularPositionRad() * (TurretIO.TURRET_GEAR_RATIO)); + motorSim.setRotorVelocity( + physicsSim.getAngularVelocityRPM() * TurretIO.TURRET_GEAR_RATIO); + }); + + simNotifier.startPeriodic(simLoopPeriod); + } +} From aa15eadf9d08c4691b36ca5010e21fb817cf4f5c Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 7 Feb 2026 15:12:32 -0800 Subject: [PATCH 02/17] update turret position stuff --- .../robot/subsystems/shooter/TurretIO.java | 50 +++++++++++-------- .../robot/subsystems/shooter/TurretIOSim.java | 18 +++---- 2 files changed, 36 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index feade78..8476082 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -23,16 +23,20 @@ public class TurretIO { public static double TURRET_GEAR_RATIO = (42.0 / 12.0) * (32.0 / 16.0) * (85.0 / 10.0); - public static double CANCODER_SHARED_GEAR_TO_TURRET_GEAR_RATIO = 85.0 / 10.0; - // dont think ts is right - // public static double CANCODER_ONE_GEAR_RATIO = (42.0/12)*(24/16); - // public static double CANCODER_TWO_GEAR_RATIO = (42.0/12)*(26/16); + // shared gear rotations per motor turn + public static double MOTOR_TO_SHARED_GEAR_GEAR_RATIO = (12 / 42) * (16 / 32); + // turret rotations per shared gear rotations: + public static double CANCODER_SHARED_GEAR_TO_TURRET_GEAR_RATIO = 10.0 / 85.0; // shared gear roations per encoder rotations public static double CANCODER_ONE_GEAR_RATIO = 24.0 / 32.0; public static double CANCODER_TWO_GEAR_RATIO = 26.0 / 32.0; - //todo ID? + // idk + public static double TURRET_MIN_ROTATIONS = 0.0; + public static double TURRET_MAX_ROTATIONS = 0.8; + + // todo ID? protected final TalonFX motor = new TalonFX(40, "*"); private final CANcoderIO cancoderOne; private final CANcoderIO cancoderTwo; @@ -126,34 +130,38 @@ public Rotation2d getTurretSetpoint() { // todo public void resetTurretPosition(Rotation2d turretRotation) { // uhh is this right - motor.setPosition(turretRotation.getRotations()); + // clamp between max and min bc it can only go so much + motor.setPosition( + Math.min( + Math.max(turretRotation.getRotations(), TURRET_MIN_ROTATIONS), TURRET_MAX_ROTATIONS)); } + //feels dangerously simple public Rotation2d getAbsoluteTurretPosition() { - // does this account for the gearing before the cancoders ig it wouldnt rlly matter nvm Rotation2d cancoder1 = cancoderOneInputs.cancoderPositionRotations; Rotation2d cancoder2 = cancoderTwoInputs.cancoderPositionRotations; - Rotation2d sharedGearFromCan1 = cancoder1.times(CANCODER_ONE_GEAR_RATIO); - Rotation2d sharedGearFromCan2 = cancoder2.times(CANCODER_TWO_GEAR_RATIO); - // find difference and wrap to -0.5 and 0.5 - //bc diff wont exceed 1 and we want it to show like which way it is... - //this is the part im less sure about + // this is bc diff wont exceed 1 and we want it to show like which direction it is double diffRotations = - MathUtil.inputModulus( - sharedGearFromCan2.getRotations() - sharedGearFromCan1.getRotations(), -0.5, 0.5); + MathUtil.inputModulus(cancoder1.getRotations() - cancoder2.getRotations(), -0.5, 0.5); - // 32/(26-24) = 16 so gearing difference repeats every 16 + // round bc we only want the full rotations i think + // actually im not sure the rounding is nessicary i think we can just find it directly but i + // could be wrong + // double fullRotations = round(diffRotations * 32 / (26 - 24)); + + // 32/(26-24) gearing difference repeats every 16 // so a full rotation of the shared gear is the difference times 16 - // because after one rotation the difference is like 2t, then 4t etc - // round bc only want the full rotations - double fullRotations = Math.round(diffRotations * 32 / (26 - 24)); + // get shared gear rotations: + + //don't ask me how i got this number + double absoluteRotations = diffRotations * (24*26)/(32*2); - // total roations by adding full rotations to the position you are on that rotation - double absoluteRotations = fullRotations + sharedGearFromCan1.getRotations(); + // total rotations by adding full rotations to the position you are on that rotation + // double absoluteRotations = fullRotations + sharedGearFromCan1.getRotations(); - // get turret by using absolute roations times the cancoder shared gear + // get turret by using absolute rotations times the cancoder shared gear double turretRotations = absoluteRotations * CANCODER_SHARED_GEAR_TO_TURRET_GEAR_RATIO; return Rotation2d.fromRotations(turretRotations); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java index ff0a935..d1995b5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java @@ -1,13 +1,9 @@ package frc.robot.subsystems.shooter; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.Notifier; @@ -15,8 +11,8 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.components.cancoder.CANcoderIO; -public class TurretIOSim extends TurretIO{ - TalonFXSimState motorSim; +public class TurretIOSim extends TurretIO { + TalonFXSimState motorSim; private final DCMotorSim physicsSim; @@ -24,12 +20,12 @@ public class TurretIOSim extends TurretIO{ private Notifier simNotifier = null; private double lastSimTime = 0.0; - - public TurretIOSim(CANcoderIO can1, CANcoderIO can2) { - super(can1, can2); + public TurretIOSim(CANcoderIO can1, CANcoderIO can2) { + super(can1, can2); physicsSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 0.01, TurretIO.TURRET_GEAR_RATIO), + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.01, TurretIO.TURRET_GEAR_RATIO), DCMotor.getKrakenX44Foc(1)); motorSim = motor.getSimState(); @@ -46,7 +42,7 @@ public TurretIOSim(CANcoderIO can1, CANcoderIO can2) { motorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); physicsSim.setInputVoltage(motorSim.getMotorVoltage()); - physicsSim.update(deltaTime); + physicsSim.update(deltaTime); motorSim.setRawRotorPosition( physicsSim.getAngularPositionRad() * (TurretIO.TURRET_GEAR_RATIO)); From 28374d4a1bfc51e893e32919a62c2df12b5d41d7 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 7 Feb 2026 17:23:38 -0800 Subject: [PATCH 03/17] adjust turrret abs pos again --- src/main/java/frc/robot/Robot.java | 32 +++++++++++++++++-- .../robot/subsystems/shooter/TurretIO.java | 9 +++--- .../subsystems/shooter/TurretSubsystem.java | 1 + 3 files changed, 35 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 01d26b8..f1d72a7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -44,6 +44,7 @@ import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.shooter.TurretIO; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; @@ -427,8 +428,11 @@ public Robot() { }) .onTrue( Commands.runOnce(() -> addAutos()) - .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) - .ignoringDisable(true)); + .alongWith(Commands.runOnce(() -> stupidstupidtest())) + .alongWith( + leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0) + .withTimeout(1.0) + .ignoringDisable(true))); // TODO tbh idk if the leds will work here // Add autos when first connecting to DS @@ -517,6 +521,30 @@ private void addAutos() { "Regenerating Autos on " + DriverStation.getAlliance().map((a) -> a.toString())); } + // TODO: delete stupidstupidtest + private void stupidstupidtest() { + for (int i = -20; i <= 20; i++) { + double cancoder1 = i; + double cancoder2 = cancoder1 * (24.0 / 26.0); + + double diffRotations = + cancoder2 - cancoder1; // MathUtil.inputModulus(cancoder2 - cancoder1, -0.5, 0.5); + + double absoluteRotations = diffRotations * (24.0 * 26.0) / (32.0 * 2.0); + + double turretRotations = + absoluteRotations * TurretIO.CANCODER_SHARED_GEAR_TO_TURRET_GEAR_RATIO; + + System.out.println(i); + System.out.println("can 1 rot: " + cancoder1); + System.out.println("can 2 rot: " + cancoder2); + System.out.println("diff: " + diffRotations); + System.out.println("shared gear: " + absoluteRotations); + System.out.println("turret rot: " + turretRotations); + System.out.println(" "); + } + } + // Sysid Autos // autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index 8476082..d03c1de 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -9,7 +9,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -136,7 +135,7 @@ public void resetTurretPosition(Rotation2d turretRotation) { Math.max(turretRotation.getRotations(), TURRET_MIN_ROTATIONS), TURRET_MAX_ROTATIONS)); } - //feels dangerously simple + // feels dangerously simple public Rotation2d getAbsoluteTurretPosition() { Rotation2d cancoder1 = cancoderOneInputs.cancoderPositionRotations; Rotation2d cancoder2 = cancoderTwoInputs.cancoderPositionRotations; @@ -144,7 +143,7 @@ public Rotation2d getAbsoluteTurretPosition() { // find difference and wrap to -0.5 and 0.5 // this is bc diff wont exceed 1 and we want it to show like which direction it is double diffRotations = - MathUtil.inputModulus(cancoder1.getRotations() - cancoder2.getRotations(), -0.5, 0.5); + cancoder1.getRotations() - cancoder2.getRotations(); // modulus thing , -0.5, 0.5); // round bc we only want the full rotations i think // actually im not sure the rounding is nessicary i think we can just find it directly but i @@ -155,8 +154,8 @@ public Rotation2d getAbsoluteTurretPosition() { // so a full rotation of the shared gear is the difference times 16 // get shared gear rotations: - //don't ask me how i got this number - double absoluteRotations = diffRotations * (24*26)/(32*2); + // don't ask me how i got this number + double absoluteRotations = diffRotations * (24 * 26) / (32 * 2); // total rotations by adding full rotations to the position you are on that rotation // double absoluteRotations = fullRotations + sharedGearFromCan1.getRotations(); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 9a2fb8f..ce66348 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -21,6 +21,7 @@ /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { + /** Creates a new TurretSubsystem. */ public static double HOOD_GEAR_RATIO = 58.96875; From 26fa5aa40637a7797e9f1a765f28cb29c5aaa9cb Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 7 Feb 2026 17:56:11 -0800 Subject: [PATCH 04/17] instantiate turret and cancoders --- src/main/java/frc/robot/Robot.java | 11 ++++++++++- .../frc/robot/components/cancoder/CANcoderIOSim.java | 11 +++++++++++ .../java/frc/robot/subsystems/shooter/TurretIO.java | 12 ++++++++++++ .../robot/subsystems/shooter/TurretSubsystem.java | 8 +++++++- 4 files changed, 40 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f1d72a7..792d65e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,6 +28,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.components.cancoder.CANcoderIOReal; +import frc.robot.components.cancoder.CANcoderIOSim; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; import frc.robot.subsystems.indexer.Indexer; @@ -45,6 +47,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.shooter.TurretIO; +import frc.robot.subsystems.shooter.TurretIOSim; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; @@ -321,7 +324,13 @@ public Robot() { ROBOT_MODE == RobotMode.REAL ? new HoodIO(HoodIO.getCompHood(), canivore, 11) : new HoodIOSim( - canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11)); + canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11), + ROBOT_MODE == RobotMode.REAL + // TODO id's + ? new TurretIO( + new CANcoderIOReal(30, TurretIO.getCancoderConfigs(), canivore), + new CANcoderIOReal(31, TurretIO.getCancoderConfigs(), canivore)) + : new TurretIOSim(new CANcoderIOSim(), new CANcoderIOSim())); // TODO climber break; diff --git a/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java b/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java new file mode 100644 index 0000000..32e080b --- /dev/null +++ b/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java @@ -0,0 +1,11 @@ +package frc.robot.components.cancoder; + +import edu.wpi.first.wpilibj2.command.Commands; + +public class CANcoderIOSim implements CANcoderIO { + public CANcoderIOSim() {} + + public void updateInputs(CANcoderIOInputs inputs) { + Commands.none(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index d03c1de..f852118 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -9,6 +10,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -126,6 +128,16 @@ public Rotation2d getTurretSetpoint() { return turretSetpoint; } + public static CANcoderConfiguration getCancoderConfigs() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.0; + + return config; + } + // todo public void resetTurretPosition(Rotation2d turretRotation) { // uhh is this right diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index ce66348..32eaf93 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -40,11 +40,15 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { FlywheelIO flywheelIO; FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + TurretIO turretIO; + TurretIOInputsAutoLogged turretInputs = new TurretIOInputsAutoLogged(); + private LinearFilter currentFilter = LinearFilter.movingAverage(10); - public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO) { + public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO, TurretIO turretIO) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; + this.turretIO = turretIO; } private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); @@ -56,6 +60,8 @@ public void periodic() { Logger.processInputs("Shooter/Flywheel", flywheelInputs); hoodIO.updateInputs(hoodInputs); Logger.processInputs("Shooter/Hood", hoodInputs); + turretIO.updateInputs(turretInputs); + Logger.processInputs("Shooter/Turret", turretInputs); currentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); } From 19c911efba06f19929119f393596516075c9cab5 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 7 Feb 2026 21:07:38 -0800 Subject: [PATCH 05/17] final turret math hopefull --- src/main/java/frc/robot/Robot.java | 28 ++++++----- .../robot/subsystems/shooter/TurretIO.java | 46 +++++++------------ 2 files changed, 34 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 792d65e..b55b1e3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -437,13 +437,11 @@ public Robot() { }) .onTrue( Commands.runOnce(() -> addAutos()) - .alongWith(Commands.runOnce(() -> stupidstupidtest())) .alongWith( leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0) .withTimeout(1.0) .ignoringDisable(true))); // TODO tbh idk if the leds will work here - // Add autos when first connecting to DS new Trigger( () -> @@ -532,23 +530,31 @@ private void addAutos() { // TODO: delete stupidstupidtest private void stupidstupidtest() { - for (int i = -20; i <= 20; i++) { - double cancoder1 = i; - double cancoder2 = cancoder1 * (24.0 / 26.0); + for (int i = 0; i <= 60; i++) { + double cancoder1 = 0.25 * (i % 4); + double cancoder2 = 0.25 * (i % 4) - ((2.0 / 26.0) * i / 4.0); + if (cancoder2 < 0) { + cancoder2 = 1 + cancoder2; + } + double diffRotations = 0.0; - double diffRotations = - cancoder2 - cancoder1; // MathUtil.inputModulus(cancoder2 - cancoder1, -0.5, 0.5); + if (cancoder1 >= cancoder2) { + diffRotations = cancoder1 - cancoder2; + } else { + diffRotations = (cancoder1 + 1) - cancoder2; + } + // MathUtil.inputModulus(cancoder2 - cancoder1, -0.5, 0.5); - double absoluteRotations = diffRotations * (24.0 * 26.0) / (32.0 * 2.0); + double absoluteRotationsCan1 = diffRotations * (26.0 / 2.0); + // (24.0 * 26.0) / (32.0 * 2.0); - double turretRotations = - absoluteRotations * TurretIO.CANCODER_SHARED_GEAR_TO_TURRET_GEAR_RATIO; + double turretRotations = absoluteRotationsCan1 * TurretIO.CANCODER_ONE_TO_TURRET_GEAR_RATIO; System.out.println(i); System.out.println("can 1 rot: " + cancoder1); System.out.println("can 2 rot: " + cancoder2); System.out.println("diff: " + diffRotations); - System.out.println("shared gear: " + absoluteRotations); + System.out.println("absolute rots can 1: " + absoluteRotationsCan1); System.out.println("turret rot: " + turretRotations); System.out.println(" "); } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index f852118..d25c2c1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -24,14 +24,8 @@ public class TurretIO { public static double TURRET_GEAR_RATIO = (42.0 / 12.0) * (32.0 / 16.0) * (85.0 / 10.0); - // shared gear rotations per motor turn - public static double MOTOR_TO_SHARED_GEAR_GEAR_RATIO = (12 / 42) * (16 / 32); - // turret rotations per shared gear rotations: - public static double CANCODER_SHARED_GEAR_TO_TURRET_GEAR_RATIO = 10.0 / 85.0; - // shared gear roations per encoder rotations - public static double CANCODER_ONE_GEAR_RATIO = 24.0 / 32.0; - public static double CANCODER_TWO_GEAR_RATIO = 26.0 / 32.0; + public static double CANCODER_ONE_TO_TURRET_GEAR_RATIO = (24.0 / 32.0) * (10.0 / 85.0); // idk public static double TURRET_MIN_ROTATIONS = 0.0; @@ -147,33 +141,27 @@ public void resetTurretPosition(Rotation2d turretRotation) { Math.max(turretRotation.getRotations(), TURRET_MIN_ROTATIONS), TURRET_MAX_ROTATIONS)); } - // feels dangerously simple public Rotation2d getAbsoluteTurretPosition() { + // give valaues between 0 and 1 Rotation2d cancoder1 = cancoderOneInputs.cancoderPositionRotations; Rotation2d cancoder2 = cancoderTwoInputs.cancoderPositionRotations; - // find difference and wrap to -0.5 and 0.5 - // this is bc diff wont exceed 1 and we want it to show like which direction it is + // if can one is bigger than can 2 its simply can1-can2 + // otherwise can1 + 1 - can2 because we want how much behind can1 it is double diffRotations = - cancoder1.getRotations() - cancoder2.getRotations(); // modulus thing , -0.5, 0.5); - - // round bc we only want the full rotations i think - // actually im not sure the rounding is nessicary i think we can just find it directly but i - // could be wrong - // double fullRotations = round(diffRotations * 32 / (26 - 24)); - - // 32/(26-24) gearing difference repeats every 16 - // so a full rotation of the shared gear is the difference times 16 - // get shared gear rotations: - - // don't ask me how i got this number - double absoluteRotations = diffRotations * (24 * 26) / (32 * 2); - - // total rotations by adding full rotations to the position you are on that rotation - // double absoluteRotations = fullRotations + sharedGearFromCan1.getRotations(); - - // get turret by using absolute rotations times the cancoder shared gear - double turretRotations = absoluteRotations * CANCODER_SHARED_GEAR_TO_TURRET_GEAR_RATIO; + (cancoder1.getRotations() >= cancoder2.getRotations()) + ? cancoder1.getRotations() - cancoder2.getRotations() + : (cancoder1.getRotations() + 1) - cancoder2.getRotations(); + + // keeping track of how many total rots can1 is doing using the diff with can 2 + // which is just diff times 26/2 because every 13 turns they both reach some full amount of rots + // bc gear ratio + double absoluteRotationsCan1 = diffRotations * (26.0 / 2.0); + + // turret maxes out at like 11 can 1 rotations anyways so it should work up to there and i + // tested + // multiply abs can1 rots by the gear ratio + double turretRotations = absoluteRotationsCan1 * TurretIO.CANCODER_ONE_TO_TURRET_GEAR_RATIO; return Rotation2d.fromRotations(turretRotations); } From c061b9cef619582c5fab87ce68d18e4cc848e90a Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 7 Feb 2026 22:09:30 -0800 Subject: [PATCH 06/17] add turret spinning component to turret subsystem --- .../robot/subsystems/shooter/TurretIO.java | 55 ++++++++++++------- .../subsystems/shooter/TurretSubsystem.java | 7 +++ 2 files changed, 42 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index d25c2c1..36deb43 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -28,8 +28,8 @@ public class TurretIO { public static double CANCODER_ONE_TO_TURRET_GEAR_RATIO = (24.0 / 32.0) * (10.0 / 85.0); // idk - public static double TURRET_MIN_ROTATIONS = 0.0; - public static double TURRET_MAX_ROTATIONS = 0.8; + public static Rotation2d TURRET_MIN_ROTATIONS = Rotation2d.fromRotations(0.0); + public static Rotation2d TURRET_MAX_ROTATIONS = Rotation2d.fromRotations(0.8); // todo ID? protected final TalonFX motor = new TalonFX(40, "*"); @@ -38,6 +38,26 @@ public class TurretIO { private final CANcoderIOInputsAutoLogged cancoderOneInputs = new CANcoderIOInputsAutoLogged(); private final CANcoderIOInputsAutoLogged cancoderTwoInputs = new CANcoderIOInputsAutoLogged(); + @AutoLogOutput(key = "Shooter/Turret/Setpoint") + public Rotation2d getTurretSetpoint() { + return turretSetpoint; + } + + @AutoLogOutput(key = "Shooter/Turret/Cancoder One") + public Rotation2d getTurretCancoderOne() { + return cancoderOneInputs.cancoderPositionRotations; + } + + @AutoLogOutput(key = "Shooter/Turret/Cancoder Two") + public Rotation2d getTurretCancoderTwo() { + return cancoderOneInputs.cancoderPositionRotations; + } + + @AutoLogOutput(key = "Shooter/Turret/Turret Rotation") + public Rotation2d getTurretRotation() { + return getAbsoluteTurretRotations(); + } + @AutoLog public static class TurretIOInputs { public double angularVelocityRotationsPerSec = 0.0; @@ -61,7 +81,6 @@ public static class TurretIOInputs { // todo private Rotation2d turretSetpoint = Rotation2d.kZero; - private Rotation2d turretAbsolutePos = getAbsoluteTurretPosition(); public TurretIO(CANcoderIO cancoderOne, CANcoderIO cancoderTwo) { this.cancoderOne = cancoderOne; @@ -95,11 +114,6 @@ public TurretIO(CANcoderIO cancoderOne, CANcoderIO cancoderTwo) { motor.optimizeBusUtilization(); } - public void setPositionAngle(Rotation2d positionAngle) { - turretSetpoint = positionAngle; - motor.setControl(positionVoltage.withPosition(positionAngle.getRotations())); - } - public void updateInputs(TurretIOInputs inputs) { BaseStatusSignal.refreshAll( positionRotations, @@ -115,11 +129,8 @@ public void updateInputs(TurretIOInputs inputs) { inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); inputs.supplyCurrentAmp = supplyCurrentAmps.getValueAsDouble(); inputs.tempCelcius = tempCelcius.getValueAsDouble(); - } - - @AutoLogOutput(key = "Shooter/Turret/Setpoint") - public Rotation2d getTurretSetpoint() { - return turretSetpoint; + cancoderOne.updateInputs(cancoderOneInputs); + cancoderTwo.updateInputs(cancoderTwoInputs); } public static CANcoderConfiguration getCancoderConfigs() { @@ -132,16 +143,20 @@ public static CANcoderConfiguration getCancoderConfigs() { return config; } - // todo + public void setTurretPosition(Rotation2d positionAngle) { + turretSetpoint = positionAngle; + motor.setControl( + positionVoltage.withPosition( + Math.min( + Math.max(positionAngle.getRotations(), TURRET_MIN_ROTATIONS.getRotations()), + TURRET_MAX_ROTATIONS.getRotations()))); + } + public void resetTurretPosition(Rotation2d turretRotation) { - // uhh is this right - // clamp between max and min bc it can only go so much - motor.setPosition( - Math.min( - Math.max(turretRotation.getRotations(), TURRET_MIN_ROTATIONS), TURRET_MAX_ROTATIONS)); + motor.setPosition(turretRotation.getRotations()); } - public Rotation2d getAbsoluteTurretPosition() { + public Rotation2d getAbsoluteTurretRotations() { // give valaues between 0 and 1 Rotation2d cancoder1 = cancoderOneInputs.cancoderPositionRotations; Rotation2d cancoder2 = cancoderTwoInputs.cancoderPositionRotations; diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 32eaf93..3bbc476 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -78,6 +78,8 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar .getDistance(feedTarget.get().getTranslation())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + // TODO + // turretIO.turretIO.setTurretPosition(shotData.turretRotation()); }); } @@ -87,6 +89,7 @@ public Command rest() { () -> { hoodIO.setHoodPosition(HOOD_MIN_ROTATION); // TODO: TUNE TUCKED POSITION IF NEEDED flywheelIO.setFlywheelVoltage(0.0); + turretIO.setTurretPosition(turretIO.TURRET_MIN_ROTATIONS); }); } @@ -96,6 +99,7 @@ public Command spit() { () -> { hoodIO.setHoodPosition(HOOD_MIN_ROTATION); flywheelIO.setMotionProfiledFlywheelVelocity(20); + turretIO.setTurretPosition(turretIO.TURRET_MIN_ROTATIONS); }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } @@ -133,6 +137,7 @@ public Command testShoot() { () -> { hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + turretIO.setTurretPosition(turretIO.TURRET_MIN_ROTATIONS); }); } @@ -144,6 +149,8 @@ public Command shoot(Supplier robotPoseSupplier) { AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + //TODO + // turretIO.Position(shotData.turretRotation()); }); } } From fe7905d9ae5b10ee624b3c5168adffa6b911fa94 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 7 Feb 2026 22:56:34 -0800 Subject: [PATCH 07/17] auto aim for turret --- .../java/frc/robot/subsystems/shooter/TurretIO.java | 3 +++ .../frc/robot/subsystems/shooter/TurretSubsystem.java | 7 +++---- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 10 ++++++++++ 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index 36deb43..cf260c9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -11,7 +11,10 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 3bbc476..757e14d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.utils.FieldUtils; import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; @@ -78,8 +79,7 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar .getDistance(feedTarget.get().getTranslation())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - // TODO - // turretIO.turretIO.setTurretPosition(shotData.turretRotation()); + turretIO.setTurretPosition(AutoAim.getTargetFacingTurretPosition(robotPoseSupplier.get(), feedTarget.get())); }); } @@ -149,8 +149,7 @@ public Command shoot(Supplier robotPoseSupplier) { AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - //TODO - // turretIO.Position(shotData.turretRotation()); + turretIO.setTurretPosition(AutoAim.getTargetFacingTurretPosition(robotPoseSupplier.get(), FieldUtils.getCurrentHubPose())); }); } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index d1f27a5..b8e4cf5 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -51,4 +51,14 @@ public static double distanceToHub(Pose2d pose) { Logger.recordOutput("Autoaim/Distance To Hub", distance); return distance; } + +public static Rotation2d getTargetFacingTurretPosition(Pose2d robotPose, Pose2d targetPose) { + double dx = targetPose.getX() - robotPose.getX(); + double dy = targetPose.getY() - robotPose.getY(); + + Rotation2d angleToTarget = new Rotation2d(dx, dy); + + Rotation2d turretAngle = angleToTarget.minus(robotPose.getRotation()); + return turretAngle; + } } From cc95d5f7b0a74be86d55b6c6fac5cab139a35d53 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 7 Feb 2026 23:01:15 -0800 Subject: [PATCH 08/17] notes --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index b8e4cf5..07b4547 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -58,6 +58,7 @@ public static Rotation2d getTargetFacingTurretPosition(Pose2d robotPose, Pose2d Rotation2d angleToTarget = new Rotation2d(dx, dy); + //feels right atm but maybe i should check this math tmrw Rotation2d turretAngle = angleToTarget.minus(robotPose.getRotation()); return turretAngle; } From 64ae638c963991693598ce16dbff16bc63921521 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Tue, 10 Feb 2026 09:41:05 -0800 Subject: [PATCH 09/17] move cancoders into turret subsystem --- src/main/deploy/choreo/test.traj | 32 ++++++++ src/main/java/frc/robot/Robot.java | 14 ++-- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../robot/subsystems/shooter/TurretIO.java | 74 ++----------------- .../robot/subsystems/shooter/TurretIOSim.java | 3 +- .../subsystems/shooter/TurretSubsystem.java | 71 +++++++++++++++++- 6 files changed, 121 insertions(+), 75 deletions(-) create mode 100644 src/main/deploy/choreo/test.traj diff --git a/src/main/deploy/choreo/test.traj b/src/main/deploy/choreo/test.traj new file mode 100644 index 0000000..631ce9d --- /dev/null +++ b/src/main/deploy/choreo/test.traj @@ -0,0 +1,32 @@ +{ + "name":"test", + "version":3, + "snapshot":{ + "waypoints":[], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.8321799635887146 m", "val":0.8321799635887146}, "y":{"exp":"0.7060884237289429 m", "val":0.7060884237289429}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.659694671630859 m", "val":4.659694671630859}, "y":{"exp":"0.6415539979934692 m", "val":0.6415539979934692}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.725978374481201 m", "val":2.725978374481201}, "y":{"exp":"4.064524173736572 m", "val":4.064524173736572}, "heading":{"exp":"-33.321136223151115 mrad", "val":-0.03332113622315112}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.7410621643066406 m", "val":2.7410621643066406}, "y":{"exp":"5.301389694213867 m", "val":5.301389694213867}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":null, + "sampleType":null, + "waypoints":[], + "samples":[], + "splits":[] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b55b1e3..86f1cd1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOReal; import frc.robot.components.cancoder.CANcoderIOSim; import frc.robot.components.rollers.RollerIO; @@ -327,11 +328,14 @@ public Robot() { canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11), ROBOT_MODE == RobotMode.REAL // TODO id's - ? new TurretIO( - new CANcoderIOReal(30, TurretIO.getCancoderConfigs(), canivore), - new CANcoderIOReal(31, TurretIO.getCancoderConfigs(), canivore)) - : new TurretIOSim(new CANcoderIOSim(), new CANcoderIOSim())); - + ? new TurretIO() + : new TurretIOSim(), + ROBOT_MODE == RobotMode.REAL + ? new CANcoderIOReal(30, TurretSubsystem.getCancoderConfigs(), canivore) + : new CANcoderIOSim(), + ROBOT_MODE == RobotMode.REAL + ? new CANcoderIOReal(30, TurretSubsystem.getCancoderConfigs(), canivore) + : new CANcoderIOSim()); // TODO climber break; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index cacef67..8574c09 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -34,7 +34,7 @@ public interface Shooter { /** Check if hood is at its desired position */ public boolean atHoodSetpoint(); - + /** Reset hood encoder to its minimum position */ public Command zeroHood(); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index cf260c9..c49c077 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -12,6 +12,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -36,30 +37,6 @@ public class TurretIO { // todo ID? protected final TalonFX motor = new TalonFX(40, "*"); - private final CANcoderIO cancoderOne; - private final CANcoderIO cancoderTwo; - private final CANcoderIOInputsAutoLogged cancoderOneInputs = new CANcoderIOInputsAutoLogged(); - private final CANcoderIOInputsAutoLogged cancoderTwoInputs = new CANcoderIOInputsAutoLogged(); - - @AutoLogOutput(key = "Shooter/Turret/Setpoint") - public Rotation2d getTurretSetpoint() { - return turretSetpoint; - } - - @AutoLogOutput(key = "Shooter/Turret/Cancoder One") - public Rotation2d getTurretCancoderOne() { - return cancoderOneInputs.cancoderPositionRotations; - } - - @AutoLogOutput(key = "Shooter/Turret/Cancoder Two") - public Rotation2d getTurretCancoderTwo() { - return cancoderOneInputs.cancoderPositionRotations; - } - - @AutoLogOutput(key = "Shooter/Turret/Turret Rotation") - public Rotation2d getTurretRotation() { - return getAbsoluteTurretRotations(); - } @AutoLog public static class TurretIOInputs { @@ -68,7 +45,7 @@ public static class TurretIOInputs { public double statorCurrentAmps = 0.0; public double supplyCurrentAmp = 0.0; public double voltage = 0.0; - public double tempCelcius = 0.0; + public double tempCelsius = 0.0; } private final StatusSignal angularVelocityRotationsPerSec = motor.getVelocity(); @@ -85,9 +62,7 @@ public static class TurretIOInputs { // todo private Rotation2d turretSetpoint = Rotation2d.kZero; - public TurretIO(CANcoderIO cancoderOne, CANcoderIO cancoderTwo) { - this.cancoderOne = cancoderOne; - this.cancoderTwo = cancoderTwo; + public TurretIO() { final TalonFXConfiguration config = new TalonFXConfiguration(); @@ -131,27 +106,14 @@ public void updateInputs(TurretIOInputs inputs) { inputs.voltage = voltage.getValueAsDouble(); inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); inputs.supplyCurrentAmp = supplyCurrentAmps.getValueAsDouble(); - inputs.tempCelcius = tempCelcius.getValueAsDouble(); - cancoderOne.updateInputs(cancoderOneInputs); - cancoderTwo.updateInputs(cancoderTwoInputs); - } - - public static CANcoderConfiguration getCancoderConfigs() { - CANcoderConfiguration config = new CANcoderConfiguration(); - - config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - config.MagnetSensor.MagnetOffset = 0.0; - config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.0; - - return config; + inputs.tempCelsius = tempCelcius.getValueAsDouble(); } public void setTurretPosition(Rotation2d positionAngle) { turretSetpoint = positionAngle; motor.setControl( positionVoltage.withPosition( - Math.min( - Math.max(positionAngle.getRotations(), TURRET_MIN_ROTATIONS.getRotations()), + MathUtil.clamp(positionAngle.getRotations(), TURRET_MIN_ROTATIONS.getRotations(), TURRET_MAX_ROTATIONS.getRotations()))); } @@ -159,28 +121,8 @@ public void resetTurretPosition(Rotation2d turretRotation) { motor.setPosition(turretRotation.getRotations()); } - public Rotation2d getAbsoluteTurretRotations() { - // give valaues between 0 and 1 - Rotation2d cancoder1 = cancoderOneInputs.cancoderPositionRotations; - Rotation2d cancoder2 = cancoderTwoInputs.cancoderPositionRotations; - - // if can one is bigger than can 2 its simply can1-can2 - // otherwise can1 + 1 - can2 because we want how much behind can1 it is - double diffRotations = - (cancoder1.getRotations() >= cancoder2.getRotations()) - ? cancoder1.getRotations() - cancoder2.getRotations() - : (cancoder1.getRotations() + 1) - cancoder2.getRotations(); - - // keeping track of how many total rots can1 is doing using the diff with can 2 - // which is just diff times 26/2 because every 13 turns they both reach some full amount of rots - // bc gear ratio - double absoluteRotationsCan1 = diffRotations * (26.0 / 2.0); - - // turret maxes out at like 11 can 1 rotations anyways so it should work up to there and i - // tested - // multiply abs can1 rots by the gear ratio - double turretRotations = absoluteRotationsCan1 * TurretIO.CANCODER_ONE_TO_TURRET_GEAR_RATIO; - - return Rotation2d.fromRotations(turretRotations); + @AutoLogOutput(key = "Shooter/Turret/Setpoint") + public Rotation2d getTurretSetpoint() { + return turretSetpoint; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java index d1995b5..c300357 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java @@ -20,8 +20,7 @@ public class TurretIOSim extends TurretIO { private Notifier simNotifier = null; private double lastSimTime = 0.0; - public TurretIOSim(CANcoderIO can1, CANcoderIO can2) { - super(can1, can2); + public TurretIOSim() { physicsSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 757e14d..7d2f52b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.components.cancoder.CANcoderIO; +import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; @@ -20,6 +22,9 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.signals.SensorDirectionValue; + /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { @@ -35,6 +40,12 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; double currentFilterValue = 0.0; + private final CANcoderIO cancoderOne; + private final CANcoderIO cancoderTwo; + private final CANcoderIOInputsAutoLogged cancoderOneInputs = new CANcoderIOInputsAutoLogged(); + private final CANcoderIOInputsAutoLogged cancoderTwoInputs = new CANcoderIOInputsAutoLogged(); + + //shouldnt this be private final HoodIO hoodIO; HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); @@ -46,10 +57,12 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { private LinearFilter currentFilter = LinearFilter.movingAverage(10); - public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO, TurretIO turretIO) { + public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO, TurretIO turretIO, CANcoderIO cancoderOne, CANcoderIO cancoderTwo) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; this.turretIO = turretIO; + this.cancoderOne = cancoderOne; + this.cancoderTwo = cancoderTwo; } private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); @@ -63,10 +76,22 @@ public void periodic() { Logger.processInputs("Shooter/Hood", hoodInputs); turretIO.updateInputs(turretInputs); Logger.processInputs("Shooter/Turret", turretInputs); + cancoderOne.updateInputs(cancoderOneInputs); + cancoderTwo.updateInputs(cancoderTwoInputs); currentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); } + public static CANcoderConfiguration getCancoderConfigs() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.0; + + return config; + } + @Override public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { return this.run( @@ -103,6 +128,34 @@ public Command spit() { }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } + public Rotation2d getAbsoluteTurretRotations() { + // give valaues between 0 and 1 + Rotation2d cancoder1 = cancoderOneInputs.cancoderPositionRotations; + Rotation2d cancoder2 = cancoderTwoInputs.cancoderPositionRotations; + + // if can one is bigger than can 2 its simply can1-can2 + // otherwise can1 + 1 - can2 because we want how much behind can1 it is + double diffRotations = + (cancoder1.getRotations() >= cancoder2.getRotations()) + ? cancoder1.getRotations() - cancoder2.getRotations() + : (cancoder1.getRotations() + 1) - cancoder2.getRotations(); + + // keeping track of how many total rots can1 is doing using the diff with can2 + //26/2 because gear difference of 2 + double absoluteRotationsCan1 = diffRotations * (26.0 / 2.0); + + // turret maxes out at less then 1 rotation which is like 11 can1 rotations anyways and it should work up to there + // multiply abs can1 rots by the gear ratio + double turretRotations = absoluteRotationsCan1 * TurretIO.CANCODER_ONE_TO_TURRET_GEAR_RATIO; + + return Rotation2d.fromRotations(turretRotations); + } + + @AutoLogOutput(key = "Shooter/Turret/Turret Absolute Rotation") + public Rotation2d getTurretRotation() { + return getAbsoluteTurretRotations(); + } + @Override @AutoLogOutput(key = "Shooter/At Vel Setpoint") public boolean atFlywheelVelocitySetpoint() { @@ -119,6 +172,22 @@ public boolean atHoodSetpoint() { hoodInputs.hoodPositionRotations.getDegrees(), hoodIO.getHoodSetpoint().getDegrees(), 1); } + @AutoLogOutput(key = "Shooter/Turret/At Setpoint") + public boolean atTurretSetpoint() { + return MathUtil.isNear( + getAbsoluteTurretRotations().getDegrees(), turretIO.getTurretSetpoint().getDegrees(), 1); + } + + @AutoLogOutput(key = "Shooter/Turret/Cancoder One") + public Rotation2d getTurretCancoderOne() { + return cancoderOneInputs.cancoderPositionRotations; + } + + @AutoLogOutput(key = "Shooter/Turret/Cancoder Two") + public Rotation2d getTurretCancoderTwo() { + return cancoderOneInputs.cancoderPositionRotations; + } + @Override public Command zeroHood() { return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ROTATION)); From 279ca072827db34a5d6d92f078d30fbdcf255f17 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Tue, 10 Feb 2026 18:38:06 -0800 Subject: [PATCH 10/17] motion magic control --- src/main/java/frc/robot/Robot.java | 45 +++---------------- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../robot/subsystems/shooter/TurretIO.java | 15 +++---- .../robot/subsystems/shooter/TurretIOSim.java | 1 - .../subsystems/shooter/TurretSubsystem.java | 44 +++++++++--------- .../java/frc/robot/utils/autoaim/AutoAim.java | 4 +- 6 files changed, 39 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 86f1cd1..5d6c05d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,7 +28,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOReal; import frc.robot.components.cancoder.CANcoderIOSim; import frc.robot.components.rollers.RollerIO; @@ -329,13 +328,13 @@ public Robot() { ROBOT_MODE == RobotMode.REAL // TODO id's ? new TurretIO() - : new TurretIOSim(), - ROBOT_MODE == RobotMode.REAL - ? new CANcoderIOReal(30, TurretSubsystem.getCancoderConfigs(), canivore) + : new TurretIOSim(), + ROBOT_MODE == RobotMode.REAL + ? new CANcoderIOReal(30, TurretSubsystem.getCancoderConfigs(), canivore) : new CANcoderIOSim(), - ROBOT_MODE == RobotMode.REAL - ? new CANcoderIOReal(30, TurretSubsystem.getCancoderConfigs(), canivore) - : new CANcoderIOSim()); + ROBOT_MODE == RobotMode.REAL + ? new CANcoderIOReal(30, TurretSubsystem.getCancoderConfigs(), canivore) + : new CANcoderIOSim()); // TODO climber break; } @@ -532,38 +531,6 @@ private void addAutos() { "Regenerating Autos on " + DriverStation.getAlliance().map((a) -> a.toString())); } - // TODO: delete stupidstupidtest - private void stupidstupidtest() { - for (int i = 0; i <= 60; i++) { - double cancoder1 = 0.25 * (i % 4); - double cancoder2 = 0.25 * (i % 4) - ((2.0 / 26.0) * i / 4.0); - if (cancoder2 < 0) { - cancoder2 = 1 + cancoder2; - } - double diffRotations = 0.0; - - if (cancoder1 >= cancoder2) { - diffRotations = cancoder1 - cancoder2; - } else { - diffRotations = (cancoder1 + 1) - cancoder2; - } - // MathUtil.inputModulus(cancoder2 - cancoder1, -0.5, 0.5); - - double absoluteRotationsCan1 = diffRotations * (26.0 / 2.0); - // (24.0 * 26.0) / (32.0 * 2.0); - - double turretRotations = absoluteRotationsCan1 * TurretIO.CANCODER_ONE_TO_TURRET_GEAR_RATIO; - - System.out.println(i); - System.out.println("can 1 rot: " + cancoder1); - System.out.println("can 2 rot: " + cancoder2); - System.out.println("diff: " + diffRotations); - System.out.println("absolute rots can 1: " + absoluteRotationsCan1); - System.out.println("turret rot: " + turretRotations); - System.out.println(" "); - } - } - // Sysid Autos // autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 8574c09..cacef67 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -34,7 +34,7 @@ public interface Shooter { /** Check if hood is at its desired position */ public boolean atHoodSetpoint(); - + /** Reset hood encoder to its minimum position */ public Command zeroHood(); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index c49c077..ab1afa3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -2,27 +2,21 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; - import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; -import frc.robot.components.cancoder.CANcoderIO; -import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.AutoLogOutput; @@ -58,6 +52,7 @@ public static class TurretIOInputs { private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); + private MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0.0).withEnableFOC(true); // todo private Rotation2d turretSetpoint = Rotation2d.kZero; @@ -112,8 +107,10 @@ public void updateInputs(TurretIOInputs inputs) { public void setTurretPosition(Rotation2d positionAngle) { turretSetpoint = positionAngle; motor.setControl( - positionVoltage.withPosition( - MathUtil.clamp(positionAngle.getRotations(), TURRET_MIN_ROTATIONS.getRotations(), + motionMagicVoltage.withPosition( + MathUtil.clamp( + positionAngle.getRotations(), + TURRET_MIN_ROTATIONS.getRotations(), TURRET_MAX_ROTATIONS.getRotations()))); } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java index c300357..47703aa 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.components.cancoder.CANcoderIO; public class TurretIOSim extends TurretIO { TalonFXSimState motorSim; diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 7d2f52b..86c600a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -4,6 +4,8 @@ package frc.robot.subsystems.shooter; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; @@ -22,9 +24,6 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.signals.SensorDirectionValue; - /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { @@ -45,7 +44,7 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { private final CANcoderIOInputsAutoLogged cancoderOneInputs = new CANcoderIOInputsAutoLogged(); private final CANcoderIOInputsAutoLogged cancoderTwoInputs = new CANcoderIOInputsAutoLogged(); - //shouldnt this be private final + // shouldnt this be private final HoodIO hoodIO; HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); @@ -57,7 +56,12 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { private LinearFilter currentFilter = LinearFilter.movingAverage(10); - public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO, TurretIO turretIO, CANcoderIO cancoderOne, CANcoderIO cancoderTwo) { + public TurretSubsystem( + FlywheelIO flywheelIO, + HoodIO hoodIO, + TurretIO turretIO, + CANcoderIO cancoderOne, + CANcoderIO cancoderTwo) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; this.turretIO = turretIO; @@ -82,7 +86,7 @@ public void periodic() { currentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); } - public static CANcoderConfiguration getCancoderConfigs() { + public static CANcoderConfiguration getCancoderConfigs() { CANcoderConfiguration config = new CANcoderConfiguration(); config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; @@ -104,7 +108,8 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar .getDistance(feedTarget.get().getTranslation())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.getTargetFacingTurretPosition(robotPoseSupplier.get(), feedTarget.get())); + turretIO.setTurretPosition( + AutoAim.getTargetFacingTurretPosition(robotPoseSupplier.get(), feedTarget.get())); }); } @@ -114,7 +119,7 @@ public Command rest() { () -> { hoodIO.setHoodPosition(HOOD_MIN_ROTATION); // TODO: TUNE TUCKED POSITION IF NEEDED flywheelIO.setFlywheelVoltage(0.0); - turretIO.setTurretPosition(turretIO.TURRET_MIN_ROTATIONS); + turretIO.setTurretPosition(TurretIO.TURRET_MIN_ROTATIONS); }); } @@ -124,7 +129,7 @@ public Command spit() { () -> { hoodIO.setHoodPosition(HOOD_MIN_ROTATION); flywheelIO.setMotionProfiledFlywheelVelocity(20); - turretIO.setTurretPosition(turretIO.TURRET_MIN_ROTATIONS); + turretIO.setTurretPosition(TurretIO.TURRET_MIN_ROTATIONS); }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } @@ -135,23 +140,20 @@ public Rotation2d getAbsoluteTurretRotations() { // if can one is bigger than can 2 its simply can1-can2 // otherwise can1 + 1 - can2 because we want how much behind can1 it is - double diffRotations = - (cancoder1.getRotations() >= cancoder2.getRotations()) - ? cancoder1.getRotations() - cancoder2.getRotations() - : (cancoder1.getRotations() + 1) - cancoder2.getRotations(); - + double diffRotations = (cancoder1.getRotations() - cancoder2.getRotations()) % 1; // keeping track of how many total rots can1 is doing using the diff with can2 - //26/2 because gear difference of 2 + // 26/2 because gear difference of 2 double absoluteRotationsCan1 = diffRotations * (26.0 / 2.0); - // turret maxes out at less then 1 rotation which is like 11 can1 rotations anyways and it should work up to there + // turret maxes out at less then 1 rotation which is like 11 can1 rotations anyways and it + // should work up to there // multiply abs can1 rots by the gear ratio double turretRotations = absoluteRotationsCan1 * TurretIO.CANCODER_ONE_TO_TURRET_GEAR_RATIO; return Rotation2d.fromRotations(turretRotations); } - @AutoLogOutput(key = "Shooter/Turret/Turret Absolute Rotation") + @AutoLogOutput(key = "Shooter/Turret/Turret Absolute Rotation") public Rotation2d getTurretRotation() { return getAbsoluteTurretRotations(); } @@ -178,7 +180,7 @@ public boolean atTurretSetpoint() { getAbsoluteTurretRotations().getDegrees(), turretIO.getTurretSetpoint().getDegrees(), 1); } - @AutoLogOutput(key = "Shooter/Turret/Cancoder One") + @AutoLogOutput(key = "Shooter/Turret/Cancoder One") public Rotation2d getTurretCancoderOne() { return cancoderOneInputs.cancoderPositionRotations; } @@ -206,7 +208,7 @@ public Command testShoot() { () -> { hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); - turretIO.setTurretPosition(turretIO.TURRET_MIN_ROTATIONS); + turretIO.setTurretPosition(TurretIO.TURRET_MIN_ROTATIONS); }); } @@ -218,7 +220,9 @@ public Command shoot(Supplier robotPoseSupplier) { AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.getTargetFacingTurretPosition(robotPoseSupplier.get(), FieldUtils.getCurrentHubPose())); + turretIO.setTurretPosition( + AutoAim.getTargetFacingTurretPosition( + robotPoseSupplier.get(), FieldUtils.getCurrentHubPose())); }); } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 07b4547..4c68a2c 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -52,13 +52,13 @@ public static double distanceToHub(Pose2d pose) { return distance; } -public static Rotation2d getTargetFacingTurretPosition(Pose2d robotPose, Pose2d targetPose) { + public static Rotation2d getTargetFacingTurretPosition(Pose2d robotPose, Pose2d targetPose) { double dx = targetPose.getX() - robotPose.getX(); double dy = targetPose.getY() - robotPose.getY(); Rotation2d angleToTarget = new Rotation2d(dx, dy); - //feels right atm but maybe i should check this math tmrw + // feels right atm but maybe i should check this math tmrw Rotation2d turretAngle = angleToTarget.minus(robotPose.getRotation()); return turretAngle; } From fbcf671b3c993296ca8cd2066afdbc9467096446 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Tue, 10 Feb 2026 18:42:07 -0800 Subject: [PATCH 11/17] fix and delete stuff --- src/main/java/frc/robot/subsystems/shooter/TurretIO.java | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index ab1afa3..fb38ea2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -49,10 +49,7 @@ public static class TurretIOInputs { private final StatusSignal voltage = motor.getMotorVoltage(); private final StatusSignal tempCelcius = motor.getDeviceTemp(); - private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); - private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); - private MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0.0).withEnableFOC(true); + private MotionMagicVoltage motionMagic = new MotionMagicVoltage(0.0); // todo private Rotation2d turretSetpoint = Rotation2d.kZero; @@ -107,7 +104,7 @@ public void updateInputs(TurretIOInputs inputs) { public void setTurretPosition(Rotation2d positionAngle) { turretSetpoint = positionAngle; motor.setControl( - motionMagicVoltage.withPosition( + motionMagic.withPosition( MathUtil.clamp( positionAngle.getRotations(), TURRET_MIN_ROTATIONS.getRotations(), From 02a7f5dc013d3daea12897f99ff7a4acc6d154b1 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Tue, 10 Feb 2026 18:56:36 -0800 Subject: [PATCH 12/17] idk --- src/main/java/frc/robot/subsystems/shooter/TurretIO.java | 3 --- .../java/frc/robot/subsystems/shooter/TurretSubsystem.java | 1 - 2 files changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index fb38ea2..2a9f355 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -4,9 +4,6 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 86c600a..76f0b6c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -47,7 +47,6 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { // shouldnt this be private final HoodIO hoodIO; HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); - FlywheelIO flywheelIO; FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); From 5be9bc700f912145bc58533e0ed6f52e56b5e96f Mon Sep 17 00:00:00 2001 From: vivi-o Date: Tue, 10 Feb 2026 19:14:37 -0800 Subject: [PATCH 13/17] debug --- src/main/java/frc/robot/Robot.java | 9 +---- .../subsystems/shooter/TurretSubsystem.java | 37 +++++++++++++------ 2 files changed, 27 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 76f494f..fce5858 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,12 +28,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -<<<<<<< HEAD +import frc.robot.Superstructure.SuperState; import frc.robot.components.cancoder.CANcoderIOReal; import frc.robot.components.cancoder.CANcoderIOSim; -======= -import frc.robot.Superstructure.SuperState; ->>>>>>> main import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; import frc.robot.subsystems.climber.ClimberIO; @@ -333,7 +330,6 @@ public Robot() { ROBOT_MODE == RobotMode.REAL ? new HoodIO(HoodIO.getCompHood(), canivore, 11) : new HoodIOSim( -<<<<<<< HEAD canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11), ROBOT_MODE == RobotMode.REAL // TODO id's @@ -346,9 +342,6 @@ public Robot() { ? new CANcoderIOReal(30, TurretSubsystem.getCancoderConfigs(), canivore) : new CANcoderIOSim()); // TODO climber -======= - canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11)); ->>>>>>> main break; } climber = diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index ee223df..47a62ef 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; -import frc.robot.utils.FieldUtils; import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; @@ -211,16 +210,32 @@ public Command testShoot() { }); } + @Override public Command score(Supplier shotDataSupplier) { - return this.run( - () -> { - ShotData shotData = - AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); - hoodIO.setHoodPosition(shotData.hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTargetFacingTurretPosition( - robotPoseSupplier.get(), FieldUtils.getCurrentHubPose())); - }); + throw new UnsupportedOperationException("Unimplemented method 'score'"); + /* return this.run( + + () -> { + ShotData shotData = + AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); + hoodIO.setHoodPosition(shotData.hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + //turretIO.setTurretPosition( + // AutoAim.getTargetFacingTurretPosition( + // robotPoseSupplier.get(), FieldUtils.getCurrentHubPose())); + }); + */ + } + + @Override + public Rotation2d getHoodSetpoint() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getHoodSetpoint'"); + } + + @Override + public boolean isFacingTarget() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isFacingTarget'"); } } From 64a033dc69b4fc2de6f05222489d3b8c389733c4 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Tue, 10 Feb 2026 21:16:00 -0800 Subject: [PATCH 14/17] use ctre sim to make cancoder sim --- src/main/java/frc/robot/Robot.java | 9 ++--- .../robot/components/cancoder/CANcoderIO.java | 32 +++++++++++++++-- .../components/cancoder/CANcoderIOReal.java | 36 ------------------- .../components/cancoder/CANcoderIOSim.java | 25 ++++++++++--- 4 files changed, 55 insertions(+), 47 deletions(-) delete mode 100644 src/main/java/frc/robot/components/cancoder/CANcoderIOReal.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fce5858..329300e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Superstructure.SuperState; +import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOReal; import frc.robot.components.cancoder.CANcoderIOSim; import frc.robot.components.rollers.RollerIO; @@ -336,11 +337,11 @@ public Robot() { ? new TurretIO() : new TurretIOSim(), ROBOT_MODE == RobotMode.REAL - ? new CANcoderIOReal(30, TurretSubsystem.getCancoderConfigs(), canivore) - : new CANcoderIOSim(), + ? new CANcoderIO(30, TurretSubsystem.getCancoderConfigs(), canivore) + : new CANcoderIOSim(30, TurretSubsystem.getCancoderConfigs(), canivore), ROBOT_MODE == RobotMode.REAL - ? new CANcoderIOReal(30, TurretSubsystem.getCancoderConfigs(), canivore) - : new CANcoderIOSim()); + ? new CANcoderIO(30, TurretSubsystem.getCancoderConfigs(), canivore) + : new CANcoderIOSim(30, TurretSubsystem.getCancoderConfigs(), canivore)); // TODO climber break; } diff --git a/src/main/java/frc/robot/components/cancoder/CANcoderIO.java b/src/main/java/frc/robot/components/cancoder/CANcoderIO.java index d3aa988..a553761 100644 --- a/src/main/java/frc/robot/components/cancoder/CANcoderIO.java +++ b/src/main/java/frc/robot/components/cancoder/CANcoderIO.java @@ -5,16 +5,42 @@ package frc.robot.components.cancoder; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; + import org.littletonrobotics.junction.AutoLog; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; + // for cancoders that aren't on a swerve module (eg arm, intake) -public interface CANcoderIO { - // TODO wherever you use this, create an alert for connected or not +public class CANcoderIO { + @AutoLog public static class CANcoderIOInputs { public boolean connected = false; public Rotation2d cancoderPositionRotations = new Rotation2d(); } - public void updateInputs(CANcoderIOInputs inputs); + protected final CANcoder cancoder; + + private final StatusSignal cancoderAbsolutePositionRotations; + + public CANcoderIO(int cancoderID, CANcoderConfiguration config, CANBus canbus) { + cancoder = new CANcoder(cancoderID, canbus); + cancoderAbsolutePositionRotations = cancoder.getAbsolutePosition(); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, cancoderAbsolutePositionRotations); + cancoder.getConfigurator().apply(config); + cancoder.optimizeBusUtilization(); + } + + public void updateInputs(CANcoderIOInputs inputs) { + BaseStatusSignal.refreshAll(cancoderAbsolutePositionRotations); + + inputs.connected = BaseStatusSignal.isAllGood(cancoderAbsolutePositionRotations); + inputs.cancoderPositionRotations = + Rotation2d.fromRotations(cancoderAbsolutePositionRotations.getValueAsDouble()); + } } diff --git a/src/main/java/frc/robot/components/cancoder/CANcoderIOReal.java b/src/main/java/frc/robot/components/cancoder/CANcoderIOReal.java deleted file mode 100644 index 43f2667..0000000 --- a/src/main/java/frc/robot/components/cancoder/CANcoderIOReal.java +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.components.cancoder; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.Angle; - -public class CANcoderIOReal implements CANcoderIO { - private final CANcoder cancoder; - - private final StatusSignal cancoderAbsolutePositionRotations; - - public CANcoderIOReal(int cancoderID, CANcoderConfiguration config, CANBus canbus) { - cancoder = new CANcoder(cancoderID, canbus); - cancoderAbsolutePositionRotations = cancoder.getAbsolutePosition(); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, cancoderAbsolutePositionRotations); - cancoder.getConfigurator().apply(config); - cancoder.optimizeBusUtilization(); - } - - @Override - public void updateInputs(CANcoderIOInputs inputs) { - BaseStatusSignal.refreshAll(cancoderAbsolutePositionRotations); - - inputs.connected = BaseStatusSignal.isAllGood(cancoderAbsolutePositionRotations); - inputs.cancoderPositionRotations = - Rotation2d.fromRotations(cancoderAbsolutePositionRotations.getValueAsDouble()); - } -} diff --git a/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java b/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java index 32e080b..aca654e 100644 --- a/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java +++ b/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java @@ -1,11 +1,28 @@ package frc.robot.components.cancoder; -import edu.wpi.first.wpilibj2.command.Commands; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.sim.CANcoderSimState; +import edu.wpi.first.math.geometry.Rotation2d; -public class CANcoderIOSim implements CANcoderIO { - public CANcoderIOSim() {} +public class CANcoderIOSim extends CANcoderIO { + private final CANcoderSimState sim; + + private double positionRotations; + + public CANcoderIOSim(int cancoderID, CANcoderConfiguration config, CANBus canbus) { + super(cancoderID, config, canbus); + this.sim = cancoder.getSimState(); + } + + public void setSimValues(double positionRotations) { + this.positionRotations = positionRotations; + } public void updateInputs(CANcoderIOInputs inputs) { - Commands.none(); + sim.setRawPosition(positionRotations); + + inputs.cancoderPositionRotations = Rotation2d.fromRotations(positionRotations); } } From c79b655277fbb868796cb994d07f653672eea35e Mon Sep 17 00:00:00 2001 From: vivi-o Date: Wed, 11 Feb 2026 18:20:42 -0800 Subject: [PATCH 15/17] fixes and naming --- src/main/java/frc/robot/Robot.java | 9 +-- .../robot/components/cancoder/CANcoderIO.java | 8 +- .../components/cancoder/CANcoderIOSim.java | 1 - .../frc/robot/subsystems/shooter/Shooter.java | 3 +- .../robot/subsystems/shooter/TurretIO.java | 2 + .../subsystems/shooter/TurretSubsystem.java | 81 ++++++++++--------- .../java/frc/robot/utils/autoaim/AutoAim.java | 16 ++-- 7 files changed, 60 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 329300e..26bbad6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,7 +30,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Superstructure.SuperState; import frc.robot.components.cancoder.CANcoderIO; -import frc.robot.components.cancoder.CANcoderIOReal; import frc.robot.components.cancoder.CANcoderIOSim; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; @@ -337,11 +336,11 @@ public Robot() { ? new TurretIO() : new TurretIOSim(), ROBOT_MODE == RobotMode.REAL - ? new CANcoderIO(30, TurretSubsystem.getCancoderConfigs(), canivore) - : new CANcoderIOSim(30, TurretSubsystem.getCancoderConfigs(), canivore), + ? new CANcoderIO(30, TurretSubsystem.getCancoder24tConfigs(), canivore) + : new CANcoderIOSim(30, TurretSubsystem.getCancoder24tConfigs(), canivore), ROBOT_MODE == RobotMode.REAL - ? new CANcoderIO(30, TurretSubsystem.getCancoderConfigs(), canivore) - : new CANcoderIOSim(30, TurretSubsystem.getCancoderConfigs(), canivore)); + ? new CANcoderIO(30, TurretSubsystem.getCancoder26tConfigs(), canivore) + : new CANcoderIOSim(30, TurretSubsystem.getCancoder26tConfigs(), canivore)); // TODO climber break; } diff --git a/src/main/java/frc/robot/components/cancoder/CANcoderIO.java b/src/main/java/frc/robot/components/cancoder/CANcoderIO.java index a553761..5d3967e 100644 --- a/src/main/java/frc/robot/components/cancoder/CANcoderIO.java +++ b/src/main/java/frc/robot/components/cancoder/CANcoderIO.java @@ -4,16 +4,14 @@ package frc.robot.components.cancoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.Angle; - -import org.littletonrobotics.junction.AutoLog; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import org.littletonrobotics.junction.AutoLog; // for cancoders that aren't on a swerve module (eg arm, intake) public class CANcoderIO { diff --git a/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java b/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java index aca654e..20943ca 100644 --- a/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java +++ b/src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java @@ -2,7 +2,6 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.sim.CANcoderSimState; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 73f83f6..ea02888 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -18,7 +18,8 @@ public interface Shooter extends Subsystem { * Sets hood angle and flywheel velocity based on distance from hub from the shot map + current * pose */ - public Command score(Supplier shotDataSupplier); + //can i just get rid of this because its not the same in the shooter and turret + //public Command score(Supplier shotDataSupplier); /** * Sets hood angle and flywheel velocity based on distance from hub from the feed map + current diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index 2a9f355..0d5f589 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -46,6 +47,7 @@ public static class TurretIOInputs { private final StatusSignal voltage = motor.getMotorVoltage(); private final StatusSignal tempCelcius = motor.getDeviceTemp(); + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private MotionMagicVoltage motionMagic = new MotionMagicVoltage(0.0); // todo diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 47a62ef..ba758b9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -14,8 +14,11 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Superstructure; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; +import frc.robot.utils.FieldUtils; +import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; @@ -38,19 +41,18 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; double currentFilterValue = 0.0; - private final CANcoderIO cancoderOne; - private final CANcoderIO cancoderTwo; - private final CANcoderIOInputsAutoLogged cancoderOneInputs = new CANcoderIOInputsAutoLogged(); - private final CANcoderIOInputsAutoLogged cancoderTwoInputs = new CANcoderIOInputsAutoLogged(); + private CANcoderIO cancoder24t; + private CANcoderIO cancoder26t; + private CANcoderIOInputsAutoLogged cancoder24tInputs = new CANcoderIOInputsAutoLogged(); + private CANcoderIOInputsAutoLogged cancoder26tInputs = new CANcoderIOInputsAutoLogged(); - // shouldnt this be private final - HoodIO hoodIO; - HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); - FlywheelIO flywheelIO; - FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + private HoodIO hoodIO; + private HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); + private FlywheelIO flywheelIO; + private FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); - TurretIO turretIO; - TurretIOInputsAutoLogged turretInputs = new TurretIOInputsAutoLogged(); + private TurretIO turretIO; + private TurretIOInputsAutoLogged turretInputs = new TurretIOInputsAutoLogged(); private LinearFilter currentFilter = LinearFilter.movingAverage(10); @@ -58,13 +60,13 @@ public TurretSubsystem( FlywheelIO flywheelIO, HoodIO hoodIO, TurretIO turretIO, - CANcoderIO cancoderOne, - CANcoderIO cancoderTwo) { + CANcoderIO cancoder24t, + CANcoderIO cancoder26t) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; this.turretIO = turretIO; - this.cancoderOne = cancoderOne; - this.cancoderTwo = cancoderTwo; + this.cancoder24t = cancoder24t; + this.cancoder26t = cancoder26t; } private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); @@ -78,13 +80,24 @@ public void periodic() { Logger.processInputs("Shooter/Hood", hoodInputs); turretIO.updateInputs(turretInputs); Logger.processInputs("Shooter/Turret", turretInputs); - cancoderOne.updateInputs(cancoderOneInputs); - cancoderTwo.updateInputs(cancoderTwoInputs); - + cancoder24t.updateInputs(cancoder24tInputs); + Logger.processInputs("Shooter/Turret Cancoder24t", cancoder24tInputs); + cancoder26t.updateInputs(cancoder26tInputs); + Logger.processInputs("Shooter/Turret Cancoder26t", cancoder26tInputs); currentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); } - public static CANcoderConfiguration getCancoderConfigs() { + public static CANcoderConfiguration getCancoder24tConfigs() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.0; + + return config; + } + + public static CANcoderConfiguration getCancoder26tConfigs() { CANcoderConfiguration config = new CANcoderConfiguration(); config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; @@ -106,8 +119,7 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar .getDistance(feedTarget.get().getTranslation())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTargetFacingTurretPosition(robotPoseSupplier.get(), feedTarget.get())); + turretIO.setTurretPosition(AutoAim.getTurretTargetRotation(FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose().getTranslation(), robotPoseSupplier.get())); }); } @@ -133,8 +145,8 @@ public Command spit() { public Rotation2d getAbsoluteTurretRotations() { // give valaues between 0 and 1 - Rotation2d cancoder1 = cancoderOneInputs.cancoderPositionRotations; - Rotation2d cancoder2 = cancoderTwoInputs.cancoderPositionRotations; + Rotation2d cancoder1 = cancoder24tInputs.cancoderPositionRotations; + Rotation2d cancoder2 = cancoder26tInputs.cancoderPositionRotations; // if can one is bigger than can 2 its simply can1-can2 // otherwise can1 + 1 - can2 because we want how much behind can1 it is @@ -178,14 +190,14 @@ public boolean atTurretSetpoint() { getAbsoluteTurretRotations().getDegrees(), turretIO.getTurretSetpoint().getDegrees(), 1); } - @AutoLogOutput(key = "Shooter/Turret/Cancoder One") - public Rotation2d getTurretCancoderOne() { - return cancoderOneInputs.cancoderPositionRotations; + @AutoLogOutput(key = "Shooter/Turret/Cancoder 24t position") + public Rotation2d getTurretCancoder24tPosition() { + return cancoder24tInputs.cancoderPositionRotations; } - @AutoLogOutput(key = "Shooter/Turret/Cancoder Two") - public Rotation2d getTurretCancoderTwo() { - return cancoderOneInputs.cancoderPositionRotations; + @AutoLogOutput(key = "Shooter/Turret/Cancoder 26t position") + public Rotation2d getTurretCancoder26tPosition() { + return cancoder26tInputs.cancoderPositionRotations; } @Override @@ -210,21 +222,16 @@ public Command testShoot() { }); } - @Override - public Command score(Supplier shotDataSupplier) { - throw new UnsupportedOperationException("Unimplemented method 'score'"); - /* return this.run( + public Command score(Supplier robotPoseSupplier, Supplier shotDataSupplier) { + return this.run( () -> { ShotData shotData = AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - //turretIO.setTurretPosition( - // AutoAim.getTargetFacingTurretPosition( - // robotPoseSupplier.get(), FieldUtils.getCurrentHubPose())); + turretIO.setTurretPosition(AutoAim.getTurretTargetRotation(FieldUtils.getCurrentHubTranslation(), robotPoseSupplier.get())); }); - */ } @Override diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 628753d..0d5bcfb 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -89,6 +89,11 @@ public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPos return rot; } + public static Rotation2d getTurretTargetRotation(Translation2d target, Pose2d robotPose) { + Rotation2d rot = getTargetRotation(target, robotPose).minus(robotPose.getRotation()); + return rot; + } + public static Rotation2d getVirtualHubYaw(ChassisSpeeds fieldRelativeSpeeds, Pose2d robotPose) { return getVirtualTargetYaw( FieldUtils.getCurrentHubTranslation(), fieldRelativeSpeeds, robotPose); @@ -125,15 +130,4 @@ public static ShotData getCompensatedSOTMShotData( ))); return getSOTMShotData(compensatedPose, targetTranslation, fieldRelativeSpeeds); } - - public static Rotation2d getTargetFacingTurretPosition(Pose2d robotPose, Pose2d targetPose) { - double dx = targetPose.getX() - robotPose.getX(); - double dy = targetPose.getY() - robotPose.getY(); - - Rotation2d angleToTarget = new Rotation2d(dx, dy); - - // feels right atm but maybe i should check this math tmrw - Rotation2d turretAngle = angleToTarget.minus(robotPose.getRotation()); - return turretAngle; - } } From 3d9e313731431556fd577c545a43405d3d484e24 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Wed, 11 Feb 2026 18:36:22 -0800 Subject: [PATCH 16/17] fixes --- src/main/java/frc/robot/Superstructure.java | 4 ++ .../frc/robot/subsystems/shooter/Shooter.java | 3 +- .../subsystems/shooter/ShooterSubsystem.java | 2 +- .../subsystems/shooter/TurretSubsystem.java | 54 ++++++++++--------- 4 files changed, 36 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index be47ad1..bf59b56 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -284,6 +284,7 @@ private void addCommands() { intake.rest(), indexer.rest(), shooter.score( + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), () -> AutoAim.getCompensatedSOTMShotData( swerve.getPose(), @@ -296,6 +297,7 @@ private void addCommands() { intake.rest(), indexer.kick(), shooter.score( + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), () -> AutoAim.getCompensatedSOTMShotData( swerve.getPose(), @@ -308,6 +310,7 @@ private void addCommands() { intake.rest(), indexer.kick(), shooter.score( + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), () -> AutoAim.getCompensatedSOTMShotData( swerve.getPose(), @@ -319,6 +322,7 @@ private void addCommands() { intake.intake(), indexer.kick(), shooter.score( + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), () -> AutoAim.getCompensatedSOTMShotData( swerve.getPose(), diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ea02888..7c42e06 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -18,8 +18,7 @@ public interface Shooter extends Subsystem { * Sets hood angle and flywheel velocity based on distance from hub from the shot map + current * pose */ - //can i just get rid of this because its not the same in the shooter and turret - //public Command score(Supplier shotDataSupplier); + public Command score(Supplier robotPoseSupplier, Supplier shotDataSupplier); /** * Sets hood angle and flywheel velocity based on distance from hub from the feed map + current diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 864f880..3b45694 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -86,7 +86,7 @@ public Command testShoot() { }); } - public Command score(Supplier shotDataSupplier) { + public Command score(Supplier robotPoseSupplier, Supplier shotDataSupplier) { return this.run( () -> { hoodSetpoint = shotDataSupplier.get().hoodAngle(); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index ba758b9..466d40b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -119,7 +119,12 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar .getDistance(feedTarget.get().getTranslation())); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.getTurretTargetRotation(FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose().getTranslation(), robotPoseSupplier.get())); + turretIO.setTurretPosition( + AutoAim.getTurretTargetRotation( + FeedTargets.getFeedTarget(Superstructure.getFeedTarget()) + .getPose() + .getTranslation(), + robotPoseSupplier.get())); }); } @@ -181,13 +186,25 @@ public boolean atFlywheelVelocitySetpoint() { @AutoLogOutput(key = "Shooter/Hood/At Setpoint") public boolean atHoodSetpoint() { return MathUtil.isNear( - hoodInputs.hoodPositionRotations.getDegrees(), hoodIO.getHoodSetpoint().getDegrees(), 1); + hoodInputs.hoodPositionRotations.getDegrees(), getHoodSetpoint().getDegrees(), 1); } + @Override @AutoLogOutput(key = "Shooter/Turret/At Setpoint") - public boolean atTurretSetpoint() { + public boolean isFacingTarget() { return MathUtil.isNear( - getAbsoluteTurretRotations().getDegrees(), turretIO.getTurretSetpoint().getDegrees(), 1); + getAbsoluteTurretRotations().getDegrees(), getTurretSetpoint().getDegrees(), 2); + } + + @Override + @AutoLogOutput(key = "Shooter/Hood/Setpoint") + public Rotation2d getHoodSetpoint() { + return hoodIO.getHoodSetpoint(); + } + + @AutoLogOutput(key = "Shooter/Turret/Setpoint") + public Rotation2d getTurretSetpoint() { + return turretIO.getTurretSetpoint(); } @AutoLogOutput(key = "Shooter/Turret/Cancoder 24t position") @@ -224,25 +241,14 @@ public Command testShoot() { public Command score(Supplier robotPoseSupplier, Supplier shotDataSupplier) { return this.run( - - () -> { - ShotData shotData = - AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); - hoodIO.setHoodPosition(shotData.hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.getTurretTargetRotation(FieldUtils.getCurrentHubTranslation(), robotPoseSupplier.get())); - }); - } - - @Override - public Rotation2d getHoodSetpoint() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getHoodSetpoint'"); - } - - @Override - public boolean isFacingTarget() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'isFacingTarget'"); + () -> { + ShotData shotData = + AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); + hoodIO.setHoodPosition(shotData.hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + turretIO.setTurretPosition( + AutoAim.getTurretTargetRotation( + FieldUtils.getCurrentHubTranslation(), robotPoseSupplier.get())); + }); } } From 2662d57323999c04dffd3ed2c9c9f8f2ba2982ac Mon Sep 17 00:00:00 2001 From: vivi-o Date: Wed, 11 Feb 2026 20:19:12 -0800 Subject: [PATCH 17/17] note --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 0d5bcfb..c5a8b21 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -89,6 +89,7 @@ public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPos return rot; } + //this should also adjust for like the turret offset from the robot rotation but like I don't know what that is public static Rotation2d getTurretTargetRotation(Translation2d target, Pose2d robotPose) { Rotation2d rot = getTargetRotation(target, robotPose).minus(robotPose.getRotation()); return rot;