shooter jam prevention
This commit is contained in:
@@ -5,17 +5,24 @@
|
|||||||
"data": {
|
"data": {
|
||||||
"commands": [
|
"commands": [
|
||||||
{
|
{
|
||||||
"type": "named",
|
"type": "parallel",
|
||||||
"data": {
|
"data": {
|
||||||
"name": "intake down"
|
"commands": [
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"type": "path",
|
"type": "path",
|
||||||
"data": {
|
"data": {
|
||||||
"pathName": "left start to center"
|
"pathName": "left start to center"
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "intake down"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
{
|
{
|
||||||
"type": "parallel",
|
"type": "parallel",
|
||||||
"data": {
|
"data": {
|
||||||
|
|||||||
@@ -5,17 +5,24 @@
|
|||||||
"data": {
|
"data": {
|
||||||
"commands": [
|
"commands": [
|
||||||
{
|
{
|
||||||
"type": "named",
|
"type": "parallel",
|
||||||
"data": {
|
"data": {
|
||||||
"name": "intake down"
|
"commands": [
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"type": "path",
|
"type": "path",
|
||||||
"data": {
|
"data": {
|
||||||
"pathName": "left start to center"
|
"pathName": "left start to center"
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "intake down"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
{
|
{
|
||||||
"type": "parallel",
|
"type": "parallel",
|
||||||
"data": {
|
"data": {
|
||||||
|
|||||||
@@ -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)
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -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(
|
||||||
|
|||||||
Reference in New Issue
Block a user