Team_2648/2026-Robot-Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Work from 3/19 Meeting

Browse Source
This commit is contained in:
Bradley Bickford
2026-03-19 18:54:52 -04:00
parent 235f43fd2e
commit 7e02ec1ccc
5 changed files with 83 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
src/main/java/frc/robot/RobotContainer.java
View File

@@ -5,6 +5,7 @@
package frc.robot;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import java.util.Optional;
import java.util.OptionalDouble;
@@ -27,6 +28,7 @@ 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.Commands;
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -89,6 +91,7 @@ public class RobotContainer {
vp.visualPose()
);
});
vision.addPoseEstimateConsumer((vp) -> {
if(resetOdometryToVisualPose) {
drivetrain.resetOdometry(vp.visualPose());
@@ -96,7 +99,6 @@ public class RobotContainer {
}
});
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
@@ -104,8 +106,9 @@ public class RobotContainer {
shiftTimer.reset();
configureBindings();
//testConfigureBindings();
configureShiftDisplay();
//testConfigureBindings();
if(AutoConstants.kAutoConfigOk) {
autoChooser = AutoBuilder.buildAutoChooser();
@@ -285,7 +288,17 @@ public class RobotContainer {
driver.leftBumper().whileTrue(intakeRoller.runOut());
driver.rightBumper().whileTrue(intakeRoller.runIn());
driver.rightTrigger().whileTrue(spindexer.spinToShooter());
driver.rightTrigger().whileTrue(
spindexer.spinToShooter().alongWith(
intakeRoller.runIn(),
intakePivot.jimmy(.5)
)
);
driver.rightTrigger().onFalse(
intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5)
);
driver.b().whileTrue(spindexer.spinToIntake());
/* driver.b().whileTrue(
drivetrain.lockToYaw(
@@ -310,13 +323,6 @@ public class RobotContainer {
secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
//10 degrees good for shooting ~33in away from hub
secondary.y().whileTrue(
intakePivot.jimmy(.2)
.andThen(
intakePivot.manualSpeed(() -> -0.5)
)
);
hood.setDefaultCommand(hood.trackToAngle(() -> {
Pose2d drivetrainPose = drivetrain.getPose();
Pose2d hubPose = Utilities.getHubPose();
@@ -340,6 +346,24 @@ public class RobotContainer {
return 0;
}
}));
new Trigger(() -> MathUtil.isNear(
shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(),
shooter.getAverageActualSpeeds(),
150)).onTrue(
new FunctionalCommand(
() -> {},
() -> {
driver.setRumble(RumbleType.kBothRumble, .75);
secondary.setRumble(RumbleType.kBothRumble, .75);
},
(b) -> {
driver.setRumble(RumbleType.kBothRumble, 0);
secondary.setRumble(RumbleType.kBothRumble, 0);
},
() -> false
).withTimeout(5)
);
}
private void configureNamedCommands() {

1
src/main/java/frc/robot/constants/ShooterConstants.java
View File

@@ -1,5 +1,6 @@
package frc.robot.constants;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;

17
src/main/java/frc/robot/subsystems/Hood.java
View File

@@ -12,7 +12,9 @@ import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -68,6 +70,21 @@ public class Hood extends SubsystemBase {
@Override
public void periodic() {
SmartDashboard.putNumber(
"HoodTargetDegrees",
Math.toDegrees(currentTargetDegrees)
);
SmartDashboard.putNumber(
"HoodCurrentAngle",
Math.toDegrees(encoder.getPosition())
);
SmartDashboard.putBoolean(
"HoodAtSetpoint",
controller.isAtSetpoint()
);
Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));

8
src/main/java/frc/robot/subsystems/IntakePivot.java
View File

@@ -89,8 +89,8 @@ public class IntakePivot extends SubsystemBase {
*/
public Command jimmy(double time){
return Commands.repeatingSequence(
manualSpeed(() -> -0.75).withTimeout(time)
.andThen(manualSpeed(() -> 0.75).withTimeout(time))
manualSpeed(() -> -0.75).withTimeout(time),
manualSpeed(() -> 0.75).withTimeout(time)
);
}
@@ -102,8 +102,8 @@ public class IntakePivot extends SubsystemBase {
*/
public Command jimmy(DoubleSupplier time) {
return Commands.repeatingSequence(
manualSpeed(() -> -0.75).withTimeout(time.getAsDouble())
.andThen(manualSpeed(() -> 0.75).withTimeout(time.getAsDouble()))
manualSpeed(() -> -0.75).withTimeout(time.getAsDouble()),
manualSpeed(() -> 0.75).withTimeout(time.getAsDouble())
);
}

27
src/main/java/frc/robot/subsystems/Shooter.java
View File

@@ -14,6 +14,7 @@ import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ShooterConstants;
@@ -63,6 +64,30 @@ public class Shooter extends SubsystemBase {
@Override
public void periodic() {
SmartDashboard.putNumber(
"ShooterTargetRPM",
targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM());
SmartDashboard.putNumber(
"ShooterLeftSideRPM",
leftEncoder.getVelocity()
);
SmartDashboard.putNumber(
"ShooterRightSideRPM",
rightEncoder.getVelocity()
);
SmartDashboard.putBoolean(
"ShooterLeftSideAtSetpoint",
leftClosedLoopController.isAtSetpoint()
);
SmartDashboard.putBoolean(
"ShooterRightSideAtSetpoint",
rightClosedLoopController.isAtSetpoint()
);
Logger.recordOutput(
"Shooter/TargetRPM",
targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM()
@@ -73,6 +98,8 @@ public class Shooter extends SubsystemBase {
Logger.recordOutput("Shooter/RightRollers/rightmotor", rightRelative.getVelocity());
Logger.recordOutput("Shooter/LeftRollers/OutputVoltage", leftMotor.getAppliedOutput() * leftMotor.getBusVoltage());
Logger.recordOutput("Shooter/RightRollers/OutputVoltage", rightMotor.getAppliedOutput() * rightMotor.getBusVoltage());
// TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());