Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/feat/elevator' into feat/elevator
Browse files Browse the repository at this point in the history
  • Loading branch information
@miyac22
miyac22 committed Sep 14, 2023
2 parents 80b26f2 + 57f717a commit 9fc91fe
Show file tree
Hide file tree
Showing 2 changed files with 65 additions and 55 deletions.
6 changes: 0 additions & 6 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,20 +18,15 @@
import edu.wpi.first.wpilibj.Filesystem;
import frc.robot.Constants.Drive.Dims;
import frc.robot.commands.ScoreCommand.ScoreStep;
import frc.robot.subsystems.ElevatorSubsystem;
import frc.robot.subsystems.ElevatorSubsystem.ElevatorState;

import frc.robot.subsystems.IntakeSubsystem.IntakeDetails;
import frc.robot.subsystems.NetworkWatchdogSubsystem.IPv4;
import frc.robot.subsystems.RGBSubsystem.RGBColor;
import frc.robot.subsystems.VisionSubsystem.TagCountDeviation;
import frc.robot.subsystems.VisionSubsystem.UnitDeviationParams;
import frc.util.CAN;
import frc.util.pathing.FieldObstructionMap;
import java.nio.file.Path;
import java.util.Lists;
import java.util.Map;
import java.util.Optional;
import java.util.Set;

@SuppressWarnings("java:S1118")
Expand Down Expand Up @@ -196,7 +191,6 @@ public static final class Ports {
public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(0, "elevator left motor");
public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(0, "elevator right motor");
public static final int WRIST_MOTOR_PORT = CAN.at(0, "wrist motor port");

}

public static final double MAX_HEIGHT = 10;
Expand Down
114 changes: 65 additions & 49 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,14 @@
import frc.robot.commands.EngageCommand;
import frc.robot.commands.HaltDriveCommandsCommand;
import frc.robot.commands.HashMapCommand;
import frc.robot.commands.IntakeModeCommand;
import frc.robot.commands.RotateVectorDriveCommand;
import frc.robot.commands.RotateVelocityDriveCommand;
import frc.robot.commands.ScoreCommand;
import frc.robot.commands.ScoreCommand.ScoreStep;
import frc.robot.commands.SetZeroModeCommand;
import frc.robot.commands.VibrateHIDCommand;
import frc.robot.commands.WristManualCommand;
import frc.robot.subsystems.CANWatchdogSubsystem;
import frc.robot.subsystems.DrivebaseSubsystem;
import frc.robot.subsystems.ElevatorSubsystem;
Expand Down Expand Up @@ -99,6 +101,8 @@ public class RobotContainer {

private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();

private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();

private final SharedReference<NodeSelection> currentNodeSelection =
new SharedReference<>(new NodeSelection(NodeSelectorUtility.defaultNodeStack, Height.HIGH));

Expand Down Expand Up @@ -140,9 +144,9 @@ public RobotContainer() {
will.leftBumper()));

// // FIXME: This error is here to kind of guide you...
// armSubsystem.setDefaultCommand(
// elevatorSubsystem.setDefaultCommand(
// new ArmManualCommand(
// armSubsystem,
// elevatorSubsystem,
// () -> ControllerUtil.deadband(-jason.getLeftY(), 0.2),
// () -> ControllerUtil.deadband(jason.getRightY(), 0.2)));

Expand Down Expand Up @@ -195,9 +199,9 @@ public void containerMatchStarting() {
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
Anthony.a().onTrue(new IntakeCommand(intakeSubsystem, IntakeMode.HOLD));
Anthony.b().onTrue(new IntakeCommand(intakeSubsystem, IntakeMode.INTAKE));
Anthony.x().onTrue(new IntakeCommand(intakeSubsystem, IntakeMode.OUTTAKE));
controller.a().onTrue(new IntakeModeCommand(intakeSubsystem, IntakeMode.HOLD));
controller.b().onTrue(new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE));
controller.x().onTrue(new IntakeModeCommand(intakeSubsystem, IntakeMode.OUTTAKE));

controller.leftTrigger().onTrue(new WristManualCommand(elevatorSubsystem, 0));
controller.rightTrigger().onTrue(new WristManualCommand(elevatorSubsystem, 20));
Expand Down Expand Up @@ -225,7 +229,7 @@ private void configureButtonBindings() {
will.pov(-1).whileFalse(new DefenseModeCommand(drivebaseSubsystem));

will.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem));
jason.leftStick().onTrue(new InstantCommand(() -> {}, armSubsystem));
jason.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem));

DoubleSupplier rotation =
exponential(
Expand Down Expand Up @@ -315,58 +319,63 @@ private void configureButtonBindings() {
jasonLayer
.off(jason.x())
.onTrue(new SetOuttakeModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.OFF))
.onTrue(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED));
.onTrue(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.HOLD));

