# Simulation GUI and other tools window save file
networktables.json
-networktables.json.bck
simgui.json
-simgui-ds.json
*-window.json
# Simulation data log directory
"edu.wpi.first.math.**.proto.*",
"edu.wpi.first.math.**.struct.*",
],
- "java.dependency.enableDependencyCheckup": false
+ "java.dependency.enableDependencyCheckup": false,
+ "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
}
duplicatesStrategy = DuplicatesStrategy.INCLUDE
}
-project.compileJava.dependsOn(createVersionFile)
+
gversion {
srcDir = file("src/main/java")
classPackage = "frc.robot.util"
// changes networktables.json, networktables.json.bck (both Untracked)
// Uncomment the next line, set the desired RobotId, deploy, and then comment the line out
// RobotId.setRobotId(RobotId.SwerveCompetition);
-
+ DriveConstants.update(RobotId.getRobotId());
RobotController.setBrownoutVoltage(6.0);
// obtain this robot's identity
RobotId robotId = RobotId.getRobotId();
- DriveConstants.update(robotId);
-
// Record metadata
Logger.recordMetadata("ProjectName", BuildData.MAVEN_NAME);
Logger.recordMetadata("BuildDate", BuildData.BUILD_DATE);
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.robot.commands.DoNothing;
import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.commands.vision.ShutdownAllPis;
private BaseDriverConfig driver = null;
private Operator operator = null;
- // Auto Command selection
- private final SendableChooser<Command> autoChooser = new SendableChooser<>();
-
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
* <p>
* Different robots may have different subsystems.
*/
public RobotContainer(RobotId robotId) {
- // display the current robot id on smartdashboard
- SmartDashboard.putString("RobotID", robotId.toString());
-
- // Filling the SendableChooser on SmartDashboard
- autoChooserInit();
-
+ // climb = new Climb();
// dispatch on the robot
switch (robotId) {
case TestBed1:
intake = new Intake();
climb = new Climb();
spindexer = new Spindexer();
- break;
case WaffleHouse: // AKA Betabot
turret = new Turret();
shooter = new Shooter();
- break;
+ //hood = new Hood();
case SwerveCompetition: // AKA "Vantage"
- break;
case BetaBot: // AKA "Pancake"
vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
- break;
+ // fall-through
case Vivace:
- break;
case Phil: // AKA "IHOP"
- break;
case Vertigo: // AKA "French Toast"
drive = new Drivetrain(vision, new GyroIOPigeon2());
// Detected objects need access to the drivetrain
DetectedObject.setDrive(drive);
-
+
// SignalLogger.start();
driver.configureControls();
operator.configureControls();
-
+
initializeAutoBuilder();
registerCommands();
PathGroupLoader.loadPathGroups();
}
drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
break;
- }
+ }
// This is really annoying so it's disabled
DriverStation.silenceJoystickConnectionWarning(true);
- SmartDashboard.putString("RobotId", RobotId.getRobotId().name());
-
// TODO: verify this claim.
// LiveWindow is causing periodic loop overruns
LiveWindow.disableAllTelemetry();
drive.setVisionEnabled(enabled);
}
+
public void initializeAutoBuilder() {
AutoBuilder.configure(
() -> drive.getPose(),
}
public boolean brownout() {
- if (RobotController.getBatteryVoltage() < 6.0) {
+ if(RobotController.getBatteryVoltage() < 6.0) {
return true;
- } else {
+ }
+ else {
return false;
}
}
- // Autos
-
- /**
- * Initialize the SendableChooser on the SmartDashbboard.
- * Fill the SendableChooser with available Commands.
- */
- public void autoChooserInit() {
- // add the options to the Chooser
- autoChooser.setDefaultOption("Do nothing", new DoNothing());
- autoChooser.addOption("Do nada", new DoNothing());
- autoChooser.addOption("Spin my wheels", new DoNothing());
- autoChooser.addOption("Hello world", new InstantCommand(() -> System.out.println("Hello world")));
-
- // put the Chooser on the SmartDashboard
- SmartDashboard.putData("Auto chooser", autoChooser);
- }
-
- /**
- * Gets the auto command from SmartDashboard
- *
- * @return
- */
- public Command getAutoCommand() {
- // get the selected Command
- Command autoSelected = autoChooser.getSelected();
-
- return autoSelected;
+ public Command getAutoCommand(){
+ return auto;
}
- public void logComponents() {
- if (!Constants.LOG_MECHANISMS)
- return;
-
+ public void logComponents(){
+ if(!Constants.LOG_MECHANISMS) return;
+
Logger.recordOutput(
- "ComponentPoses",
- new Pose3d[] {
+ "ComponentPoses",
+ new Pose3d[] {
// Subsystem Pose3ds
- });
+ }
+ );
}
}
+
+
}
}
- if (robotId == RobotId.Default) {
- if (Robot.isSimulation()) {
- robotId = RobotId.SwerveCompetition; // Default to competition robot for simulation
- } else {
- throw new RuntimeException("RobotId is set to Default (or was unset)! Please set it to something.");
- }
- }
+ if (robotId == RobotId.Default) {
+ throw new RuntimeException("RobotId is set to Default (or was unset)! Please set it to something.");
+ }
// return the robot identity
return robotId;
package frc.robot.commands.gpm;
+import java.util.Optional;
+
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.MathUtil;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.hood.HoodConstants;
import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.ShotInterpolation;
import frc.robot.subsystems.turret.Turret;
import frc.robot.subsystems.turret.TurretConstants;
import frc.robot.util.ShooterPhysics;
private Shooter shooter;
private Spindexer spindexer;
- // TODO: Implement state machine with WantedState/CurrentState
private enum WantedState {
IDLE,
SHOOTING,
private WantedState wantedState = WantedState.IDLE;
private CurrentState currentState = CurrentState.IDLE;
- // TODO: find maximum interpolation
- private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767,
- HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
+ private void updateStates(){
+
+ }
+
+ //TODO: find maximum interpolation
+ private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
private double turretSetpoint;
private double hoodSetpoint;
private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
-
+
private Rotation2d lastTurretAngle;
private Rotation2d turretAngle;
private double turretVelocity;
this.hood = hood;
this.shooter = shooter;
this.spindexer = spindexer;
- drivepose = drivetrain.getPose();
- // drivepose = new Pose2d(drivepose.getTranslation(),
- // drivepose.getRotation().plus(new Rotation2d(Math.PI)));
+ drivepose = drivetrain.getPose();
+ //drivepose = new Pose2d(drivepose.getTranslation(), drivepose.getRotation().plus(new Rotation2d(Math.PI)));
goalState = ShooterPhysics.getShotParams(
- FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
- 8.0); // Random initial goalState to prevent it being null
-
+ FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
+ 8.0); // Random initial goalState to prevent it being null
+
addRequirements(turret);
}
public void updateSetpoints(Pose2d drivepose) {
- Pose2d turretPosition = drivepose
- .transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),
- TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d()));
+ Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d()));
double turretToTargetDistance = target.getDistance(turretPosition.getTranslation());
-
+
// If the robot is moving, adjust the target position based on velocity
ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
- double turretVelocityX = fieldRelVel.vxMetersPerSecond
+ double turretVelocityX =
+ fieldRelVel.vxMetersPerSecond
+ fieldRelVel.omegaRadiansPerSecond
- * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY()
- * Math.cos(drivepose.getRotation().getRadians())
- - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX()
- * Math.sin(drivepose.getRotation().getRadians()));
- double turretVelocityY = fieldRelVel.vyMetersPerSecond
+ * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians())
+ - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians()));
+ double turretVelocityY =
+ fieldRelVel.vyMetersPerSecond
+ fieldRelVel.omegaRadiansPerSecond
- * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX()
- * Math.cos(drivepose.getRotation().getRadians())
- - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY()
- * Math.sin(drivepose.getRotation().getRadians()));
+ * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians())
+ - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians()));
double timeOfFlight;
Pose2d lookaheadPose = turretPosition;
- // double lookaheadTurretToTargetDistance = turretToTargetDistance;
+ //double lookaheadTurretToTargetDistance = turretToTargetDistance;
/*
* Loop (20) until lookaheadPose converges BECAUSE -->
- * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate
- * for 6m (t = 0.8s)
- * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was
- * derived using t = 1.0s
+ * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s)
+ * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s
* So we make a bunch of guesses until it converges
*/
for (int i = 0; i < 20; i++) {
- Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(),
- TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
- Translation3d target3d = new Translation3d(target.getX(), target.getY(),
- FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting
+ Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
+ Translation3d target3d = new Translation3d(target.getX(), target.getY(), FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting
goalState = ShooterPhysics.getShotParams(
- target3d.minus(lookahead3d),
- 2.0);
+ target3d.minus(lookahead3d),
+ 2.0);
timeOfFlight = goalState.timeOfFlight();
double offsetX = turretVelocityX * timeOfFlight;
double offsetY = turretVelocityY * timeOfFlight;
- lookaheadPose = new Pose2d(
+ lookaheadPose =
+ new Pose2d(
turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
turretPosition.getRotation());
- // lookaheadTurretToTargetDistance =
- // target.getDistance(lookaheadPose.getTranslation());
+ //lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation());
}
turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
if (lastTurretAngle == null) {
lastTurretAngle = turretAngle;
}
- turretVelocity = turretAngleFilter.calculate(
- turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
+ turretVelocity =
+ turretAngleFilter.calculate(
+ turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
lastTurretAngle = turretAngle;
Logger.recordOutput("Lookahead Pose", lookaheadPose);
- double adjustedTurretSetpoint = MathUtil
- .angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
+ double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
// Shortest path
- double error = MathUtil.inputModulus(
- Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180,
- 180);
+ double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error;
- // Stay within +/- 200 -- if shortest path is past 200, we go long way around
+ // Stay within +/- 200 -- if shortest path is past 200, we go long way around
double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE;
- if (potentialSetpoint > turretRange / 2) {
+ if (potentialSetpoint > turretRange/2) {
potentialSetpoint -= 360;
- } else if (potentialSetpoint < -turretRange / 2) {
+ } else if (potentialSetpoint < -turretRange/2) {
potentialSetpoint += 360;
}
// Pitch is in radians
hoodAngle = goalState.pitch();
- hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE,
- HoodConstants.MAX_ANGLE);
+ hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
lastHoodAngle = hoodAngle;
}
}
- public void updateDrivePose() {
+ public void updateDrivePose(){
Pose2d currentPose = drivetrain.getPose();
// Add 180 degrees to the rotation bc robot is backwards
drivepose = new Pose2d(
- currentPose.getTranslation(),
- currentPose.getRotation());
+ currentPose.getTranslation(),
+ currentPose.getRotation()//.plus(new Rotation2d(Math.PI))
+ );
ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
drivepose.exp(
- new Twist2d(
- robotRelVel.vxMetersPerSecond * phaseDelay,
- robotRelVel.vyMetersPerSecond * phaseDelay,
- robotRelVel.omegaRadiansPerSecond * phaseDelay));
+ new Twist2d(
+ robotRelVel.vxMetersPerSecond * phaseDelay,
+ robotRelVel.vyMetersPerSecond * phaseDelay,
+ robotRelVel.omegaRadiansPerSecond * phaseDelay));
}
@Override
updateDrivePose();
updateSetpoints(drivepose);
- turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)),
- turretVelocity - drivetrain.getAngularRate(2));
- // hood.setFieldRelativeTarget(new
- // Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
- // shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
+ //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
+ //shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint);
SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint);
SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel());
+ System.out.println("COMMAND IS WORKINNGGG");
/** Spindexer Stuff!! */
- if (spindexer != null) {
+ if(spindexer != null){
spindexer.maxSpindexer();
}
}
// Set the turret to a safe position when the command ends
turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
shooter.setShooter(0.0);
- // hood.setFieldRelativeTarget(new
- // Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0);
- if (spindexer != null) {
+ //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0);
+ if(spindexer != null){
spindexer.stopSpindexer();
}
}
package frc.robot.commands.gpm;
+import java.lang.reflect.Field;
+
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Unit;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
import frc.robot.constants.Constants;
import frc.robot.constants.FieldConstants;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.turret.ShotInterpolation;
import frc.robot.subsystems.turret.Turret;
+import frc.robot.util.FieldZone;
+import frc.robot.util.ShootingTarget;
+import frc.robot.util.Vision.TurretVision;
public class SimpleAutoShoot extends Command {
private Turret turret;
private Drivetrain drivetrain;
+ private TurretVision turretVision;
private Shooter shooter;
private double fieldAngleRad;
private double turretSetpoint;
private double adjustedSetpoint;
+ private double yawToTagCamera;
private double yawToTag;
+ private boolean turretVisionEnabled = false;
private boolean SOTM = true;
private Translation2d drivepose;
+ private double lastPos = 0;
private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
private double lastTurretAngle = 0;
this.turret = turret;
this.drivetrain = drivetrain;
this.shooter = shooter;
- drivepose = drivetrain.getPose().getTranslation();
-
+ drivepose = drivetrain.getPose().getTranslation();
+
addRequirements(turret);
}
public void updateTurretSetpoint(Translation2d drivepose) {
-
- // FieldZone currentZone = getZone(drivepose);
+
+ //FieldZone currentZone = getZone(drivepose);
Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
double D_y;
double D_x;
-
+ double timeToGoal = 0.0;
+
// If the robot is moving, adjust the target position based on velocity
if (SOTM) {
ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
double xVel = fieldRelVel.vxMetersPerSecond;
double yVel = fieldRelVel.vyMetersPerSecond;
-
- double distance = target.getDistance(drivepose);
- double speed = Math.hypot(xVel, yVel);
- double timeToGoal = speed > 0.1 ? distance / speed : 0.0;
-
+
D_y = target.getY() - drivepose.getY() - timeToGoal * yVel;
D_x = target.getX() - drivepose.getX() - timeToGoal * xVel;
} else {
fieldAngleRad = Math.atan2(D_y, D_x);
// Calculate robot heading and adjust for reverse drive
- double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive
- // adjustment
+ double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment
// Calculate turret setpoint (angle relative to robot heading)
turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0);
// System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
}
- public void updateYawToTag() {
+ public void updateYawToTag(){
// Calculate the yaw offset to the tag
double D_y = FieldConstants.getHubTranslation().getY() - drivetrain.getPose().getY();
double D_x = FieldConstants.getHubTranslation().getX() - drivetrain.getPose().getX();
public void initialize() {
// Initialize setpoint calculation and set the initial goal for the turret
updateTurretSetpoint(drivepose);
+ // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), drivetrain.getAngularRate(2));
turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), 0);
}
@Override
public void execute() {
+ //shooter.setShooterPower(ShotInterpolation.shooterPowerMap.get(FieldConstants.getHubTranslation().toTranslation2d().getDistance(drivetrain.getPose().getTranslation())));
// Continuously update setpoints and adjust based on vision if available
drivepose = drivetrain.getPose().getTranslation();
updateTurretSetpoint(drivepose);
updateYawToTag();
+ // double turretVelocity =
+ // turretAngleFilter.calculate(
+ // new Rotation2d(Units.degreesToRadians(turretSetpoint)).minus(new Rotation2d(Units.degreesToRadians(lastTurretAngle))).getRadians() / Constants.LOOP_TIME);
+
double velocityAdjustment = 0;
+ double turretAcceleration = ((-drivetrain.getAngularRate(2)) - (lastFrameVelocity)) / Constants.LOOP_TIME;
if (Math.abs(lastTurretAngle - turretSetpoint) > 90) {
velocityAdjustment = -drivetrain.getAngularRate(2) * 1.4;
}
Logger.recordOutput("Spinny accel", drivetrain.getAngularRate(2));
Logger.recordOutput("Original Turret Setpoint", turretSetpoint);
-
- turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)),
- -drivetrain.getAngularRate(2) - velocityAdjustment);
+
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) - velocityAdjustment);
+ // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), (-drivetrain.getAngularRate(2)) + turretAcceleration * 0.3);
+ //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity);
lastTurretAngle = turretSetpoint;
lastFrameVelocity = drivetrain.getAngularRate(2);
// Set the turret to a safe position when the command ends
turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
}
-}
\ No newline at end of file
+
+ public boolean leftSide(Translation2d drivepose) {
+ if (drivepose.getY() > (FieldConstants.FIELD_WIDTH / 2)) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ public FieldZone getZone(Translation2d drivepose) {
+ return FieldConstants.getZone(drivepose);
+ }
+}
+
public class AcquireGamePiece extends SequentialCommandGroup {
/**
* Intakes a game piece
- *
+ *
* @param gamePiece The supplier for the game piece to intake
- * @param drive The drivetrain
+ * @param drive The drivetrain
*/
- public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive) {
- // TODO: Replace DoNothing with next year's intake command
+ public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive){
addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive)));
}
}
\ No newline at end of file
public class Constants {
- // constants:
+ // constants:
public static final double GRAVITY_ACCELERATION = 9.8;
public static final double ROBOT_VOLTAGE = 12.0;
public static final CANBus RIO_CAN = new CANBus("rio");
public static final CANBus SUBSYSTEM_CANIVORE_CAN = new CANBus("CANivoreSub");
- // Logging
+ // Logging
public static final boolean USE_TELEMETRY = true;
public static enum Mode {
public static final double DEFAULT_DEADBAND = 0.00005;
public static final double TRANSLATIONAL_DEADBAND = 0.01;
-
+
public static final double ROTATION_DEADBAND = 0.01;
-
+
public static final double HEADING_DEADBAND = 0.05;
public static final double HEADING_SLEWRATE = 10;
- // Modes
+ //Modes
public static final Mode SIM_MODE = Mode.SIM;
public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE;
package frc.robot.constants;
+import java.lang.reflect.Field;
+
+import org.opencv.dnn.Net;
+
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.robot.Robot;
import frc.robot.util.FieldZone;
+import frc.robot.util.ShootingTarget;
public class FieldConstants {
/** Width of the field [meters] */
- public static final double FIELD_LENGTH = Units.inchesToMeters(57 * 12 + 6 + 7.0 / 8.0);
+ public static final double FIELD_LENGTH = Units.inchesToMeters(57*12 + 6+7.0/8.0);
/** Height of the field [meters] */
- public static final double FIELD_WIDTH = Units.inchesToMeters(26 * 12 + 5);
+ public static final double FIELD_WIDTH = Units.inchesToMeters(26*12 + 5);
- /** Apriltag layout for 2026 REBUILT */
+ /**Apriltag layout for 2026 REBUILT */
public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
public static final double RED_BORDER = Units.inchesToMeters(180);
public static final double RIGHT_SIDE_TARGET = FIELD_WIDTH * 0.75;
/** Location of hub target */
- public static final Translation3d HUB_BLUE = new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035,
- Units.inchesToMeters(72));
-
- public static final Translation3d HUB_RED = new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 + 20),
- 4.035 + .67, Units.inchesToMeters(72));
+ public static final Translation3d HUB_BLUE =
+ new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035, Units.inchesToMeters(72));
+
+ public static final Translation3d HUB_RED =
+ new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72));
+
+ public static final Translation3d NEUTRAL_LEFT =
+ new Translation3d(FIELD_LENGTH/2, LEFT_SIDE_TARGET, 0);
- public static final Translation3d NEUTRAL_LEFT = new Translation3d(FIELD_LENGTH / 2, LEFT_SIDE_TARGET, 0);
+ public static final Translation3d NEUTRAL_RIGHT =
+ new Translation3d(FIELD_LENGTH/2, RIGHT_SIDE_TARGET, 0);
- public static final Translation3d NEUTRAL_RIGHT = new Translation3d(FIELD_LENGTH / 2, RIGHT_SIDE_TARGET, 0);
+ public static final Translation3d ALLIANCE_LEFT_BLUE =
+ new Translation3d(BLUE_BORDER + 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back
- // previous hub + a few feet further back
- public static final Translation3d ALLIANCE_LEFT_BLUE = new Translation3d(BLUE_BORDER + 5, LEFT_SIDE_TARGET, 0);
+ public static final Translation3d ALLIANCE_RIGHT_BLUE =
+ new Translation3d(BLUE_BORDER + 5, RIGHT_SIDE_TARGET, 0);
- public static final Translation3d ALLIANCE_RIGHT_BLUE = new Translation3d(BLUE_BORDER + 5, RIGHT_SIDE_TARGET, 0);
- // previous hub + a few feet further back
- public static final Translation3d ALLIANCE_LEFT_RED = new Translation3d(RED_BORDER + 5, LEFT_SIDE_TARGET, 0);
+ public static final Translation3d ALLIANCE_LEFT_RED =
+ new Translation3d(RED_BORDER + 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back
- public static final Translation3d ALLIANCE_RIGHT_RED = new Translation3d(RED_BORDER + 5, RIGHT_SIDE_TARGET, 0);
+ public static final Translation3d ALLIANCE_RIGHT_RED =
+ new Translation3d(RED_BORDER + 5, RIGHT_SIDE_TARGET, 0);
public static final double BlueAllianceLine = BLUE_BORDER; // That's the distance from one side to the blue bump
- public static final double RedAllianceLine = RED_BORDER; // That's the distance from one side to the red bump
+ public static final double RedAllianceLine = RED_BORDER; //
public static Translation3d getHubTranslation() {
if (Robot.getAlliance() == Alliance.Blue) {
public static FieldZone getZone(Translation2d drivepose) {
double x = drivepose.getX();
double y = drivepose.getY();
- if (x < FieldConstants.RedAllianceLine) { // inside red
+ if(x < FieldConstants.RedAllianceLine) { // inside red
if (Robot.getAlliance() == Alliance.Red) {
return FieldZone.ALLIANCE;
} else {
public static final int HOOD_ID = 11;
// Spindexer
- public static final int SPINDEXER_ID = 16;
+ public static final int SPINDEXER_ID = 12;
// Intake
public static final int INTAKE_BASE_LEFT_ID = 13;
/* Neutral Modes */
public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake;
public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake;
-
- /* Gyro mount pose roll in deg (180.0 if placed under the robot) */
- public static double GYRO_MOUNT_POSE_ROLL = 0.0;
/* Drive Motor PID Values */
public static final double[] P_VALUES = {
// MK5n
INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
- // Gear ratios
+
DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0);
STEER_GEAR_RATIO = 287.0 / 11.0;
- // Gyro is mounted under the robot
- GYRO_MOUNT_POSE_ROLL = 180.0;
-
MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
} else if(robotId == RobotId.WaffleHouse){
MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
} else if (robotId == RobotId.Vertigo) {
- STEER_OFFSET_FRONT_LEFT = Units.radiansToDegrees(3.43);
- STEER_OFFSET_FRONT_RIGHT = Units.radiansToDegrees(1.91) + 180;
- STEER_OFFSET_BACK_LEFT = Units.radiansToDegrees(2.28);
- STEER_OFFSET_BACK_RIGHT = Units.radiansToDegrees(5.03);
+ STEER_OFFSET_FRONT_LEFT = 4.185;
+ STEER_OFFSET_FRONT_RIGHT = 101.519+90;
+ STEER_OFFSET_BACK_LEFT = 38.997+180;
+ STEER_OFFSET_BACK_RIGHT = 242.847-90;
DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
import java.util.function.BooleanSupplier;
+import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.Constants;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
import frc.robot.subsystems.spindexer.Spindexer;
import frc.robot.subsystems.turret.Turret;
import frc.robot.subsystems.hood.Hood;
*/
public class PS5ControllerDriverConfig extends BaseDriverConfig {
private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = () -> false;
+ private final BooleanSupplier slowModeSupplier = ()->false;
private Shooter shooter;
private Turret turret;
private Hood hood;
private Intake intake;
private Spindexer spindexer;
+ private Pose2d alignmentPose = null;
+ private Command turretAutoShoot;
private Command autoShoot;
private boolean intakeBoolean = true;
- public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake,
- Spindexer spindexer) {
+ public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake, Spindexer spindexer) {
super(drive);
this.shooter = shooter;
this.turret = turret;
this.spindexer = spindexer;
}
- public void configureControls() {
+ public void configureControls() {
// Reset the yaw. Mainly useful for testing/driver practice
driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)
+ )));
// Cancel commands
- driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
+ driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{
getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
+ getDrivetrain().setDesiredPose(()->null);
CommandScheduler.getInstance().cancelAll();
}));
// Align wheels
driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand(
- () -> getDrivetrain().setStateDeadband(false),
- getDrivetrain()::alignWheels,
- interrupted -> getDrivetrain().setStateDeadband(true),
- () -> false, getDrivetrain()).withTimeout(2));
+ ()->getDrivetrain().setStateDeadband(false),
+ getDrivetrain()::alignWheels,
+ interrupted->getDrivetrain().setStateDeadband(true),
+ ()->false, getDrivetrain()).withTimeout(2));
// Intake
- if (intake != null) {
- driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
- if (intakeBoolean) {
+ if(intake != null){
+ driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{
+ if(intakeBoolean){
intake.setSetpoint(IntakeConstants.INTAKE_ANGLE);
intake.setFlyWheel();
intakeBoolean = false;
- } else {
+ }
+ else{
intake.setSetpoint(IntakeConstants.STOW_ANGLE);
intake.stopFlyWheel();
}
}
// Auto shoot
- if (turret != null) {
+ if(turret != null){
driver.get(PS5Button.SQUARE).onTrue(
- new InstantCommand(() -> {
- if (autoShoot != null && autoShoot.isScheduled()) {
+ new InstantCommand(()->{
+ if (autoShoot != null && autoShoot.isScheduled()){
autoShoot.cancel();
- } else {
+ } else{
autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
CommandScheduler.getInstance().schedule(autoShoot);
}
- }));
+ })
+ );
}
+
}
@Override
return false;
}
- public void startRumble() {
+ public void startRumble(){
driver.rumbleOn();
}
- public void endRumble() {
+ public void endRumble(){
driver.rumbleOff();
}
}
import frc.robot.util.ClimbArmSim;
public class Climb extends SubsystemBase {
-
+
private double startingPosition = 0;
- // Motors
+ //Motors
// TODO: tune better once design is finalized
private final PIDController pid = new PIDController(0.4, 4, 0.04);
private final DCMotor climbGearBox = DCMotor.getKrakenX60(1);
private TalonFXSimState encoderSim;
- // Mechism2d display
+ //Mechism2d display
private final Mechanism2d simulationMechanism = new Mechanism2d(3, 3);
private final MechanismRoot2d mechanismRoot = simulationMechanism.getRoot("Climb", 1.5, 1.5);
private final MechanismLigament2d simLigament = mechanismRoot.append(
- new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite)));
+ new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite))
+ );
private final double climbGearRatio = 54 / 10 * 60 / 18; // gear ratio of 18
private ClimbArmSim climbSim;
public Climb() {
if (isSimulation()) {
encoderSim = motorLeft.getSimState();
- encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition) * climbGearRatio);
+ encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition)*climbGearRatio);
climbSim = new ClimbArmSim(
- climbGearBox,
- climbGearRatio,
- 0.1,
- 0.127,
- 0, // min angle
- Units.degreesToRadians(90), // max angle
- true,
- Units.degreesToRadians(startingPosition),
- 60);
+ climbGearBox,
+ climbGearRatio,
+ 0.1,
+ 0.127,
+ 0, //min angle
+ Units.degreesToRadians(90), //max angle
+ true,
+ Units.degreesToRadians(startingPosition),
+ 60
+ );
climbSim.setIsClimbing(true);
}
pid.setIZone(1);
pid.setSetpoint(Units.degreesToRadians(startingPosition));
- motorLeft.setPosition(Units.degreesToRotations(startingPosition) * climbGearRatio);
- motorRight.setPosition(Units.degreesToRotations(startingPosition) * climbGearRatio);
+ motorLeft.setPosition(Units.degreesToRotations(startingPosition)*climbGearRatio);
+ motorRight.setPosition(Units.degreesToRotations(startingPosition)*climbGearRatio);
SmartDashboard.putData("PID", pid);
SmartDashboard.putData("Climb Display", simulationMechanism);
}
@Override
- public void periodic() {
+ public void periodic() {
double motorPosition = motorLeft.getPosition().getValueAsDouble();
- double currentPosition = Units.rotationsToRadians(motorPosition / climbGearRatio);
+ double currentPosition = Units.rotationsToRadians(motorPosition/climbGearRatio);
power = pid.calculate(currentPosition);
/**
* Sets the motor to an angle from 0-90 deg
- *
* @param angle in degrees
*/
public void setAngle(double angle) {
/**
* Gets the current position of the motor in degrees
- *
* @return The angle in degrees
*/
public double getAngle() {
/**
* Turns the motor to 90 degrees (extended positiion)
*/
- public void extend() {
+ public void extend(){
double extendAngle = 90;
setAngle(extendAngle);
}
/**
* Turns the motor to 0 degrees (climb position)
*/
- public void climb() {
+ public void climb(){
setAngle(startingPosition);
}
- public boolean isSimulation() {
+ public boolean isSimulation(){
return RobotBase.isSimulation();
}
}
\ No newline at end of file
private final Queue<Double> yawPositionQueue;
private final Queue<Double> yawTimestampQueue;
private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
- private final Pigeon2Configuration config = new Pigeon2Configuration();
public GyroIOPigeon2() {
- config.MountPose.MountPoseRoll = DriveConstants.GYRO_MOUNT_POSE_ROLL;
- pigeon.getConfigurator().apply(config);
+ pigeon.getConfigurator().apply(new Pigeon2Configuration());
pigeon.getConfigurator().setYaw(0.0);
yaw.setUpdateFrequency(250);
yawVelocity.setUpdateFrequency(50.0);
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.constants.swerve.DriveConstants;
import frc.robot.constants.swerve.ModuleConstants;
import frc.robot.constants.swerve.ModuleType;
import org.littletonrobotics.junction.Logger;
+import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import frc.robot.constants.Constants;
import frc.robot.constants.IdConstants;
-public class Hood extends SubsystemBase implements HoodIO {
- private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+public class Hood extends SubsystemBase implements HoodIO{
+ private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
- private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE);
+ private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE);
private double MAX_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MAX_ANGLE);
private double MAX_VEL_RAD_PER_SEC = 25;
private double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
- private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO;
+ private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO;
private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02
, 0.02);
TalonFXConfiguration config = new TalonFXConfiguration();
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+ config.Slot0.kP = 2.0;
config.Slot0.kS = 0.1; // Static friction compensation
config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio
config.Slot0.kD = 0.02; // The "Braking" term to stop overshoot
motor.getConfigurator().apply(config);
motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO);
+
+ SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0)));
+ SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0)));
+ SmartDashboard.putData("min", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0)));
+ }
+
+ /**
+ * @return Position of the MOTOR in radians
+ */
+ public double getMotorPositionRad(){
+ return Units.rotationsToRadians(motor.getPosition().getValueAsDouble());
+ }
+
+ /**
+ * Sets the setpoint position and velocity of the hood
+ * @param angle
+ * @param velocityRadPerSec
+ */
+ public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) {
goalAngle = angle;
goalVelocityRadPerSec = velocityRadPerSec;
}
- @Override
- public void periodic() {
+ @Override
+ public void periodic() {
updateInputs();
Logger.processInputs("Hood", inputs);
}
- @Override
+ @Override
public void updateInputs() {
inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO;
inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / GEAR_RATIO;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
public class HoodConstants {
public static final double HOOD_GEAR_RATIO = 64;
public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters
+ // public static final double MAX_VELOCITY = 5; // rad/s
public static final double MAX_VELOCITY = 0.30; // rad/s
public static final double MAX_ACCELERATION = 30; // rad/s^2
public static final double MAX_ANGLE = 82; // degrees
- public static final double MIN_ANGLE = 58.5; // degrees
+ public static final double MIN_ANGLE = 58.5; // degrees
+
+ public static final double START_ANGLE = 90-22.9; // degrees
// Arena dimensions
public static final double TARGET_HEIGHT = 2.44; // meters
public static final Translation2d TRANSLATION_TARGET = new Translation2d(0, 0);
public static final Rotation2d ROTATION_TARGET_ANGLE = new Rotation2d();
// Other
- public static final double INITIAL_VELOCITY = 14.9; // meters per second
+ public static final double INITIAL_VELOCTIY = 14.9; // meters per second
+
+ // Testing purposes
+ public static final double START_DISTANCE = 2; // meters
// Calibration Purposes
public static final double CURRENT_SPIKE_THRESHHOLD = 10.0; // amps
public interface HoodIO {
@AutoLog
- public static class HoodIOInputs {
+ public static class HoodIOInputs{
public double positionDeg = HoodConstants.MAX_ANGLE;
public double velocityRadPerSec = 0.0;
public double motorCurrent = 0.0;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.interfaces.LaserCanInterface.Measurement;
+import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode;
+import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest;
+import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
// Goal Velocity / Double theCircumfrence
private double shooterTargetSpeed = 0;
+ private double feederPower = 0;
public boolean shooterAtMaxSpeed = false;
package frc.robot.subsystems.spindexer;
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.hardware.TalonFX;
-import org.littletonrobotics.junction.Logger;
-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.Constants;
import frc.robot.constants.IdConstants;
+import frc.robot.subsystems.spindexer.SpindexerIO;
-public class Spindexer extends SubsystemBase implements SpindexerIO {
- private TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+public class Spindexer extends SubsystemBase implements SpindexerIO{
+ TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID);
private double power = 0.0;
public int ballCount = 0;
- private boolean wasSpindexerSlow = false;
private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged();
- public Spindexer() {
- updateInputs();
-
- // configure current limit
- CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = SpindexerConstants.currentLimit;
- limitConfig.StatorCurrentLimitEnable = true;
- motor.getConfigurator().apply(limitConfig);
-
- SmartDashboard.putData("Max speed spindexer", new InstantCommand(() -> maxSpindexer()));
- SmartDashboard.putData("Turn off spindexer", new InstantCommand(() -> stopSpindexer()));
- SmartDashboard.putData("Spindexer 50%", new InstantCommand(() -> setSpindexer(0.5)));
+ public Spindexer(){
+ //SmartDashboard.putData("Turn on Spindexer", new InstantCommand(()-> turnOnSpindexer()));
}
@Override
public void periodic() {
- updateInputs();
-
- // if speed was set
- if (SmartDashboard.containsKey("Spindexer Power")) {
- double dashboardPower = SmartDashboard.getNumber("Spindexer Power", 0.0);
- if (dashboardPower != power) {
- power = dashboardPower;
- }
- }
-
- motor.set(power);
-
+ power = SmartDashboard.getNumber("Spindexer Power", power);
SmartDashboard.putNumber("Spindexer Power", power);
- SmartDashboard.putNumber("Spindexer Velocity", inputs.spindexerVelocity);
-
- // scale threshold based on power
- double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power;
- SmartDashboard.putNumber("Spindexer Velocity Threshold", velocityThreshold);
- SmartDashboard.putNumber("Spindexer Ball Count", ballCount);
-
- boolean isSpindexerSlow = inputs.spindexerVelocity < velocityThreshold;
- if (wasSpindexerSlow && !isSpindexerSlow && power > 0.1) {
+ motor.set(power);
+ if (inputs.spindexerVelocity < SpindexerConstants.spindexerVelocityWithBall) {
ballCount++;
}
- wasSpindexerSlow = isSpindexerSlow;
}
- public void maxSpindexer() {
- power = SpindexerConstants.spindexerMaxPower;
+ public void maxSpindexer(){
+ power = 1.0;
}
- public void stopSpindexer() {
+ public void stopSpindexer(){
power = 0.0;
}
- public void setSpindexer(double power) {
- this.power = power;
- }
-
@Override
public void updateInputs() {
- inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble() * SpindexerConstants.gearRatio;
+ inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble();
inputs.spindexerCurrent = motor.getStatorCurrent().getValueAsDouble();
- Logger.processInputs("Spindexer", inputs);
}
}
package frc.robot.subsystems.spindexer;
public class SpindexerConstants {
- public static final double gearRatio = 1.0 / 27.0;
- // TODO: measure actual velocity with/without ball to tune threshold
- public static final double spindexerVelocityWithBall = 6.0 * gearRatio; // output rps at full power
- public static final double spindexerMaxPower = 1.0;
- public static final int currentLimit = 20; // amps
+ public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls)
}
public final class ChineseRemainderTheorem {
- private ChineseRemainderTheorem() {
- }
+ private ChineseRemainderTheorem() {}
/**
* Computes x such that:
- * x ≡ a (mod n1)
- * x ≡ b (mod n2)
+ * x ≡ a (mod n1)
+ * x ≡ b (mod n2)
*
* n1 and n2 MUST be coprime.
*
int invN1modN2 = modInverse(n1, n2);
int invN2modN1 = modInverse(n2, n1);
- int result = (a * n2 * invN2modN1 +
- b * n1 * invN1modN2) % N;
+ int result =
+ (a * n2 * invN2modN1 +
+ b * n1 * invN1modN2) % N;
return (result + N) % N;
}
+++ /dev/null
-
-package frc.robot.util;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
-public class ChineseRemainderTheoremTest {
-
- @BeforeEach
- public void prepare() {
- }
-
- @AfterEach
- public void cleanup() {
- }
-
- @Test
- public void test() {
- double tolerance = 0.01;
-
- int val = ChineseRemainderTheorem.solve(5000 % 124, 124, 5000 % 127, 127);
- assertEquals(5000, val, tolerance);
- }
-}
--- /dev/null
+
+package frc.robot.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import frc.robot.util.ChineseRemainderTheorum.Encoder;
+
+public class ChineseRemainderTheorumTest {
+
+ @BeforeEach
+ public void prepare() {
+ }
+
+ @AfterEach
+ public void cleanup() {
+ }
+
+ @Test
+ public void test() {
+ double tolerance = 0.01;
+
+ Encoder a = new Encoder(5000 % 123, 123);
+ Encoder b = new Encoder(5000 % 321, 321);
+ double val = ChineseRemainderTheorum.compute(a, b, tolerance);
+ assertEquals(5000, val, tolerance);
+ }
+}
{
"fileName": "Phoenix6-frc2026-latest.json",
"name": "CTRE-Phoenix (v6)",
- "version": "26.1.1",
+ "version": "26.1.0",
"frcYear": "2026",
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
"mavenUrls": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-java",
- "version": "26.1.1"
+ "version": "26.1.0"
}
],
"jniDependencies": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "api-cpp",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "api-cpp-sim",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdi",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdle",
- "version": "26.1.1",
+ "version": "26.1.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-cpp",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_Phoenix6_WPI",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_PhoenixTools",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "wpiapi-cpp-sim",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_Phoenix6_WPISim",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_PhoenixTools_Sim",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimPigeonIMU",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimProTalonFX",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimProTalonFXS",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimProCANcoder",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimProPigeon2",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimProCANrange",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdi",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimProCANdi",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdle",
- "version": "26.1.1",
+ "version": "26.1.0",
"libName": "CTRE_SimProCANdle",
"headerClassifier": "headers",
"sharedLibrary": true,
{
"fileName": "REVLib.json",
"name": "REVLib",
- "version": "2026.0.2",
+ "version": "2026.0.1",
"frcYear": "2026",
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
"mavenUrls": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-java",
- "version": "2026.0.2"
+ "version": "2026.0.1"
}
],
"jniDependencies": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-driver",
- "version": "2026.0.2",
+ "version": "2026.0.1",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "RevLibBackendDriver",
- "version": "2026.0.2",
+ "version": "2026.0.1",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "RevLibWpiBackendDriver",
- "version": "2026.0.2",
+ "version": "2026.0.1",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-cpp",
- "version": "2026.0.2",
+ "version": "2026.0.1",
"libName": "REVLib",
"headerClassifier": "headers",
"sharedLibrary": false,
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-driver",
- "version": "2026.0.2",
+ "version": "2026.0.1",
"libName": "REVLibDriver",
"headerClassifier": "headers",
"sharedLibrary": false,
{
"groupId": "com.revrobotics.frc",
"artifactId": "RevLibBackendDriver",
- "version": "2026.0.2",
+ "version": "2026.0.1",
"libName": "BackendDriver",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
{
"groupId": "com.revrobotics.frc",
"artifactId": "RevLibWpiBackendDriver",
- "version": "2026.0.2",
+ "version": "2026.0.1",
"libName": "REVLibWpi",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"linuxarm32",
"osxuniversal"
]
+
}
]
}
\ No newline at end of file
{
- "fileName": "photonlib.json",
- "name": "photonlib",
- "version": "v2026.2.2",
- "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
- "frcYear": "2026",
- "mavenUrls": [
- "https://maven.photonvision.org/repository/internal",
- "https://maven.photonvision.org/repository/snapshots"
- ],
- "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
- "jniDependencies": [
- {
- "groupId": "org.photonvision",
- "artifactId": "photontargeting-cpp",
- "version": "v2026.2.2",
- "skipInvalidPlatforms": true,
- "isJar": false,
- "validPlatforms": [
- "windowsx86-64",
- "linuxathena",
- "linuxx86-64",
- "osxuniversal"
- ]
- }
- ],
- "cppDependencies": [
- {
- "groupId": "org.photonvision",
- "artifactId": "photonlib-cpp",
- "version": "v2026.2.2",
- "libName": "photonlib",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxathena",
- "linuxx86-64",
- "osxuniversal"
- ]
- },
- {
- "groupId": "org.photonvision",
- "artifactId": "photontargeting-cpp",
- "version": "v2026.2.2",
- "libName": "photontargeting",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxathena",
- "linuxx86-64",
- "osxuniversal"
- ]
- }
- ],
- "javaDependencies": [
- {
- "groupId": "org.photonvision",
- "artifactId": "photonlib-java",
- "version": "v2026.2.2"
- },
- {
- "groupId": "org.photonvision",
- "artifactId": "photontargeting-java",
- "version": "v2026.2.2"
- }
- ]
+ "fileName": "photonlib.json",
+ "name": "photonlib",
+ "version": "v2026.1.1-rc-3",
+ "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+ "frcYear": "2026",
+ "mavenUrls": [
+ "https://maven.photonvision.org/repository/internal",
+ "https://maven.photonvision.org/repository/snapshots"
+ ],
+ "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+ "jniDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-cpp",
+ "version": "v2026.1.1-rc-3",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photonlib-cpp",
+ "version": "v2026.1.1-rc-3",
+ "libName": "photonlib",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-cpp",
+ "version": "v2026.1.1-rc-3",
+ "libName": "photontargeting",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photonlib-java",
+ "version": "v2026.1.1-rc-3"
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-java",
+ "version": "v2026.1.1-rc-3"
+ }
+ ]
}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "yams.json",
+ "name": "Yet Another Mechanism System",
+ "version": "2026.2.3",
+ "frcYear": "2026",
+ "uuid": "a1051e86-a979-4880-a28b-a0d5362d1d96",
+ "mavenUrls": [
+ "https://yet-another-software-suite.github.io/YAMS/releases/"
+ ],
+ "jsonUrl": "https://yet-another-software-suite.github.io/YAMS/yams.json",
+ "javaDependencies": [
+ {
+ "groupId": "yams",
+ "artifactId": "YAMS-java",
+ "version": "2026.2.3"
+ }
+ ],
+ "cppDependencies": [],
+ "jniDependencies": [],
+ "requires": []
+}
\ No newline at end of file