shooter jam prevention
This commit is contained in:
@@ -5,15 +5,22 @@
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"name": "intake down"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "left start to center"
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "left start to center"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intake down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -5,15 +5,22 @@
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"name": "intake down"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "left start to center"
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "left start to center"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intake down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -301,7 +301,7 @@ public class RobotContainer {
|
||||
driver.rightBumper().whileTrue(intakeRoller.runIn());
|
||||
|
||||
driver.rightTrigger().whileTrue(
|
||||
spindexer.spinToShooter().onlyWhile(() -> shooter.getAverageActualSpeeds()>2000).alongWith(
|
||||
spindexer.spinToShooter(shooter::getAverageActualSpeeds, 2000).alongWith(
|
||||
intakeRoller.runIn(),
|
||||
intakePivot.jimmy(.5)
|
||||
)
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.ctre.phoenix6.controls.DutyCycleOut;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
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() {
|
||||
return run(() -> {
|
||||
spindexerMotor.setControl(
|
||||
|
||||
Reference in New Issue
Block a user