Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 14 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -32,10 +32,11 @@
import frc.robot.commands.intakeDeployment.RunDeployer;
import frc.robot.commands.intakeDeployment.SetDeploymentState;
import frc.robot.commands.parallels.RunHopperAndFeeder;
import frc.robot.commands.sequences.ResetAll;
import frc.robot.autochooser.AutoChooser;
import frc.robot.commands.angler.AimAngler;
import frc.robot.commands.angler.DefaultAnglerControl;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.StowAngler;
import frc.robot.commands.shooter.DefaultShooterControl;
import frc.robot.commands.auto.ExampleAuto;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -299,6 +300,9 @@ private void configureBindings() {
controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB));
controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING));
driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED)));
controller.leftTrigger().onTrue((new ResetAll(anglerSubsystem, climberSubsystem, feederSubsystem,
hopperSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem, turretSubsystem, shootState)));

if (controllerSubsystem != null) {
steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem));
}
Expand Down Expand Up @@ -419,8 +423,16 @@ public void putShuffleboardCommands() {

SmartDashboard.putData(
"angler/Home Rev (Reset)",
new RunAnglerToReverseLimit(anglerSubsystem));
new StowAngler(anglerSubsystem));

SmartDashboard.putData("stop Intake",
new StopIntake(intakeSubsystem));

SmartDashboard.putData(
"reset All",
new ResetAll(anglerSubsystem, climberSubsystem, feederSubsystem,
hopperSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem,
turretSubsystem, shootState));
SmartDashboard.putData(
"turret/TurretTest/Turret Go 45",
new SetTurretAngle(turretSubsystem, 45));
Expand Down
24 changes: 24 additions & 0 deletions src/main/java/frc/robot/commands/angler/ResetAnglerEncoder.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
package frc.robot.commands.angler;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure why this was added, we don't want to use this because it doesn't move the mechanism to the bottom position. the correct command is stowAngler. this should be removed from the PR.


import frc.robot.subsystems.AnglerSubsystem;
import frc.robot.utils.logging.commands.LoggableCommand;

public class ResetAnglerEncoder extends LoggableCommand {

private final AnglerSubsystem subsystem;

public ResetAnglerEncoder(AnglerSubsystem subsystem) {
this.subsystem = subsystem;
addRequirements(subsystem);
}

@Override
public void initialize() {
subsystem.resetEncoderToZero();
}

@Override
public boolean isFinished() {
return true;
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,12 @@
/**
* Runs the angler to the reverse limit switch and resets the encoder to zero.
*/
public class RunAnglerToReverseLimit extends LoggableCommand {
public class StowAngler extends LoggableCommand {
private final TimeoutLogger timeoutCounter;
private final AnglerSubsystem angler;
private final Timer timer = new Timer();

public RunAnglerToReverseLimit(AnglerSubsystem angler) {
public StowAngler(AnglerSubsystem angler) {
timeoutCounter = new TimeoutLogger(getName());
this.angler = angler;
addRequirements(angler);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public BlueDepotShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is incorrect, it has to be stowAngler. the same is true for all the autonomous commands. I'm not sure why this was even changed in this PR, it shouldn't be part of it. This will create conflicts to Michael who is working on the autonomous commands.
I think it would be best to not rename back StowAngler to RunAnglerToReverseLimit and undo all the changes that are related to this.

),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueDepotShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public BlueMidShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueMidShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public BlueOutpostShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueOutpostShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public RedDepotShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedDepotShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public RedMidShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedMidShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public RedOutpostShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedOutpostShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public BlueDepotShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueDepotShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public BlueMidShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueMidShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public BlueOutpostShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueOutpostShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public RedDepotShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedDepotShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public RedMidShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedMidShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public RedOutpostShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedOutpostShoot")),
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
// Copyright (c) FIRST and other WPILib contributors.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is this part of this PR?
In fact, the entire intake deployment process changed and this class is no longer in main.

// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands.intakeDeployment;
import edu.wpi.first.wpilibj.Timer;
import frc.robot.constants.Constants;
import frc.robot.subsystems.IntakeDeployerSubsystem;
import frc.robot.utils.logging.commands.LoggableCommand;

public class InitialDeploymentState extends LoggableCommand {
private final IntakeDeployerSubsystem subsystem;
private final Timer timer;

public InitialDeploymentState(IntakeDeployerSubsystem subsystem) {
timer = new Timer();
this.subsystem = subsystem;
addRequirements(subsystem);
}

@Override
public void initialize() {
timer.restart();
}

@Override
public void execute() {
switch (subsystem.getDeploymentState()) {
case UP -> subsystem.setSpeed(Constants.INITIAL_INTAKE_RETRACTION_SPEED);
case DOWN -> subsystem.setSpeed(Constants.INITIAL_INTAKE_DEPLOYMENT_SPEED);
case STOPPED -> subsystem.stopMotors();
default -> subsystem.stopMotors();
}
}

@Override
public void end(boolean interrupted) {
}

@Override
public boolean isFinished() {
return timer.hasElapsed(Constants.INTAKE_DEPLOYER_BURNOUT_TIMER);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands.intakeDeployment;

import edu.wpi.first.wpilibj.Timer;
import frc.robot.constants.Constants;
import frc.robot.subsystems.IntakeDeployerSubsystem;
Expand Down
39 changes: 39 additions & 0 deletions src/main/java/frc/robot/commands/sequences/ResetAll.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
package frc.robot.commands.sequences;

import frc.robot.commands.angler.StowAngler;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.intakeDeployment.InitialDeploymentState;
import frc.robot.commands.intakeDeployment.SetDeploymentState;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.shooter.StopShooter;
import frc.robot.commands.turret.RunTurretToRevLimit;
import frc.robot.constants.enums.DeploymentState;
import frc.robot.constants.enums.ShootingState;
import frc.robot.constants.enums.ShootingState.ShootState;
import frc.robot.subsystems.AnglerSubsystem;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.subsystems.FeederSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeDeployerSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.TurretSubsystem;
import frc.robot.utils.logging.commands.LoggableParallelCommandGroup;
import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup;

public class ResetAll extends LoggableSequentialCommandGroup {

public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem,
FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem,
IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem,
ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) {
super(
new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP),
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

the intake deployment mechanism changed, please talk to Jackson about what needs to be done here.

new LoggableParallelCommandGroup(
new InitialDeploymentState(intakeDeployerSubsystem),
new StowAngler(anglerSubsystem),
new ClimberDown(climberSubsystem),
new SetShootingState(shootState, ShootState.STOPPED),
new RunTurretToRevLimit(turretSubsystem)));
}
}
Loading