From fb84e0581f1256cf9313d61a7954c3a7bf25544e Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Thu, 5 Mar 2026 18:13:09 -0500 Subject: [PATCH 1/5] Done with all sim that is relevant --- .../frc/lib/W8/io/motor/MotorIOTalonFX.java | 20 +-- src/main/java/frc/robot/Constants.java | 151 ++++++++++++++---- src/main/java/frc/robot/Robot.java | 16 +- src/main/java/frc/robot/RobotContainer.java | 102 +++++++++--- .../java/frc/robot/subsystems/Climber.java | 31 ++-- .../java/frc/robot/subsystems/Intake.java | 34 ++-- .../java/frc/robot/subsystems/Shooter.java | 123 +++++++++----- .../frc/robot/subsystems/shooter/Shooter.java | 130 +++++++-------- 8 files changed, 404 insertions(+), 203 deletions(-) diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java index 83c744e..2bbd047 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java @@ -75,9 +75,9 @@ public record TalonFXFollower(Device.CAN id, MotorAlignmentValue opposesMain) {} // Preconfigured control objects reused for efficiency protected final CoastOut coastControl = new CoastOut(); protected final StaticBrake brakeControl = new StaticBrake(); - protected final VoltageOut voltageControl = new VoltageOut(0).withEnableFOC(true); + protected final VoltageOut voltageControl = new VoltageOut(0); protected final TorqueCurrentFOC currentControl = new TorqueCurrentFOC(0); - protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0).withEnableFOC(true); + protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0); protected final DynamicMotionMagicTorqueCurrentFOC positionControl = new DynamicMotionMagicTorqueCurrentFOC(0, 0, 0); protected final VelocityTorqueCurrentFOC velocityControl = new VelocityTorqueCurrentFOC(0); @@ -166,7 +166,7 @@ protected boolean isRunningPositionControl() { return (control instanceof PositionTorqueCurrentFOC) || (control instanceof PositionVoltage) || (control instanceof MotionMagicTorqueCurrentFOC) - || (control instanceof DynamicMotionMagicTorqueCurrentFOC) + || (control instanceof MotionMagicDutyCycle) || (control instanceof MotionMagicVoltage); } @@ -181,7 +181,7 @@ protected boolean isRunningVelocityControl() { var control = motor.getAppliedControl(); return (control instanceof VelocityTorqueCurrentFOC) || (control instanceof VelocityVoltage) - || (control instanceof MotionMagicVelocityTorqueCurrentFOC) + || (control instanceof MotionMagicVelocityDutyCycle) || (control instanceof MotionMagicVelocityVoltage); } @@ -196,8 +196,8 @@ protected boolean isRunningVelocityControl() { protected boolean isRunningMotionMagic() { var control = motor.getAppliedControl(); return (control instanceof MotionMagicTorqueCurrentFOC) - || (control instanceof DynamicMotionMagicTorqueCurrentFOC) - || (control instanceof MotionMagicVelocityTorqueCurrentFOC) + || (control instanceof MotionMagicDutyCycle) + || (control instanceof MotionMagicVelocityDutyCycle) || (control instanceof MotionMagicVoltage) || (control instanceof MotionMagicVelocityVoltage); } @@ -395,13 +395,7 @@ public void runPosition( Velocity maxJerk, PIDSlot slot) { this.goalPosition = position; - motor.setControl( - positionControl - .withPosition(position) - .withVelocity(cruiseVelocity) - .withAcceleration(acceleration) - .withJerk(maxJerk) - .withSlot(slot.getNum())); + motor.setControl(positionControl.withPosition(position).withSlot(slot.getNum())); } /** diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 99569da..f38ebaf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -154,6 +154,10 @@ public class Ports { public static final Device.CAN HopperRoller = new CAN(3, "rio"); public static final Device.CAN ClimberLinearMechanism = new CAN(4, "rio"); public static final Device.CAN ShooterRoller = new CAN(5, "rio"); + public static final Device.CAN IntakePivot = new CAN(6, "rio"); + public static final Device.CAN ShooterTower = new CAN(7, "rio"); + public static final Device.CAN ShooterFeeder = new CAN(8, "rio"); + public static final Device.CAN ShooterHood = new CAN(9, "rio"); } public class HopperConstants { @@ -258,10 +262,10 @@ public final class ShooterConstants { public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance WHEEL_RADIUS = Meters.of(0.5); - public static final double IDLE_SPEED_RPM = (1.0); - public static final double HUB_SPEED_RPM = (1.0); - public static final double TOWER_SPEED_RPM = (1.0); - public static final double DEFAULT_SPEED_RPM = (1.0); + public static final AngularVelocity IDLE_SPEED_RPM = RotationsPerSecond.of(1.0); + public static final AngularVelocity HUB_SPEED_RPM = RotationsPerSecond.of(1.0); + public static final AngularVelocity TOWER_SPEED_RPM = RotationsPerSecond.of(1.0); + public static final AngularVelocity DEFAULT_SPEED_RPM = RotationsPerSecond.of(1.0); public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0; // Hood Constants @@ -278,9 +282,58 @@ public final class ShooterConstants { new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); } - public class ShooterFlywheelConstants { + // Needs tuned + public class ShooterFeederConstants { + public static String NAME = "ShooterFeederFlywheel"; - public static String NAME = "ShooterFlywheel"; + public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); + public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); + + private static final double GEARING = (2.0 / 1.0); + + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(1.0); + + // Velocity PID + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(1000.0).withKI(0.0).withKD(0.0); + + public static TalonFXConfiguration getFXConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = SLOT0CONFIG; + + return config; + } + } + + // Needs tuned + public class ShooterTowerConstants { + public static String NAME = "ShooterTowerFlywheel"; public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); @@ -327,6 +380,54 @@ public static TalonFXConfiguration getFXConfig() { } } + public class ShooterFlywheelConstants { + public static String NAME = "ShooterFlywheel"; + + public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); + public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); + + private static final double GEARING = (5.0 / 1.0); + + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.2); + + // Velocity PID + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + + public static TalonFXConfiguration getFXConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = SLOT0CONFIG; + + return config; + } + } + public class ShooterRotaryConstants { public static String NAME = "ShooterRotary"; @@ -337,13 +438,13 @@ public class ShooterRotaryConstants { // CRUISE_VELOCITY.div(0.1).per(Units.Second); // public static final Velocity JERK = ACCELERATION.per(Second); - private static final double ROTOR_TO_SENSOR = (2.0 / 1.0); - private static final double SENSOR_TO_MECHANISM = (2.0 / 1.0); + private static final double ROTOR_TO_SENSOR = (1.0 / 1.0); + private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0); public static final Translation3d OFFSET = Translation3d.kZero; public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Rotations.of(45.0); + public static final Angle MAX_ANGLE = Degrees.of(45.0); public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance ARM_LENGTH = Meters.of(1.0); @@ -382,12 +483,12 @@ public class ShooterRotaryConstants { public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.CurrentLimits.SupplyCurrentLimitEnable = false; + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); config.CurrentLimits.SupplyCurrentLimit = 40.0; config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; config.CurrentLimits.SupplyCurrentLowerTime = 0.1; - config.CurrentLimits.StatorCurrentLimitEnable = false; + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); config.CurrentLimits.StatorCurrentLimit = 80.0; config.Voltage.PeakForwardVoltage = 12.0; @@ -405,8 +506,6 @@ public static TalonFXConfiguration getFXConfig() { config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR; config.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM; - config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0 = SLOT0CONFIG; return config; @@ -439,10 +538,10 @@ public class IntakeFlywheelConstants { public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); public static final Angle STARTING_ANGLE = Rotations.of(0.0); - public static final double PICKUP_SPEED = 0.0; + public static final double PICKUP_SPEED = 10.0; public static final Distance WHEEL_RADIUS = Meters.of(0.05); public static final Translation3d OFFSET = Translation3d.kZero; - public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.2); public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); public static final String MOTOR_NAME = "Intake Flywheel"; @@ -571,7 +670,10 @@ public static TalonFXConfiguration getFXConfig() { public class IntakePivotConstants { public static final String NAME = "Intake"; - public static final Angle PICKUP_ANGLE = Rotations.of(0.0); + public static final Angle MIN_ANGLE = Degrees.of(0.0); + public static final Angle MAX_ANGLE = Degrees.of(130.0); + + public static final Angle PICKUP_ANGLE = MAX_ANGLE; public static final Angle STOW_ANGLE = Rotations.of(0.0); public static final Angle TOLERANCE = Degrees.of(1.0); @@ -581,11 +683,8 @@ public class IntakePivotConstants { public static final Velocity JERK = RadiansPerSecondPerSecond.per(Second).of(0.1); - private static final double ROTOR_TO_SENSOR = (50.0 / 1.0); - private static final double SENSOR_TO_MECHANISM = 1.0; - - public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Degrees.of(130.0); + private static final double ROTOR_TO_SENSOR = (1.0 / 1.0); + private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0); public static final Angle STARTING_ANGLE = Radians.zero(); public static final Distance ARM_LENGTH = Foot.one(); @@ -594,12 +693,13 @@ public class IntakePivotConstants { new Translation3d(), ARM_LENGTH, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); - public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.25); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(1); // Positional PID public static final Slot0Configs SLOT_0_CONFIG = new Slot0Configs().withKP(100.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1); - // ^^^ CHANGE + + // ^^^ CHANGE public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -638,11 +738,6 @@ public static TalonFXConfiguration getFXConfig() { } } - public class FeederConstants { - public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(0.0); - public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(0.0); - } - public class ClimberConstants { public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); public static final Mass CARRIAGE_MASS = Kilograms.of(2.5); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e2e3ff8..112672b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,10 +21,10 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.lib.Rebuilt2026.FuelSim; @@ -199,9 +199,15 @@ public void simulationInit() { robotContainer.drive ::getChassisSpeeds); // Supplier of field-centric chassis speed - fuelSim.registerIntake(0.4, 0.5, -0.4, 0.4, - () -> robotContainer.intake.canIntake(), - () -> {robotContainer.intake.simBalls++;}); + fuelSim.registerIntake( + 0.4, + 0.5, + -0.4, + 0.4, + () -> robotContainer.intake.canIntake(), + () -> { + robotContainer.intake.simBalls++; + }); fuelSim.start(); // enables the simulation to run (updateSim must still be called periodically) @@ -215,7 +221,7 @@ public void simulationPeriodic() { fuelSim.updateSim(); Logger.recordOutput("Zero Pose", new Pose3d()); - + SmartDashboard.putNumber("Sim Balls", (double) robotContainer.intake.simBalls); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34a9873..bb0d245 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.AngleUnit; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -24,19 +25,26 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.W8.io.motor.*; +import frc.lib.W8.io.vision.VisionIO; import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; import frc.lib.W8.mechanisms.flywheel.*; +import frc.lib.W8.mechanisms.linear.LinearMechanism; +import frc.lib.W8.mechanisms.linear.LinearMechanismReal; +import frc.lib.W8.mechanisms.linear.LinearMechanismSim; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal; import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; +import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; import frc.robot.Constants.IntakePivotConstants; import frc.robot.Constants.Ports; +import frc.robot.Constants.ShooterFeederConstants; import frc.robot.Constants.ShooterFlywheelConstants; import frc.robot.Constants.ShooterRotaryConstants; +import frc.robot.Constants.ShooterTowerConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.BallCounter; @@ -51,6 +59,9 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; + +import static edu.wpi.first.units.Units.Degrees; + import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -66,6 +77,7 @@ public class RobotContainer { // Subsystems public final Drive drive; private final Hopper hopper; + private final Climber climber; private final Shooter shooter; public final Intake intake; private final BallCounter ballCounter; @@ -109,14 +121,19 @@ public RobotContainer() { Ports.ShooterRoller)), new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), - Ports.ShooterRoller)), + ShooterFeederConstants.NAME, + ShooterFeederConstants.getFXConfig(), + Ports.ShooterFeeder)), + new FlywheelMechanismReal( + new MotorIOTalonFX( + ShooterTowerConstants.NAME, + ShooterTowerConstants.getFXConfig(), + Ports.ShooterTower)), new RotaryMechanismReal( new MotorIOTalonFX( ShooterRotaryConstants.NAME, ShooterRotaryConstants.getFXConfig(), - Ports.ShooterRoller), + Ports.ShooterHood), Constants.ShooterRotaryConstants.CONSTANTS, java.util.Optional.empty())); @@ -141,6 +158,15 @@ public RobotContainer() { VisionConstants.robotToCamera0, VisionConstants.aprilTagLayout, PoseStrategy.CONSTRAINED_SOLVEPNP)); + climber = + new Climber( + new LinearMechanismReal( + new MotorIOTalonFX( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberLinearMechanism), + ClimberConstants.CHARACTERISTICS)); + break; case SIM: @@ -175,20 +201,28 @@ public RobotContainer() { ShooterFlywheelConstants.TOLERANCE), new FlywheelMechanismSim( new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), - Ports.ShooterRoller), - ShooterFlywheelConstants.DCMOTOR, - ShooterFlywheelConstants.MOI, - ShooterFlywheelConstants.TOLERANCE), + ShooterFeederConstants.NAME, + ShooterFeederConstants.getFXConfig(), + Ports.ShooterFeeder), + ShooterFeederConstants.DCMOTOR, + ShooterFeederConstants.MOI, + ShooterFeederConstants.TOLERANCE), + new FlywheelMechanismSim( + new MotorIOTalonFXSim( + ShooterTowerConstants.NAME, + ShooterTowerConstants.getFXConfig(), + Ports.ShooterTower), + ShooterTowerConstants.DCMOTOR, + ShooterTowerConstants.MOI, + ShooterTowerConstants.TOLERANCE), new RotaryMechanismSim( new MotorIOTalonFXSim( ShooterRotaryConstants.NAME, ShooterRotaryConstants.getFXConfig(), - Ports.ShooterRoller), + Ports.ShooterHood), ShooterRotaryConstants.DCMOTOR, ShooterRotaryConstants.MOI, - true, + false, ShooterRotaryConstants.CONSTANTS, java.util.Optional.empty())); @@ -206,7 +240,7 @@ public RobotContainer() { new MotorIOTalonFXSim( IntakePivotConstants.NAME, IntakePivotConstants.getFXConfig(), - Ports.IntakeRoller), + Ports.IntakePivot), IntakePivotConstants.DCMOTOR, IntakePivotConstants.MOI, false, @@ -221,6 +255,18 @@ public RobotContainer() { VisionConstants.aprilTagLayout, PoseStrategy.CONSTRAINED_SOLVEPNP, VisionConstants.getSystemSim())); + + climber = + new Climber( + new LinearMechanismSim( + new MotorIOTalonFXSim( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberLinearMechanism), + ClimberConstants.DCMOTOR, + ClimberConstants.CARRIAGE_MASS, + ClimberConstants.CHARACTERISTICS, + false)); break; default: @@ -237,6 +283,7 @@ public RobotContainer() { shooter = new Shooter( + new FlywheelMechanism() {}, new FlywheelMechanism() {}, new FlywheelMechanism() {}, new RotaryMechanism(null, null) {}); @@ -245,7 +292,11 @@ public RobotContainer() { new Intake( new FlywheelMechanism() {}, new RotaryMechanism(IntakePivotConstants.NAME, IntakePivotConstants.CONSTANTS) {}); - vision = new Vision(null); + vision = new Vision(new VisionIO() {}); + climber = + new Climber( + new LinearMechanism( + ClimberConstants.MOTOR_NAME, ClimberConstants.CHARACTERISTICS) {}); break; } @@ -315,18 +366,19 @@ private void configureButtonBindings() { .x() .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - controller - .rightTrigger() - .onTrue(Commands.runOnce(() -> shooter.simShoot())); + // Commands for sim testing! + // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); + + // controller + // .leftTrigger() + // .onTrue( + // Commands.runOnce( + // () -> { + // Robot.fuelSim.clearFuel(); + // Robot.fuelSim.spawnStartingFuel(); + // intake.simBalls = 0; + // })); - controller - .leftTrigger() - .onTrue(Commands.runOnce(() -> { - Robot.fuelSim.clearFuel(); - Robot.fuelSim.spawnStartingFuel(); - intake.simBalls = 0; - })); - controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1))); controller.leftBumper().onTrue(intake.intake()); controller.rightBumper().onTrue(intake.stowAndStopRollers()); } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 4aade98..ebca328 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,13 +2,11 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; @@ -43,21 +41,6 @@ public boolean isAboveCurrentLimit() { } } - @Override - public void periodic() { - _io.periodic(); - - double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position - - // The z of the Translation3D should be - // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing - // motor configs. - Logger.recordOutput( - "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); - - _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); - } - public Command runClimber() { return this.runOnce( () -> @@ -79,4 +62,18 @@ public boolean nearGoalposition() { return false; } } + + @Override + public void periodic() { + _io.periodic(); + + Logger.recordOutput( + "3DField/4_Climber", + new Pose3d( + new Translation3d( + 0, 0, ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)), + new Rotation3d(0, 0, 0))); + + // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); // For testing climber motion! + } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2442832..8abfe0e 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -16,6 +15,7 @@ import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; +import frc.lib.W8.util.LoggerHelper; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; import org.littletonrobotics.junction.Logger; @@ -42,16 +42,13 @@ public void setVelocity(double velocity) { _rollerIO.runVelocity(angVelo, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); } - public Command setPivotAngle(Angle pivotAngle) { - return this.runOnce( - () -> - _pivotIO.runPosition( + public void setPivotAngle(Angle pivotAngle) { + _pivotIO.runPosition( pivotAngle, IntakeFlywheelConstants.CRUISE_VELOCITY, IntakeFlywheelConstants.ACCELERATION, IntakeFlywheelConstants.JERK, - PIDSlot.SLOT_0)); - // .withName("Go To " + setpoint.toString() + " Setpoint"); + PIDSlot.SLOT_0); } public AngularVelocity getVelocity() { @@ -68,8 +65,9 @@ public void stop() { public Command intake() { return Commands.sequence( - Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), - setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); + Commands.runOnce(() -> this.setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)), + Commands.runOnce(() -> this.setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)) + ); } public boolean isIntendedAngle() { @@ -77,9 +75,10 @@ public boolean isIntendedAngle() { <= IntakePivotConstants.TOLERANCE.magnitude(); } - public boolean canIntake() - { - return _pivotIO.getPosition().in(Degree) > (IntakePivotConstants.MAX_ANGLE.in(Degree) - 10) && simBalls < 45 && simBalls >= 0; + public boolean canIntake() { + return _pivotIO.getPosition().in(Degree) > (IntakePivotConstants.MAX_ANGLE.in(Degree) - 10) + && simBalls < 45 + && simBalls >= 0; } public Command stowAndStopRollers() { @@ -101,9 +100,11 @@ private Command setStowAngle(Angle stowAngle) { @Override public void periodic() { - if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) _pivotIO.runVoltage(Volts.of(0.25)); + LoggerHelper.recordCurrentCommand("1_Intake", this); _pivotIO.periodic(); + _rollerIO.periodic(); + Logger.recordOutput( "3DField/1_Intake", new Pose3d( @@ -115,6 +116,11 @@ public void periodic() { new Translation3d(Math.sin(_pivotIO.getPosition().in(Radians) * 0.1055), 0, 0), new Rotation3d(0, 0, 0))); - //_pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot + // For testing purposes, opens hopper in sim + // if (_pivotIO.getPosition().in(Degrees) < IntakePivotConstants.MAX_ANGLE.in(Degrees) - 10) + // _pivotIO.runVoltage(Volts.of(7)); + + // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*5.0)); //--- Tests the pivot + // _rollerIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*5.0)); //--- Tests the pivot } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 3ad1ebe..d3aa7a5 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -3,27 +3,24 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; -import frc.robot.Constants.FeederConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.ShooterFeederConstants; +import frc.robot.Constants.ShooterTowerConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -31,22 +28,36 @@ public class Shooter extends SubsystemBase { private final FlywheelMechanism _flywheel; private final FlywheelMechanism _feeder; + private final FlywheelMechanism _tower; private final RotaryMechanism _hood; // desired target values private double desiredVelo; private double hoodAngle; - public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder, RotaryMechanism hood) { + public Shooter( + FlywheelMechanism flywheel, + FlywheelMechanism feeder, + FlywheelMechanism tower, + RotaryMechanism hood) { _flywheel = flywheel; _feeder = feeder; + _tower = tower; _hood = hood; } // Sets feeder motor speed public void runFeeder() { _feeder.runVelocity( - FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); + ShooterFeederConstants.MAX_VELOCITY, + ShooterFeederConstants.MAX_ACCELERATION, + PIDSlot.SLOT_2); + } + + // Sets tower motor speed + public void runTower() { + _feeder.runVelocity( + ShooterTowerConstants.MAX_VELOCITY, ShooterTowerConstants.MAX_ACCELERATION, PIDSlot.SLOT_2); } // Sets the flywheel velocity based on an input. @@ -58,20 +69,21 @@ public void setFlywheelVelocity(double velocity) { _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); } - public enum State { - OFF(Units.RevolutionsPerSecond.of(0.0)), - IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), - SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), - SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), - SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), - SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); + // // Broken aha !! + // public enum State { + // OFF(Units.RevolutionsPerSecond.of(0.0)), + // IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), + // SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), + // SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), + // SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), + // SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); - private final AngularVelocity stateVelocity; + // private final AngularVelocity stateVelocity; - State(AngularVelocity stateVelocity) { - this.stateVelocity = stateVelocity; - } - } + // State(AngularVelocity stateVelocity) { + // this.stateVelocity = stateVelocity; + // } + // } // Checks if the flywheel is at speed and returns a boolean public boolean flyAtVelocity() { @@ -126,36 +138,75 @@ public Command shoot(double velocity) { Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)), // feed once ready Commands.runOnce(() -> runFeeder()), + Commands.runOnce(() -> runTower()), // stop flywheel when finished Commands.runOnce(() -> setFlywheelVelocity(0))); } - public void simShoot() - { + public void simShoot() { if (Robot.robotContainer.intake.simBalls <= 0) return; - double flywheelSpeed = 6; Translation2d robotPose2d = Robot.robotContainer.drive.getPose().getTranslation(); + Pose3d robotPose3d = + new Pose3d( + new Translation3d(robotPose2d.getX(), robotPose2d.getY(), 0), + new Rotation3d(robotPose2d.getAngle())); + Pose3d shooterPose3d = + new Pose3d( + new Translation3d(-0.0075, 0.0, 0.523), + new Rotation3d(0, _hood.getPosition().in(Radians), 0)); + + double flywheelSpeed = _flywheel.getVelocity().magnitude(); + double Yaw = Robot.robotContainer.drive.getPose().getRotation().getRadians(); - Pose3d robotPose3d = new Pose3d(new Translation3d(robotPose2d.getX(),robotPose2d.getY(),0), new Rotation3d(robotPose2d.getAngle())); - Pose3d shooterPose3d = new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, _hood.getPosition().in(Radians), 0)); - - double V_xy = Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians)))*flywheelSpeed; + double V_xy = + Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) + * flywheelSpeed; + + // ChassisSpeeds driveChassisSpeeds = Robot.robotContainer.drive.getChassisSpeeds(); + // Translation3d driveSpeed3d = new Translation3d( + // 0.0, + // 0.0, + // 0.0 + // ); + + Robot.fuelSim.spawnFuel( + robotPose3d + .plus( + new Transform3d( + shooterPose3d.getX(), + shooterPose3d.getY(), + shooterPose3d.getZ(), + new Rotation3d(0, 0, 0))) + .getTranslation(), + new Translation3d( + V_xy * Math.cos(Yaw), + V_xy * Math.sin(Yaw), + Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) + * flywheelSpeed)); - Robot.fuelSim.spawnFuel(robotPose3d.plus(new Transform3d(shooterPose3d.getX(), shooterPose3d.getY(), shooterPose3d.getZ(), new Rotation3d(0, 0, 0))).getTranslation(), new Translation3d(V_xy*Math.cos(Yaw), V_xy*Math.sin(Yaw), Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) *flywheelSpeed)); Robot.robotContainer.intake.simBalls--; } public void periodic() { _hood.periodic(); - // _feeder.periodic(); - // _flywheel.periodic(); - - double pitch = Math.toRadians(Math.abs(Math.sin(Timer.getFPGATimestamp())*45)); // Placeholder for position - - // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing motor configs. - Logger.recordOutput("3DField/3_Hood", new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, pitch, 0))); - - _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); + _feeder.periodic(); + _tower.periodic(); + _flywheel.periodic(); + + Logger.recordOutput( + "3DField/3_Hood", + new Pose3d( + new Translation3d(-0.0075, 0.0, 0.523), + new Rotation3d(0, _hood.getPosition().in(Radians), 0))); + + // For testing purposes, raises the hood + // if (_hood.getPosition().in(Degrees) < ShooterRotaryConstants.MAX_ANGLE.in(Degrees) - 10) + // _hood.runVoltage(Volts.of(7)); + + // _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 5)); + // _feeder.runVoltage(Volts.of(5)); + // _tower.runVoltage(Volts.of(5)); + // _flywheel.runVoltage(Volts.of(5)); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fd86a44..7413bfa 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,79 +1,79 @@ -package frc.robot.subsystems.shooter; +// package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.RotationsPerSecond; +// import static edu.wpi.first.units.Units.RotationsPerSecond; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.W8.io.motor.MotorIO.PIDSlot; -import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; -import frc.robot.Constants.FeederConstants; -import frc.robot.Constants.ShooterConstants; +// import edu.wpi.first.units.Units; +// import edu.wpi.first.units.measure.AngularVelocity; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.Commands; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc.lib.W8.io.motor.MotorIO.PIDSlot; +// import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; +// import frc.robot.Constants.FeederConstants; +// import frc.robot.Constants.ShooterConstants; -public class Shooter extends SubsystemBase { +// public class Shooter extends SubsystemBase { - private FlywheelMechanism _flywheel; - private FlywheelMechanism _feeder; - public double desiredVelo; +// private FlywheelMechanism _flywheel; +// private FlywheelMechanism _feeder; +// public double desiredVelo; - public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder) { - _flywheel = flywheel; - _feeder = feeder; - } +// public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder) { +// _flywheel = flywheel; +// _feeder = feeder; +// } - // Sets feeder motor speed - public void runFeeder() { - _feeder.runVelocity( - FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); - } +// // Sets feeder motor speed +// public void runFeeder() { +// _feeder.runVelocity( +// FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); +// } - // Sets the flywheel velocity based on an input. - public void setFlywheelVelocity(double velocity) { - AngularVelocity angVelo = RotationsPerSecond.of(velocity); - velocity = desiredVelo; +// // Sets the flywheel velocity based on an input. +// public void setFlywheelVelocity(double velocity) { +// AngularVelocity angVelo = RotationsPerSecond.of(velocity); +// velocity = desiredVelo; - _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); - } +// _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); +// } - public enum State { - OFF(Units.RevolutionsPerSecond.of(0.0)), - IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), - SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), - SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), - SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), - SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); +// public enum State { +// OFF(Units.RevolutionsPerSecond.of(0.0)), +// IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), +// SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), +// SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), +// SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), +// SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); - private final AngularVelocity stateVelocity; +// private final AngularVelocity stateVelocity; - State(AngularVelocity stateVelocity) { - this.stateVelocity = stateVelocity; - } - } +// State(AngularVelocity stateVelocity) { +// this.stateVelocity = stateVelocity; +// } +// } - // Checks if the flywheel is at speed and returns a boolean - public boolean flyAtVelocity() { - return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond)) - <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; - } +// // Checks if the flywheel is at speed and returns a boolean +// public boolean flyAtVelocity() { +// return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond)) +// <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; +// } - public Command shoot(double velocity) { - return Commands.run( - () -> { - setFlywheelVelocity(velocity); - }) - .until(() -> flyAtVelocity()) - .andThen( - () -> { - runFeeder(); - }) - .andThen( - () -> { - setFlywheelVelocity(0); - }); - } +// public Command shoot(double velocity) { +// return Commands.run( +// () -> { +// setFlywheelVelocity(velocity); +// }) +// .until(() -> flyAtVelocity()) +// .andThen( +// () -> { +// runFeeder(); +// }) +// .andThen( +// () -> { +// setFlywheelVelocity(0); +// }); +// } - @Override - public void periodic() {} -} +// @Override +// public void periodic() {} +// } From 21f2c0ecec2baef707cb0249a24a587622275fe0 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Thu, 5 Mar 2026 18:51:29 -0500 Subject: [PATCH 2/5] please no merge conflicts i beg --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 112672b..cfd0191 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -40,7 +40,7 @@ * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or * the package after creating this project, you must also update the build.gradle file in the - * project. + * project. */ public class Robot extends LoggedRobot { private Command autonomousCommand; From ca20c0d2e4c6c0fb574d86b34c519fdba7c785d2 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Thu, 5 Mar 2026 19:22:39 -0500 Subject: [PATCH 3/5] great value unicorn sparkle ice cream --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +----- src/main/java/frc/robot/subsystems/Climber.java | 3 ++- src/main/java/frc/robot/subsystems/Intake.java | 13 ++++++------- src/main/java/frc/robot/subsystems/Shooter.java | 3 ++- 5 files changed, 12 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cfd0191..112672b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -40,7 +40,7 @@ * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or * the package after creating this project, you must also update the build.gradle file in the - * project. + * project. */ public class Robot extends LoggedRobot { private Command autonomousCommand; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b7f111e..26f864a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.AngleUnit; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -59,9 +58,6 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; - -import static edu.wpi.first.units.Units.Degrees; - import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -378,7 +374,7 @@ private void configureButtonBindings() { .x() .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - // Commands for sim testing! + // Commands for sim testing! // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); // controller diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index ebca328..7354a3a 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -74,6 +74,7 @@ public void periodic() { 0, 0, ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)), new Rotation3d(0, 0, 0))); - // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); // For testing climber motion! + // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); // For testing climber + // motion! } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 8abfe0e..1961f6c 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -44,11 +44,11 @@ public void setVelocity(double velocity) { public void setPivotAngle(Angle pivotAngle) { _pivotIO.runPosition( - pivotAngle, - IntakeFlywheelConstants.CRUISE_VELOCITY, - IntakeFlywheelConstants.ACCELERATION, - IntakeFlywheelConstants.JERK, - PIDSlot.SLOT_0); + pivotAngle, + IntakeFlywheelConstants.CRUISE_VELOCITY, + IntakeFlywheelConstants.ACCELERATION, + IntakeFlywheelConstants.JERK, + PIDSlot.SLOT_0); } public AngularVelocity getVelocity() { @@ -66,8 +66,7 @@ public void stop() { public Command intake() { return Commands.sequence( Commands.runOnce(() -> this.setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)), - Commands.runOnce(() -> this.setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)) - ); + Commands.runOnce(() -> this.setVelocity(IntakeFlywheelConstants.PICKUP_SPEED))); } public boolean isIntendedAngle() { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 8bd7025..0d8931a 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -165,7 +165,8 @@ public void simShoot() { new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, _hood.getPosition().in(Radians), 0)); - double flywheelSpeed = _lflywheel.getVelocity().magnitude() + _rflywheel.getVelocity().magnitude(); + double flywheelSpeed = + _lflywheel.getVelocity().magnitude() + _rflywheel.getVelocity().magnitude(); double Yaw = Robot.robotContainer.drive.getPose().getRotation().getRadians(); double V_xy = From cc233918bcff7884e6a5e46b560fb42ab482c279 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sat, 7 Mar 2026 15:55:52 -0500 Subject: [PATCH 4/5] sim done part 46^(12534221) --- src/main/java/frc/robot/Constants.java | 15 ++------ src/main/java/frc/robot/RobotContainer.java | 34 +++++++++---------- .../java/frc/robot/subsystems/Climber.java | 2 ++ .../java/frc/robot/subsystems/Intake.java | 3 ++ .../java/frc/robot/subsystems/Shooter.java | 26 +++++++++----- 5 files changed, 42 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 406dbf6..ca1ba8f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,16 +14,6 @@ package frc.robot; import static edu.wpi.first.units.Units.*; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.KilogramSquareMeters; -import static edu.wpi.first.units.Units.Kilograms; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CANdleConfiguration; @@ -356,7 +346,7 @@ public class ShooterTowerConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(1000.0).withKI(0.0).withKD(0.0); + new Slot0Configs().withKP(0.0).withKI(0.0).withKD(0.0); public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -390,7 +380,8 @@ public static TalonFXConfiguration getFXConfig() { } public class ShooterFlywheelConstants { - public static String NAME = "ShooterFlywheel"; + public static String NAME_L = "ShooterFlywheelLeft"; + public static String NAME_R = "ShooterFlywheelRight"; public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26f864a..4ed57d2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -112,12 +112,12 @@ public RobotContainer() { new Shooter( new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.NAME_L, ShooterFlywheelConstants.getFXConfig(), Ports.LeftFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.NAME_R, ShooterFlywheelConstants.getFXConfig(), Ports.RightFlywheel)), new FlywheelMechanismReal( @@ -192,7 +192,7 @@ public RobotContainer() { new Shooter( new FlywheelMechanismSim( new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.NAME_L, ShooterFlywheelConstants.getFXConfig(), Ports.LeftFlywheel), ShooterFlywheelConstants.DCMOTOR, @@ -200,7 +200,7 @@ public RobotContainer() { ShooterFlywheelConstants.TOLERANCE), new FlywheelMechanismSim( new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.NAME_R, ShooterFlywheelConstants.getFXConfig(), Ports.RightFlywheel), ShooterFlywheelConstants.DCMOTOR, @@ -375,21 +375,21 @@ private void configureButtonBindings() { .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); // Commands for sim testing! - // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); + controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); - // controller - // .leftTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // Robot.fuelSim.clearFuel(); - // Robot.fuelSim.spawnStartingFuel(); - // intake.simBalls = 0; - // })); + controller + .leftTrigger() + .onTrue( + Commands.runOnce( + () -> { + Robot.fuelSim.clearFuel(); + Robot.fuelSim.spawnStartingFuel(); + intake.simBalls = 0; + })); - controller.leftBumper().onTrue(intake.intake()); - controller.rightBumper().onTrue(intake.stowAndStopRollers()); - controller.a().onTrue(shooter.runFlywheel()); + // controller.leftBumper().onTrue(intake.intake()); + // controller.rightBumper().onTrue(intake.stowAndStopRollers()); + // controller.a().onTrue(shooter.runFlywheel()); } /** diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 7354a3a..e151de5 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,11 +2,13 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1961f6c..b4f3e73 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,14 +1,17 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Degree; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 0d8931a..73b00e0 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -3,14 +3,18 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,7 +24,9 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterFeederConstants; +import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.Constants.ShooterTowerConstants; +import frc.robot.generated.TunerConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -173,12 +179,12 @@ public void simShoot() { Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) * flywheelSpeed; - // ChassisSpeeds driveChassisSpeeds = Robot.robotContainer.drive.getChassisSpeeds(); - // Translation3d driveSpeed3d = new Translation3d( - // 0.0, - // 0.0, - // 0.0 - // ); + ChassisSpeeds driveChassisSpeeds = Robot.robotContainer.drive.getChassisSpeeds(); + Translation3d driveSpeed3d = new Translation3d( + 0.0, + 0.0, + 0.0 + ); Robot.fuelSim.spawnFuel( robotPose3d @@ -193,7 +199,8 @@ public void simShoot() { V_xy * Math.cos(Yaw), V_xy * Math.sin(Yaw), Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) - * flywheelSpeed)); + * flywheelSpeed) + .plus(driveSpeed3d)); Robot.robotContainer.intake.simBalls--; } @@ -213,11 +220,12 @@ public void periodic() { // For testing purposes, raises the hood // if (_hood.getPosition().in(Degrees) < ShooterRotaryConstants.MAX_ANGLE.in(Degrees) - 10) - // _hood.runVoltage(Volts.of(7)); + // _hood.runVelocity(RotationsPerSecond.of(1), RotationsPerSecondPerSecond.of(0.01), PIDSlot.SLOT_0); // _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 5)); // _feeder.runVoltage(Volts.of(5)); // _tower.runVoltage(Volts.of(5)); - // _flywheel.runVoltage(Volts.of(5)); + // _lflywheel.runVoltage(Volts.of(3)); + // _rflywheel.runVoltage(Volts.of(3)); } } From 819fa84587e2e8b0aaaf023846efd5e90d47a62e Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sat, 7 Mar 2026 15:57:06 -0500 Subject: [PATCH 5/5] i forgot spotless --- .../java/frc/robot/subsystems/Climber.java | 2 -- .../java/frc/robot/subsystems/Intake.java | 3 --- .../java/frc/robot/subsystems/Shooter.java | 26 +++++++------------ 3 files changed, 10 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index e151de5..7354a3a 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,13 +2,11 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index b4f3e73..1961f6c 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,17 +1,14 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Degree; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 73b00e0..eb85b4e 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -3,8 +3,6 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -14,7 +12,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -24,9 +21,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterFeederConstants; -import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.Constants.ShooterTowerConstants; -import frc.robot.generated.TunerConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -180,11 +175,7 @@ public void simShoot() { * flywheelSpeed; ChassisSpeeds driveChassisSpeeds = Robot.robotContainer.drive.getChassisSpeeds(); - Translation3d driveSpeed3d = new Translation3d( - 0.0, - 0.0, - 0.0 - ); + Translation3d driveSpeed3d = new Translation3d(0.0, 0.0, 0.0); Robot.fuelSim.spawnFuel( robotPose3d @@ -196,11 +187,13 @@ public void simShoot() { new Rotation3d(0, 0, 0))) .getTranslation(), new Translation3d( - V_xy * Math.cos(Yaw), - V_xy * Math.sin(Yaw), - Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) - * flywheelSpeed) - .plus(driveSpeed3d)); + V_xy * Math.cos(Yaw), + V_xy * Math.sin(Yaw), + Math.sin( + Math.PI / 2 + - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) + * flywheelSpeed) + .plus(driveSpeed3d)); Robot.robotContainer.intake.simBalls--; } @@ -220,7 +213,8 @@ public void periodic() { // For testing purposes, raises the hood // if (_hood.getPosition().in(Degrees) < ShooterRotaryConstants.MAX_ANGLE.in(Degrees) - 10) - // _hood.runVelocity(RotationsPerSecond.of(1), RotationsPerSecondPerSecond.of(0.01), PIDSlot.SLOT_0); + // _hood.runVelocity(RotationsPerSecond.of(1), RotationsPerSecondPerSecond.of(0.01), + // PIDSlot.SLOT_0); // _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 5)); // _feeder.runVoltage(Volts.of(5));