Skip to content

Finalizing subsystems before testing #41

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
Carbonic-Acidd merged 1 commit into from
Feb 17, 2026
Merged

Finalizing subsystems before testing #41

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
95 changes: 77 additions & 18 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,3 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import static edu.wpi.first.units.Units.Degrees;
Expand All @@ -10,6 +6,7 @@
import static edu.wpi.first.units.Units.FeetPerSecond;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
Expand Down Expand Up @@ -167,22 +164,26 @@ public static class IntakeConstants {
public static final int armMainID = 14;
public static final int armFollowerID = 15;
public static final int intakeID = 16;
public static final int armEncoderID = 17;

public static final double armGearRatio = 3.0;
public static final int armMagnetID = 17;

public static final Angle maxPosition = Rotations.of(1.0);
public static final Angle minPosition = Rotations.of(0.0);
public static final double armGearRatio = (5 / 1) * (36 / 10);
// 111.182298 - 15.025 = 96;
public static final Angle minPosition = Degrees.of(0.0);
public static final Angle maxPosition = Degrees.of(96.157298);

public static final Angle downPosition = Rotations.of(1.0);
public static final Angle upPosition = Rotations.of(0.0);
public static final Angle downPosition = maxPosition;
public static final Angle upPosition = minPosition;

public static final Angle armDownPositionTolerance = maxPosition.plus(minPosition).div(2);

public static final Angle armMagnetOffset = Rotations.of(0);

public static final double intakeSpeed = .353;

public static final double armStallCurrent = 6.7; // amps
public static final double armStallVelocity = 0.1353; // rps
public static final double armZeroSpeed = -0.20;

public static final MotionMagicConfigs motionMagicConfigs =
new MotionMagicConfigs()
.withMotionMagicCruiseVelocity(RotationsPerSecond.of(0))
Expand Down Expand Up @@ -516,13 +517,17 @@ public static class AutoConstants {
}

public static class HoodConstants {
public static final int hoodMotorID = 21;
public static final int hoodMotorID = 21; // 21

public static final double hoodStallCurrent = 6.7; // amps
public static final double hoodStallVelocity = 0.1353; // rps
public static final double hoodZeroSpeed = -0.10;

public static final Angle minAngle = Degrees.of(21.448);
public static final Angle maxAngle = Degrees.of(59.231);

public static final double hoodGearRatio =
((48 / 12) * (30 / 15) * (15 / 10)) / ((maxAngle.minus(minAngle)).in(Degrees) / 360);
public static final double hoodGearRatio = ((48 / 12) * (30 / 15) * (15 / 10));
// / ((maxAngle.minus(minAngle)).in(Degrees) / 360);

public static final MotionMagicConfigs motionMagicConfigs =
new MotionMagicConfigs()
Expand Down Expand Up @@ -570,9 +575,63 @@ public static class HoodConstants {
}

public static class SpindexerConstants {
public static final int SpindexerMotorID = 22;
public static final int SpindexerLaserID = 23;
public static final double SpindexerMotorSpeed = 0.5;
public static final double SpindexerDistance = 100;
public static final int spindexerMotorID = 22;
public static final int kickerMotorID = 23;
public static final double spindexerMotorSpeed = 0.5;
public static final double kickerMotorSpeed = 0.5;

public static final MotorOutputConfigs motorOutputConfigs =
new MotorOutputConfigs()
.withInverted(InvertedValue.CounterClockwise_Positive)
.withNeutralMode(NeutralModeValue.Brake);

public static final CurrentLimitsConfigs currentLimitConfigs =
new CurrentLimitsConfigs().withSupplyCurrentLimit(45).withSupplyCurrentLimitEnable(true);

public static final TalonFXConfiguration spindexerConfigs =
new TalonFXConfiguration()
.withCurrentLimits(currentLimitConfigs)
.withMotorOutput(motorOutputConfigs);
}

public static class ShooterConstants {
public static final int shooterMotorID = 24;

public static final double shooterGearRatio = 1;
public static final Distance flyWheelRadius = Inches.of(2.0);

public static final LinearVelocity shooterSpeedTolerance = MetersPerSecond.of(0.1);

public static final MotionMagicConfigs motionMagicConfigs =
new MotionMagicConfigs().withMotionMagicAcceleration(0.0).withMotionMagicJerk(0.0);

public static final Slot0Configs slot0Configs =
new Slot0Configs()
.withKS(0.00)
.withKV(0.00)
.withKA(0.00)
.withKP(0.0)
.withKI(0.00)
.withKD(0.00)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign);

public static final FeedbackConfigs feedbackConfigs =
new FeedbackConfigs().withSensorToMechanismRatio(shooterGearRatio);

public static final MotorOutputConfigs motorOutputConfigs =
new MotorOutputConfigs()
.withInverted(
InvertedValue.CounterClockwise_Positive) // needs to spin left when wires up
.withNeutralMode(NeutralModeValue.Coast);
public static final CurrentLimitsConfigs currentLimitConfigs =
new CurrentLimitsConfigs().withSupplyCurrentLimit(45).withSupplyCurrentLimitEnable(true);

public static final TalonFXConfiguration shooterConfigs =
new TalonFXConfiguration()
.withCurrentLimits(currentLimitConfigs)
.withSlot0(slot0Configs)
.withMotionMagic(motionMagicConfigs)
.withFeedback(feedbackConfigs)
.withMotorOutput(motorOutputConfigs);
}
}
53 changes: 44 additions & 9 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 @@ -32,6 +32,7 @@
import frc.robot.subsystems.Hood;
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.Shooter;
import frc.robot.subsystems.Spindexer;
import frc.robot.subsystems.Swerve;
import frc.robot.subsystems.Turret;
import frc.robot.util.AllianceUtil;
Expand Down Expand Up @@ -86,6 +87,9 @@ public class RobotContainer {
@Logged(name = "Intake")
private final Intake intake = new Intake();

@Logged(name = "Spindexer")
private final Spindexer spindexer = new Spindexer();

@Logged(name = "3D Visualization")
private final RobotVisualization robotVisualization =
new RobotVisualization(turret, hood, swerve, shooter);
Expand Down Expand Up @@ -131,7 +135,13 @@ public RobotContainer() {
NamedCommands.registerCommand(
"Shoot On The Move",
new ShootOnTheMove(
swerve, turret, hood, shooter, AllianceUtil::getHubPose, robotVisualization));
swerve,
turret,
hood,
shooter,
spindexer,
AllianceUtil::getHubPose,
robotVisualization));