// intake presets
// jasonLayer
// .off(jason.a())
// .onTrue(new ScoreCommand(outtakeSubsystem, armSubsystem, Setpoints.GROUND_INTAKE))
// .onTrue(new ScoreCommand(outtakeSubsystem, elevatorSubsystem, Setpoints.GROUND_INTAKE))
// .whileTrue(
// new ForceOuttakeSubsystemModeCommand(outtakeSubsystem,
// OuttakeSubsystem.Modes.INTAKE));

jasonLayer
.off(jason.b())
// FIXME: This error is here to kind of guide you...
.onTrue(new ElevatorPositionCommand(armSubsystem, Arm.Setpoints.SHELF_INTAKE))
.onTrue(
new ElevatorPositionCommand(elevatorSubsystem, Constants.Arm.Setpoints.SHELF_INTAKE))
.whileTrue(
new ForceOuttakeSubsystemModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.INTAKE));
new ForceOuttakeSubsystemModeCommand(
intakeSubsystem, IntakeSubsystem.IntakeMode.INTAKE));

// Reset arm position
jasonLayer
.off(jason.y())
// FIXME: This error is here to kind of guide you...
.onTrue(new ElevatorPositionCommand(armSubsystem, Arm.Setpoints.STOWED))
.onTrue(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED));
jason.start().onTrue(new SetZeroModeCommand(armSubsystem));
.onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED))
.onTrue(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.HOLD));
jason.start().onTrue(new SetZeroModeCommand(elevatorSubsystem));

jasonLayer
.off(jason.a())
.onTrue(
new GroundPickupCommand(
intakeSubsystem,
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
() ->
jason.getHID().getPOV() == 180
? IntakeSubsystem.Modes.INTAKE_LOW
: IntakeSubsystem.Modes.INTAKE));
? IntakeSubsystem.IntakeMode.INTAKE_LOW
: IntakeSubsystem.IntakeMode.INTAKE));

jason.start().onTrue(new ZeroIntakeCommand(intakeSubsystem));
jason.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem));

jason
.back()
.whileTrue(
new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)
new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.INTAKE)
// FIXME: This error is here to kind of guide you...
.alongWith(new ElevatorPositionCommand(armSubsystem, Arm.Setpoints.HANDOFF))
.alongWith(
new ElevatorPositionCommand(
elevatorSubsystem, Constants.Elevator.Setpoints.HANDOFF))
.alongWith(
new ForceOuttakeSubsystemModeCommand(
outtakeSubsystem, OuttakeSubsystem.Modes.OFF)))
outtakeSubsystem, IntakeSubsystem.IntakeMode.OFF)))
.onFalse(
// FIXME: This error is here to kind of guide you...
new ElevatorPositionCommand(armSubsystem, Arm.Setpoints.STOWED)
.alongWith(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED)));
new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)
.alongWith(
new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.HOLD)));

// scoring
// jasonLayer
Expand All @@ -385,15 +394,15 @@ private void configureButtonBindings() {
.onTrue(
new InstantCommand(
() -> currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.MID)),
armSubsystem));
elevatorSubsystem));

jasonLayer
.on(jason.y())
.onTrue(
new InstantCommand(
() ->
currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.HIGH)),
armSubsystem));
elevatorSubsystem));

var scoreCommandMap = new HashMap<NodeSelectorUtility.ScoreTypeIdentifier, Command>();

Expand All @@ -402,7 +411,7 @@ private void configureButtonBindings() {
scoreType,
new ScoreCommand(
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
Constants.SCORE_STEP_MAP.get(scoreType),
jason.leftBumper()));

Expand Down Expand Up @@ -453,15 +462,17 @@ private void setupAutonomousCommands() {
Map.of(
"stow arm",
"zero everything",
(new SetZeroModeCommand(armSubsystem))
.alongWith(new ZeroIntakeCommand(intakeSubsystem)),
(new SetZeroModeCommand(elevatorSubsystem))
.alongWith(new ZeroIntakeModeCommand(intakeSubsystem)),
"intake",
new GroundPickupCommand(
intakeSubsystem,
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
() ->
intakeLow[0] ? IntakeSubsystem.Modes.INTAKE_LOW : IntakeSubsystem.Modes.INTAKE),
intakeLow[0]
? IntakeSubsystem.IntakeMode.INTAKE_LOW
: IntakeSubsystem.IntakeMode.INTAKE),
"squeeze intake",
new CommandBase() {
private double lastTime = Timer.getFPGATimestamp();
Expand All @@ -483,29 +494,31 @@ public void end(boolean interrupted) {
}
},
"stage outtake",
new ScoreCommand(outtakeSubsystem, armSubsystem, drivingCubeOuttake.subList(0, 1), 1),
new ScoreCommand(
outtakeSubsystem, elevatorSubsystem, drivingCubeOuttake.subList(0, 1), 1),
"stage outtake high",
new ScoreCommand(
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
Constants.SCORE_STEP_MAP.get(NodeType.CUBE.atHeight(Height.HIGH)).subList(0, 1)),
"stage outtake mid",
new ScoreCommand(
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
Constants.SCORE_STEP_MAP.get(NodeType.CUBE.atHeight(Height.MID)).subList(0, 1)),

// FIXME: How can we get this working again?
"outtake",
new ScoreCommand(outtakeSubsystem, armSubsystem, drivingCubeOuttake.subList(1, 2), 1)
new ScoreCommand(
outtakeSubsystem, elevatorSubsystem, drivingCubeOuttake.subList(1, 2), 1)
.andThen(
new ElevatorPositionCommand(armSubsystem, Arm.Setpoints.STOWED)
new ElevatorPositionCommand(elevatorSubsystem, Arm.Setpoints.STOWED)
.andThen(
new SetOuttakeModeCommand(
outtakeSubsystem, OuttakeSubsystem.Modes.OFF))),
"armbat preload",
new ElevatorPositionCommand(armSubsystem, new ArmState(30, 0))
.andThen(new ElevatorPositionCommand(armSubsystem, Arm.Setpoints.STOWED)));
new ElevatorPositionCommand(elevatorSubsystem, new ArmState(30, 0))
.andThen(new ElevatorPositionCommand(elevatorSubsystem, Arm.Setpoints.STOWED)));

