shooter jam prevention

This commit is contained in:
2026-03-26 12:06:41 -04:00
parent 429fa04f99
commit 2b464d2f32
4 changed files with 49 additions and 17 deletions

View File

@@ -5,15 +5,22 @@
"data": { "data": {
"commands": [ "commands": [
{ {
"type": "named", "type": "parallel",
"data": { "data": {
"name": "intake down" "commands": [
} {
}, "type": "path",
{ "data": {
"type": "path", "pathName": "left start to center"
"data": { }
"pathName": "left start to center" },
{
"type": "named",
"data": {
"name": "intake down"
}
}
]
} }
}, },
{ {

View File

@@ -5,15 +5,22 @@
"data": { "data": {
"commands": [ "commands": [
{ {
"type": "named", "type": "parallel",
"data": { "data": {
"name": "intake down" "commands": [
} {
}, "type": "path",
{ "data": {
"type": "path", "pathName": "left start to center"
"data": { }
"pathName": "left start to center" },
{
"type": "named",
"data": {
"name": "intake down"
}
}
]
} }
}, },
{ {

View File

@@ -301,7 +301,7 @@ public class RobotContainer {
driver.rightBumper().whileTrue(intakeRoller.runIn()); driver.rightBumper().whileTrue(intakeRoller.runIn());
driver.rightTrigger().whileTrue( driver.rightTrigger().whileTrue(
spindexer.spinToShooter().onlyWhile(() -> shooter.getAverageActualSpeeds()>2000).alongWith( spindexer.spinToShooter(shooter::getAverageActualSpeeds, 2000).alongWith(
intakeRoller.runIn(), intakeRoller.runIn(),
intakePivot.jimmy(.5) intakePivot.jimmy(.5)
) )

View File

@@ -1,5 +1,7 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.PersistMode; import com.revrobotics.PersistMode;
@@ -44,6 +46,22 @@ public class Spindexer extends SubsystemBase {
}); });
} }
public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM) {
return run(() -> {
if(shooterSpeedRPM.getAsDouble() < cutOffRPM) {
spindexerMotor.setControl(
spindexerMotorOutput.withOutput(0)
);
feederMotor.set(0);
} else {
spindexerMotor.setControl(
spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed)
);
feederMotor.set(SpindexerConstants.kFeederSpeed);
}
});
}
public Command spinToIntake() { public Command spinToIntake() {
return run(() -> { return run(() -> {
spindexerMotor.setControl( spindexerMotor.setControl(