diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 83ddf0e..d82af9b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -579,7 +579,7 @@ public class IntakePivotConstants { public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(10); public static final AngularAcceleration ACCELERATION = RadiansPerSecondPerSecond.of(100); public static final Velocity JERK = - RadiansPerSecondPerSecond.per(Second).of(0); + 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; @@ -598,7 +598,9 @@ public class IntakePivotConstants { // Positional PID public static final Slot0Configs SLOT_0_CONFIG = - new Slot0Configs().withKP(10.0).withKI(2.0).withKD(8).withKS(0.07).withKV(0.1); + new Slot0Configs().withKP(100.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1); + + // ^^^ CHANGE public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b40f89d..112672b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,6 +24,7 @@ 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; @@ -43,9 +44,9 @@ */ public class Robot extends LoggedRobot { private Command autonomousCommand; - private RobotContainer robotContainer; + public static RobotContainer robotContainer; private Alert lowBatteryAlert = new Alert("The robot is low on battery!", AlertType.kWarning); - public FuelSim fuelSim = new FuelSim(); + public static FuelSim fuelSim = new FuelSim(); public Robot() { CanBridge.runTCP(); @@ -198,6 +199,16 @@ 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.start(); // enables the simulation to run (updateSim must still be called periodically) fuelSim.enableAirResistance(); // an additional drag force will be applied to fuel in physics @@ -210,5 +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 6d6cec9..330cbdd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -66,7 +66,7 @@ public class RobotContainer { public final Drive drive; private final Hopper hopper; private final Shooter shooter; - private final Intake intake; + public final Intake intake; private final BallCounter ballCounter; private final Vision vision; @@ -314,6 +314,17 @@ private void configureButtonBindings() { .x() .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); + controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); + + 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/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 672bf1b..414cebc 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,7 +1,9 @@ package frc.robot.subsystems; +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; @@ -21,11 +23,16 @@ public class Intake extends SubsystemBase { private FlywheelMechanism _rollerIO; private RotaryMechanism _pivotIO; + double velocity; + double pivotAngle; public double desiredAngle; + public int simBalls; public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { _rollerIO = rollerIO; _pivotIO = pivotIO; + + simBalls = 0; } // Velocity of Rollers @@ -70,6 +77,12 @@ 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 Command stowAndStopRollers() { return Commands.sequence( Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), @@ -89,6 +102,9 @@ 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)); + _pivotIO.periodic(); Logger.recordOutput( "3DField/1_Intake", @@ -101,6 +117,6 @@ 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 + // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5f89310..31ee0ca 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,11 +1,13 @@ package frc.robot.subsystems; 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.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.Units; @@ -21,6 +23,7 @@ import frc.robot.Constants.FeederConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Robot; import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { @@ -126,8 +129,47 @@ public Command shoot(double velocity) { Commands.runOnce(() -> setFlywheelVelocity(0))); } + public void simShoot() { + if (Robot.robotContainer.intake.simBalls <= 0) return; + + double flywheelSpeed = 6; + Translation2d robotPose2d = Robot.robotContainer.drive.getPose().getTranslation(); + 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; + + 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