Skip to content

Intake sim ready for usage #58

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
rhit-collinkn merged 2 commits into from
Feb 23, 2026
Merged

Intake sim ready for usage #58

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
423a467
The Shooter and Climber have improper motor configs, but intake works…
GhostR406 Feb 21, 2026
b8fa3af
fixing spotless issue maybe?
rhit-collinkn Feb 23, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -340,8 +340,8 @@ public class ShooterRotaryConstants {

public static final Translation3d OFFSET = Translation3d.kZero;

public static final Angle MIN_ANGLE = Degrees.of(-130.0);
public static final Angle MAX_ANGLE = Rotations.of(.5);
public static final Angle MIN_ANGLE = Degrees.of(0.0);
public static final Angle MAX_ANGLE = Rotations.of(45.0);
public static final Angle STARTING_ANGLE = Rotations.of(0.0);
public static final Distance ARM_LENGTH = Meters.of(1.0);

Expand Down Expand Up @@ -432,7 +432,7 @@ public static CANcoderConfiguration getCANcoderConfig(boolean sim) {

public static final int CANDLE_ID = 50;

public class IntakeConstants {
public class IntakeFlywheelConstants {
// Constants for Intake
public static final Angle MIN_ANGLE = Rotations.of(0.0);
public static final Angle MAX_ANGLE = Rotations.of(1);
Expand Down Expand Up @@ -578,8 +578,8 @@ public class IntakePivotConstants {
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(-90.0);
public static final Angle MAX_ANGLE = Degrees.of(90.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 STARTING_ANGLE = Radians.zero();
public static final Distance ARM_LENGTH = Foot.one();

Expand Down
35 changes: 29 additions & 6 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,17 @@

package frc.robot;

import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;

import au.grapplerobotics.CanBridge;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
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.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.lib.Rebuilt2026.FuelSim;
import frc.robot.generated.TunerConstants;
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.LoggedRobot;
Expand All @@ -32,13 +38,10 @@
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
import au.grapplerobotics.CanBridge;



public class Robot extends LoggedRobot {
private Command autonomousCommand;
private RobotContainer robotContainer;
public FuelSim fuelSim = new FuelSim();

public Robot() {
CanBridge.runTCP();
Expand Down Expand Up @@ -176,9 +179,29 @@ public void testPeriodic() {}

/** This function is called once when the robot is first started up. */
@Override
public void simulationInit() {}
public void simulationInit() {
fuelSim.spawnStartingFuel(); // spawns fuel in the depots and neutral zone

// Register a robot for collision with fuel
fuelSim.registerRobot(
0.8, // from left to right in meters
0.8, // from front to back in meters
Inches.of(7).in(Meters), // from floor to top of bumpers in meters
() -> robotContainer.drive.getPose(), // Supplier<Pose2d> of robot pose
robotContainer.drive
::getChassisSpeeds); // Supplier<ChassisSpeeds> of field-centric chassis speed

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
// update step
}

/** This function is called periodically whilst in simulation. */
@Override
public void simulationPeriodic() {}
public void simulationPeriodic() {
fuelSim.updateSim();

Logger.recordOutput("Zero Pose", new Pose3d());
}
}
71 changes: 31 additions & 40 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.lib.W8.io.absoluteencoder.AbsoluteEncoderIOCANCoderSim;
import frc.lib.W8.io.motor.*;
import frc.lib.W8.io.vision.VisionIOPhotonVision;
import frc.lib.W8.io.vision.VisionIOPhotonVisionSim;
Expand All @@ -36,8 +35,9 @@
import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim;
import frc.robot.Constants.ClimberConstants;
import frc.robot.Constants.HopperConstants;
import frc.robot.Constants.IntakeConstants;
import frc.robot.Constants.IntakeConstants.VisionConstants;
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.ShooterFlywheelConstants;
import frc.robot.Constants.ShooterRotaryConstants;
Expand All @@ -55,13 +55,10 @@
import frc.robot.subsystems.drive.ModuleIO;
import frc.robot.subsystems.drive.ModuleIOSim;
import frc.robot.subsystems.drive.ModuleIOTalonFX;

import java.util.Optional;

import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;


/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
Expand All @@ -71,7 +68,7 @@
public class RobotContainer {

// Subsystems
private final Drive drive;
public final Drive drive;
private final Hopper hopper;
private final Shooter shooter;
private final Climber climber;
Expand Down Expand Up @@ -122,8 +119,8 @@ public RobotContainer() {
Ports.ShooterRoller)),
new RotaryMechanismReal(
new MotorIOTalonFX(
ShooterFlywheelConstants.NAME,
ShooterFlywheelConstants.getFXConfig(),
ShooterRotaryConstants.NAME,
ShooterRotaryConstants.getFXConfig(),
Ports.ShooterRoller),
Constants.ShooterRotaryConstants.CONSTANTS,
java.util.Optional.empty()));
Expand All @@ -140,20 +137,16 @@ public RobotContainer() {
new Intake(
new FlywheelMechanismReal(
new MotorIOTalonFX(
IntakeConstants.MOTOR_NAME,
IntakeConstants.getFXConfig(),
IntakeFlywheelConstants.MOTOR_NAME,
IntakeFlywheelConstants.getFXConfig(),
Ports.IntakeRoller)),
new RotaryMechanismReal(
new MotorIOTalonFX(
IntakeConstants.MOTOR_NAME,
IntakeConstants.getFXConfig(),
IntakePivotConstants.NAME,
IntakePivotConstants.getFXConfig(),
Ports.IntakeRoller),
IntakeConstants.CONSTANTS,
Optional.of(
new AbsoluteEncoderIOCANCoderSim(
Ports.IntakeRoller,
IntakeConstants.MOTOR_NAME + " Encoder",
IntakeConstants.getCANcoderConfig(false)))));
IntakePivotConstants.CONSTANTS,
Optional.empty()));
vision =
new Vision(
new VisionIOPhotonVision(
Expand Down Expand Up @@ -203,8 +196,8 @@ public RobotContainer() {
ShooterFlywheelConstants.TOLERANCE),
new RotaryMechanismSim(
new MotorIOTalonFXSim(
ShooterFlywheelConstants.NAME,
ShooterFlywheelConstants.getFXConfig(),
ShooterRotaryConstants.NAME,
ShooterRotaryConstants.getFXConfig(),
Ports.ShooterRoller),
ShooterRotaryConstants.DCMOTOR,
ShooterRotaryConstants.MOI,
Expand All @@ -227,26 +220,22 @@ public RobotContainer() {
new Intake(
new FlywheelMechanismSim(
new MotorIOTalonFXSim(
IntakeConstants.MOTOR_NAME,
IntakeConstants.getFXConfig(),
IntakeFlywheelConstants.MOTOR_NAME,
IntakeFlywheelConstants.getFXConfig(),
Ports.IntakeRoller),
IntakeConstants.DCMOTOR,
IntakeConstants.MOI,
IntakeConstants.TOLERANCE),
IntakeFlywheelConstants.DCMOTOR,
IntakeFlywheelConstants.MOI,
IntakeFlywheelConstants.TOLERANCE),
new RotaryMechanismSim(
new MotorIOTalonFXSim(
IntakeConstants.MOTOR_NAME,
IntakeConstants.getFXConfig(),
IntakePivotConstants.NAME,
IntakePivotConstants.getFXConfig(),
Ports.IntakeRoller),
IntakeConstants.DCMOTOR,
IntakeConstants.MOI,
IntakePivotConstants.DCMOTOR,
IntakePivotConstants.MOI,
false,
IntakeConstants.CONSTANTS,
Optional.of(
new AbsoluteEncoderIOCANCoderSim(
Ports.IntakeRoller,
IntakeConstants.MOTOR_NAME + " Encoder",
IntakeConstants.getCANcoderConfig(false)))));
IntakePivotConstants.CONSTANTS,
Optional.empty()));
vision =
new Vision(
new VisionIOPhotonVisionSim(
Expand All @@ -259,7 +248,7 @@ public RobotContainer() {
break;

default:
//Replayed robot, disable IO implementations
// Replayed robot, disable IO implementations
drive =
new Drive(
new GyroIO() {},
Expand All @@ -283,7 +272,7 @@ public RobotContainer() {
intake =
new Intake(
new FlywheelMechanism() {},
new RotaryMechanism(IntakeConstants.MOTOR_NAME, IntakeConstants.CONSTANTS) {});
new RotaryMechanism(IntakePivotConstants.NAME, IntakePivotConstants.CONSTANTS) {});
vision = new Vision(null);
break;
}
Expand Down Expand Up @@ -326,7 +315,7 @@ private void configureButtonBindings() {
() -> controller.getLeftX(),
() -> -controller.getRightX()));

//Lock to 0° when A button is held
// Lock to 0° when A button is held
controller
.a()
.whileTrue(
Expand All @@ -336,7 +325,7 @@ private void configureButtonBindings() {
() -> -controller.getLeftX(),
() -> new Rotation2d()));

//Switch to X pattern when X button is pressed
// Switch to X pattern when X button is pressed
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));

// Reset gyro to 0° when B button is pressed
Expand All @@ -353,6 +342,8 @@ private void configureButtonBindings() {
controller
.x()
.onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper));

controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1)));
}

/**
Expand Down
11 changes: 6 additions & 5 deletions src/main/java/frc/robot/subsystems/BallCounter.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

public class BallCounter extends SubsystemBase {
Expand Down Expand Up @@ -57,7 +56,9 @@ public void shootBall() {

public boolean ballShot() {
Measurement measurement = _laserCAN.getMeasurement();
return measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm <= 30;
return measurement != null
&& measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT
&& measurement.distance_mm <= 30;
}

public double calculateFireRate() {
Expand Down Expand Up @@ -87,12 +88,12 @@ public void periodic() {

SmartDashboard.putNumber("Balls Fired", getBallsShot());
SmartDashboard.putNumber("Ball Fire Rate (per second)", Math.round(fireRate * 100.0) / 100.0);
SmartDashboard.putNumber("Seconds Since Last Fire", Math.round(secondsSinceLastFired() * 100.0) / 100.0);
SmartDashboard.putNumber(
"Seconds Since Last Fire", Math.round(secondsSinceLastFired() * 100.0) / 100.0);

if (!isBlocked && !canFire) {
canFire = true;
}
else if (isBlocked && canFire) {
} else if (isBlocked && canFire) {
shootBall();
canFire = false;
}
Expand Down
22 changes: 20 additions & 2 deletions src/main/java/frc/robot/subsystems/Climber.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,22 +2,28 @@

import static edu.wpi.first.units.Units.Inches;
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.Units;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.units.measure.LinearVelocity;
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;
import frc.lib.W8.mechanisms.linear.LinearMechanism;
import frc.robot.Constants.ClimberConstants;
import org.littletonrobotics.junction.Logger;

public class Climber extends SubsystemBase {
private LinearMechanism _io;
Distance goalDistance;

public Climber(LinearMechanism io) {
io = _io;
_io = io;
}

public void Position(double position) {
Expand Down Expand Up @@ -55,7 +61,19 @@ private State(LinearVelocity velocity) {
}

@Override
public void periodic() {}
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 void runClimber() {
runClimber();
Expand Down
Loading