{
"name": "Constraints Zone",
"minWaypointRelativePos": 0.7761529808773961,
- "maxWaypointRelativePos": 1.642294713160863,
+ "maxWaypointRelativePos": 2.2491730981256937,
"constraints": {
"maxVelocity": 1.0,
"maxAcceleration": 2.0,
}
],
"pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Stop Hood Down",
- "waypointRelativePos": 4.968503937007888,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Stop Hood Down"
- }
- }
- },
- {
- "name": "Start Spindexer",
- "waypointRelativePos": 5.0,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Start Spindexer"
- }
- }
- }
- ],
+ "eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 2.5,
},
"prevControl": {
"x": 8.155370348167532,
- "y": 0.8386737269319345
+ "y": 0.8386737269319344
},
"nextControl": {
"x": 8.45722898958081,
{
"name": "Constraints Zone",
"minWaypointRelativePos": 0.7761529808773961,
- "maxWaypointRelativePos": 1.642294713160863,
+ "maxWaypointRelativePos": 2.2491730981256826,
"constraints": {
"maxVelocity": 1,
"maxAcceleration": 2,
}
],
"pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Stop Hood Down",
- "waypointRelativePos": 4.968503937007888,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Stop Hood Down"
- }
- }
- },
- {
- "name": "Start Spindexer",
- "waypointRelativePos": 5,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Start Spindexer"
- }
- }
- }
- ],
+ "eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3,
"maxAcceleration": 2.5,
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import frc.robot.commands.DoNothing;
import frc.robot.commands.LogCommand;
+import frc.robot.commands.auto_comm.DynamicAutoBuilder;
import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.commands.gpm.IntakeMovementCommand;
import frc.robot.commands.gpm.LockedShoot;
}
}
+ public void addAuto(String name, Command auto) {
+ try {
+ autoChooser.addOption(name, auto);
+ } catch (AutoBuilderException e){
+ e.printStackTrace();
+ System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND");
+ }
+ }
+
/**
* Initialize the SendableChooser on the SmartDashboard.
* Fill the SendableChooser with available Commands.
addAuto(rightDoNothing);
addAuto(centerDoNothing);
+ DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake, breaker);
+
+ String leftDynamicDoubleSwipe = "LeftDynamicDoubleLiberalSwipe";
+ addAuto(leftDynamicDoubleSwipe, dynamicAutoBuilder.getLeftDynamicDoubleLiberalSwipe());
+
// put the Chooser on the SmartDashboard
SmartDashboard.putData("Auto chooser", autoChooser);
}
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.Constants;
+import frc.robot.subsystems.spindexer.Spindexer;
import frc.robot.util.Elastic;
import frc.robot.util.HubActive;
import frc.robot.util.Elastic.Notification;
@Override
public void execute() {
boolean current = HubActive.isHubActive();
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("HubActive", current);
- }
+ Logger.recordOutput("HubActive", current);
if (current && !hubActive) {
Elastic.sendNotification(new Notification(NotificationLevel.INFO, "HUB ACTIVE", ""));
var timer = new Timer();
timer.start();
- return new RunSpindexer(spindexer, turret, hood, intake).raceWith(
- new WaitUntilCommand(() -> breaker.getAverageCurrentDraw(BreakerConstants.SPINDEXER_PORTS) < 5.0 && timer.hasElapsed(3.0)));
+ // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(
+ // new WaitUntilCommand(() -> spindexer.spinningAir() && timer.hasElapsed(3.0)));
+ // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new WaitCommand(3.0));
+ return new RunSpindexer(spindexer, turret, hood, intake).until(() -> spindexer.spinningAir() && timer.hasElapsed(3.0));
+
// return new ParallelDeadlineGroup(new WaitUntilCommand(() ->
// spindexer.spinningAir()), new RunSpindexer(spindexer, turret, hood, intake));
package frc.robot.commands.gpm;
+import org.littletonrobotics.junction.Logger;
+
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.filter.Debouncer.DebounceType;
import edu.wpi.first.wpilibj.Timer;
return; // this is so the balls don't fly out when unaligned
}
boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD;
+ Logger.recordOutput("SpindexerJammed", jammed);
if (jam_debouncer.calculate(jammed)) {
+ Logger.recordOutput("SpindexerJammedDebounced", jammed);
+
reversing = true;
reverseTimer.reset();
reverseTimer.start();
--- /dev/null
+package frc.robot.commands.gpm;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.Intake.IntakeConstants;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.spindexer.SpindexerConstants;
+import frc.robot.subsystems.turret.Turret;
+
+public class RunSpindexerWithStop extends Command {
+ private Spindexer spindexer;
+ private Turret turret;
+ private Hood hood;
+ private Intake intake;
+
+ private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising); // if there is jam I would think this is 0 -> 1
+
+ private boolean reversing = false;
+ private boolean wasHoodForcedDown = false;
+
+ private Timer reverseTimer = new Timer();
+
+ private double storedIntakeSpeed = 0.0;
+
+
+ public RunSpindexerWithStop(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
+ this.spindexer = spindexer;
+ this.turret = turret;
+ this.hood = hood;
+ this.intake = intake;
+
+ addRequirements(spindexer);
+ }
+
+ // public RunSpindexer(Spindexer spindexer) {
+ // this.spindexer = spindexer;
+ // addRequirements(spindexer);
+ // }
+
+ @Override
+ public void initialize() {
+ wasHoodForcedDown = hood.getHoodForcedDown();
+ timer.reset();
+ timer.start();
+ }
+
+ @Override
+ public void execute() {
+ boolean hoodForcedDown = hood.getHoodForcedDown();
+
+ if (wasHoodForcedDown && !hoodForcedDown) {
+ spindexer.maxSpindexer();
+ }
+ wasHoodForcedDown = hoodForcedDown;
+
+ if (!turret.atSetpoint() || hoodForcedDown || spindexer.noIndexing) {
+ spindexer.stopSpindexer();
+ reversing = false;
+ return; // this is so the balls don't fly out when unaligned
+ }
+ boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD;
+ Logger.recordOutput("SpindexerJammed", jammed);
+ if (jam_debouncer.calculate(jammed)) {
+ Logger.recordOutput("SpindexerJammedDebounced", jammed);
+
+ reversing = true;
+ reverseTimer.reset();
+ reverseTimer.start();
+ storedIntakeSpeed = intake.getSpeed();
+ }
+ if (!reversing) {
+ spindexer.maxSpindexer();
+ } else {
+ spindexer.reverseSpindexer();
+
+ if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) {
+ intake.spinReverse();
+ } else {
+ intake.extend();
+ }
+
+ if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) {
+ reversing = false;
+ intake.spin(storedIntakeSpeed);
+ }
+ }
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putBoolean("Spindexer Jamming", reversing);
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ spindexer.stopSpindexer();
+ reversing = false;
+ }
+
+ private Timer timer = new Timer();
+
+ @Override
+ public boolean isFinished() {
+ return spindexer.spinningAir() && timer.hasElapsed(1.0);
+ }
+}
if (!Constants.DISABLE_SMART_DASHBOARD) {
SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE);
}
+
+ Logger.recordOutput("HasBalls", spinningAir());
+
}
public void setMotorVoltages(double voltage) {
return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent;
}
+ public double getMotorOneStatorCurrent() {
+ return inputs.spindexerOneStatorCurrent;
+ }
+
+ public double getMotorTwoStatorCurrent() {
+ return inputs.spindexerTwoStatorCurrent;
+ }
+
public double getSubsystemSupplyCurrent() {
return inputs.spindexerOneSupplyCurrent + inputs.spindexerTwoSupplyCurrent;
}
+ public boolean spinningAir() {
+ return getMotorOneStatorCurrent() < 16.0 && getMotorTwoStatorCurrent() < 28.0;
+ }
+
@Override
public void updateInputs() {
inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble();
public static final double CURRENT_FORWARD_STATOR_LIMIT = 150.0;
public static final double CURRENT_REVERSE_STATOR_LIMIT = 20.0;
public static final double CURRENT_TIME_LIMIT = 1.0; //s
- public static final double JAM_CURRENT_THRESHOLD = 75.0; // A
+ public static final double JAM_CURRENT_THRESHOLD = 40.0; // A
public static final double JAM_DEBOUNCE_TIME = 0.3; // seconds
public static final double REVERSE_DEBOUNCE_TIME = 0.25; // seconds
}