autoSelector.setDefaultOption(
"N1 1Cone + 2Cube Low Mobility Engage",
Expand All @@ -515,13 +528,13 @@ public void end(boolean interrupted) {
eventMap,
intakeSubsystem,
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
drivebaseSubsystem));

autoSelector.setDefaultOption(
"N1 1Cone + 2Cube Low Mobility NO ENGAGE",
new N1_1ConePlus2CubeHybridMobility(
4.95, 4, eventMap, outtakeSubsystem, armSubsystem, drivebaseSubsystem));
4.95, 4, eventMap, outtakeSubsystem, elevatorSubsystem, drivebaseSubsystem));

autoSelector.setDefaultOption(
"N9 1Cone + 1Cube + Grab Cube Mobility",
Expand All @@ -531,19 +544,19 @@ public void end(boolean interrupted) {
eventMap,
intakeSubsystem,
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
drivebaseSubsystem));

autoSelector.addOption(
"Just Zero Arm [DOES NOT CALIBRATE]", new SetZeroModeCommand(armSubsystem));
"Just Zero Arm [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem));

autoSelector.addOption(
"Near Substation Mobility [APRILTAG]",
new MobilityAuto(
manueverGenerator,
drivebaseSubsystem,
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
rgbSubsystem,
new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0))));

Expand All @@ -553,40 +566,43 @@ public void end(boolean interrupted) {
manueverGenerator,
drivebaseSubsystem,
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
rgbSubsystem,
new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0))));

autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem));

autoSelector.addOption(
"N3 1Cone + Mobility Engage",
new N3_1ConePlusMobilityEngage(5, 3.5, outtakeSubsystem, armSubsystem, drivebaseSubsystem));
new N3_1ConePlusMobilityEngage(
5, 3.5, outtakeSubsystem, elevatorSubsystem, drivebaseSubsystem));

autoSelector.setDefaultOption(
"N3 1Cone + Mobility",
new N3_1ConePlusMobility(4.95, 3.5, outtakeSubsystem, armSubsystem, drivebaseSubsystem));
new N3_1ConePlusMobility(
4.95, 3.5, outtakeSubsystem, elevatorSubsystem, drivebaseSubsystem));

autoSelector.setDefaultOption(
"N6 1Cone + Engage",
new N6_1ConePlusEngage(5, 3.5, outtakeSubsystem, armSubsystem, drivebaseSubsystem));
new N6_1ConePlusEngage(5, 3.5, outtakeSubsystem, elevatorSubsystem, drivebaseSubsystem));

autoSelector.addOption(
"N9 1Cone + Mobility Engage",
new N9_1ConePlusMobilityEngage(5, 3.5, outtakeSubsystem, armSubsystem, drivebaseSubsystem));
new N9_1ConePlusMobilityEngage(
5, 3.5, outtakeSubsystem, elevatorSubsystem, drivebaseSubsystem));

autoSelector.addOption(
"N9 1Cone + Mobility",
new N9_1ConePlusMobility(4.95, 3, outtakeSubsystem, armSubsystem, drivebaseSubsystem));
new N9_1ConePlusMobility(4.95, 3, outtakeSubsystem, elevatorSubsystem, drivebaseSubsystem));

autoSelector.addOption(
"Score High Cone [DOES NOT CALIBRATE]",
new SetZeroModeCommand(armSubsystem)
new SetZeroModeCommand(elevatorSubsystem)
.raceWith(new SetOuttakeModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.INTAKE))
.andThen(
new ScoreCommand(
outtakeSubsystem,
armSubsystem,
elevatorSubsystem,
Constants.SCORE_STEP_MAP.get(
NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH)))));

Expand Down

0 comments on commit 9fc91fe

Please sign in to comment.