NamedCommands.registerCommand(
"Pathfind to Mid-Left Bumper", swerve.pathFindToPose(FieldConstants.midLBumperPose));
Expand All @@ -140,19 +150,26 @@ public RobotContainer() {

NamedCommands.registerCommand(
"FerryOTM",
new ShootOnTheMove(swerve, turret, hood, shooter, ferryPoseSupplier, robotVisualization));
new ShootOnTheMove(
swerve, turret, hood, shooter, spindexer, ferryPoseSupplier, robotVisualization));

NamedCommands.registerCommand(
"IntakeUntilFull", Commands.waitUntil(() -> !robotVisualization.canSimIntake()));

NamedCommands.registerCommand(
"SOTM Until Empty",
new ShootOnTheMove(
swerve, turret, hood, shooter, AllianceUtil::getHubPose, robotVisualization)
swerve,
turret,
hood,
shooter,
spindexer,
AllianceUtil::getHubPose,
robotVisualization)
.until(() -> robotVisualization.isEmpty()));

NamedCommands.registerCommand(
"Shoot", Commands.run(() -> shooter.setSpeed(MetersPerSecond.of(1))).withTimeout(1));
"Shoot", Commands.run(() -> shooter.setGoalSpeed(MetersPerSecond.of(1))).withTimeout(1));

configureDriverBindings();
// configureOperatorBindings();
Expand All @@ -169,6 +186,9 @@ public RobotContainer() {
configureFuelSim();
}

SmartDashboard.putData(intake.zeroArmCommand());
SmartDashboard.putData(hood.zeroHoodCommand());

goalShotTarget = AllianceUtil.getHubPose();

inAllianceZoneTrigger.onTrue(
Expand Down Expand Up @@ -236,7 +256,13 @@ private void configureDriverBindings() {
.and(tooCloseToHubTrigger.negate())
.whileTrue(
new ShootOnTheMove(
swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization));
swerve,
turret,
hood,
shooter,
spindexer,
goalShotTargetSupplier,
robotVisualization));

swerve.setDefaultCommand(
new GuidedTeleopSwerve(
Expand All @@ -256,9 +282,11 @@ private void configureDriverBindings() {

hood.setDefaultCommand(hood.aimForTarget(goalShotTargetSupplier, swerve::getRobotPose));

intake.setDefaultCommand(intake.intakeToPosition(false));

shootButton.whileTrue(
new ShootOnTheMove(
swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization));
swerve, turret, hood, shooter, spindexer, goalShotTargetSupplier, robotVisualization));
}

private void configureOperatorBindings() {
Expand All @@ -268,15 +296,22 @@ private void configureOperatorBindings() {
// .and(shootButton.negate())
.whileTrue(
new ShootOnTheMove(
swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization));
swerve,
turret,
hood,
shooter,
spindexer,
goalShotTargetSupplier,
robotVisualization));

Trigger ferryMode = operatorController.leftTrigger();
// turret.setDefaultCommand(turret.faceTarget(AllianceUtil::getHubPose, swerve::getRobotPose));

hood.setDefaultCommand(hood.aimForTarget(AllianceUtil::getHubPose, swerve::getRobotPose));
// hood.setDefaultCommand(hood.aimForTarget(AllianceUtil::getHubPose, swerve::getRobotPose));

ferryMode.whileTrue(
new ShootOnTheMove(swerve, turret, hood, shooter, ferryPoseSupplier, robotVisualization));
new ShootOnTheMove(
swerve, turret, hood, shooter, spindexer, ferryPoseSupplier, robotVisualization));

operatorController
.y()
Expand Down
17 changes: 14 additions & 3 deletions src/main/java/frc/robot/commands/GuidedTeleopSwerve.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,12 +122,23 @@ private double getMaxTranslationalSpeed() {
}

private double getForwardSpeed() {
return forwardRateLimiter.calculate(
-forwardSupplier.getAsDouble() * getMaxTranslationalSpeed());
double forwardSpeed =
forwardRateLimiter.calculate(-forwardSupplier.getAsDouble() * getMaxTranslationalSpeed());
if (Math.abs(forwardSupplier.getAsDouble()) <= .065) {
forwardSpeed = 0;
forwardRateLimiter.reset(0);
}
return forwardSpeed;
}

private double getStrafeSpeed() {
return strafeRateLimiter.calculate(-strafeSupplier.getAsDouble() * getMaxTranslationalSpeed());
double stafeSpeed =
strafeRateLimiter.calculate(-strafeSupplier.getAsDouble() * getMaxTranslationalSpeed());
if (Math.abs(strafeSupplier.getAsDouble()) <= .065) {
stafeSpeed = 0;
strafeRateLimiter.reset(0);
}
return stafeSpeed;
}

private double getRotationSpeed() {
Expand Down
Loading
Loading