Skip to content

Commit a4c1475

Browse files
ReeledWarrior14ReeledWarrior14
authored
Merge branch 'main' into led-subsystem
2 parents 5fa991e + 5e631bf commit a4c1475

File tree

10 files changed

+198
-130
lines changed

10 files changed

+198
-130
lines changed

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -253,7 +253,7 @@ public static final class EndEffectorConstants{
253253

254254
public static final double kPivotTolerance = 0.05; // pivot tolerance in degrees
255255

256-
public static final double kSensorDistanceThreshold = .1; // meters, TODO: tune
256+
public static final double kSensorDistanceThreshold = 0.1; // meters, TODO: tune
257257

258258
public static final double kMinAlgaeExtension = 0.3;
259259

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,8 @@
1111
import edu.wpi.first.math.MathUtil;
1212
import edu.wpi.first.math.geometry.Pose2d;
1313
import edu.wpi.first.wpilibj.DriverStation;
14-
import edu.wpi.first.wpilibj.XboxController;
1514
import edu.wpi.first.wpilibj.DriverStation.Alliance;
15+
import edu.wpi.first.wpilibj.XboxController;
1616
import edu.wpi.first.wpilibj.XboxController.Button;
1717
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
1818
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -25,18 +25,17 @@
2525
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
2626
import edu.wpi.first.wpilibj2.command.StartEndCommand;
2727
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
28-
import frc.robot.Constants.AutonConstants;
2928
import edu.wpi.first.wpilibj2.command.button.POVButton;
3029
import edu.wpi.first.wpilibj2.command.button.Trigger;
30+
import frc.robot.Constants.AutonConstants;
3131
import frc.robot.Constants.DriveConstants;
3232
import frc.robot.Constants.ElevatorConstants;
3333
import frc.robot.Constants.IOConstants;
3434
import frc.robot.commands.AutonCommands;
35+
import frc.robot.commands.IntakeOuttakeCoralCommand;
3536
import frc.robot.commands.DriveToReef;
3637
import frc.robot.commands.ElevatorSemiAutomaticDriveCommand;
3738
import frc.robot.commands.HapticCommand;
38-
import frc.robot.commands.PlaceGrabCoralCommand;
39-
import frc.robot.commands.auton.DriveForwardsL1;
4039
import frc.robot.commands.scoring.BargeFlipCommand;
4140
import frc.robot.commands.scoring.L1Command;
4241
import frc.robot.commands.scoring.L2Command;
@@ -47,6 +46,7 @@
4746
import frc.robot.subsystems.ElevatorSubsystem;
4847
import frc.robot.subsystems.EndEffectorSubsystem;
4948
import frc.robot.subsystems.LEDSubsystem;
49+
import frc.robot.subsystems.EndEffectorSubsystem.IntakeState;
5050
import frc.robot.utils.Interlocks;
5151

5252
/*
@@ -212,7 +212,7 @@ private void configureBindings() {
212212

213213
// operator hold to intake coral (or weakly outtake algae)
214214
new JoystickButton(m_operatorController, Button.kA.value)
215-
.onTrue(new InstantCommand(m_endEffector::intakeCoral, m_endEffector))
215+
.onTrue(new InstantCommand(m_endEffector::forceCoral, m_endEffector))
216216
.onFalse(new InstantCommand(m_endEffector::stopEffector, m_endEffector));
217217

218218
// operator hold to outtake coral (or weakly intake algae)
@@ -245,7 +245,7 @@ private void configureBindings() {
245245
.whileTrue(new ConditionalCommand(
246246
// coral
247247
new SequentialCommandGroup(
248-
new PlaceGrabCoralCommand(m_endEffector, false),
248+
new IntakeOuttakeCoralCommand(m_endEffector, IntakeState.IntakeCoral),
249249
new ParallelCommandGroup(
250250
new HapticCommand(m_driverController),
251251
new InstantCommand(() -> m_ledSubsystem.tripleBlink(0, 255, 0, "Intookened"), m_ledSubsystem),
@@ -258,7 +258,7 @@ private void configureBindings() {
258258
new Trigger(() -> m_driverController.getRightTriggerAxis() > IOConstants.kControllerDeadband)
259259
.whileTrue(new ConditionalCommand(
260260
// coral
261-
new PlaceGrabCoralCommand(m_endEffector, true),
261+
new IntakeOuttakeCoralCommand(m_endEffector, IntakeState.OuttakeCoral),
262262
// algae
263263
new StartEndCommand(m_endEffector::outtakeAlgae, m_endEffector::stopEffector),
264264
() -> m_coralMode));

‎src/main/java/frc/robot/commands/AutonCommands.java‎

Lines changed: 29 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,19 @@
44

55
import edu.wpi.first.wpilibj2.command.Command;
66
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
7-
import frc.robot.commands.scoring.L4Command;
87
import frc.robot.commands.scoring.coral.CoralL1Command;
98
import frc.robot.commands.scoring.coral.CoralL4Command;
109
import frc.robot.subsystems.ElevatorSubsystem;
1110
import frc.robot.subsystems.EndEffectorSubsystem;
11+
import frc.robot.subsystems.EndEffectorSubsystem.IntakeState;
1212

1313
public class AutonCommands {
1414
private static ElevatorSubsystem m_elevator;
1515
private static EndEffectorSubsystem m_endEffector;
1616

1717
/**
1818
* Call before registering commands
19+
*
1920
* @param elevator
2021
* @param endEffector
2122
*/
@@ -25,37 +26,33 @@ public static void setSubsystems(ElevatorSubsystem elevator, EndEffectorSubsyste
2526
}
2627

2728
public static enum Commands {
28-
// TODO: fill stubs
29-
GET_CORAL(
30-
new SequentialCommandGroup(new PlaceGrabCoralCommand(m_endEffector, false))),
31-
PLACE_CORAL_L4(
32-
new SequentialCommandGroup()),
33-
RAISE_ELEVATOR_L4(
34-
new SequentialCommandGroup(new CoralL4Command(m_endEffector, m_elevator))),
35-
36-
OUTTAKE_CORAL(
37-
new SequentialCommandGroup(new PlaceGrabCoralCommand(m_endEffector, true))
38-
),
39-
LOWER_ELEVATOR(
40-
new SequentialCommandGroup(new CoralL1Command(m_endEffector, m_elevator))
41-
),
42-
43-
GET_ALGAE(
44-
new SequentialCommandGroup()),
45-
PLACE_ALGAE(
46-
new SequentialCommandGroup()),
47-
;
48-
49-
private final Command m_command;
50-
51-
private Commands(Command command) {
52-
m_command = command;
53-
}
54-
55-
public static void registerAutonCommands() {
56-
for (Commands command : values()) {
57-
NamedCommands.registerCommand(command.name(), command.m_command);
58-
}
29+
GET_CORAL(
30+
new SequentialCommandGroup(new IntakeOuttakeCoralCommand(m_endEffector, IntakeState.IntakeCoral))),
31+
PLACE_CORAL_L4(
32+
new SequentialCommandGroup()),
33+
RAISE_ELEVATOR_L4(
34+
new SequentialCommandGroup(new CoralL4Command(m_endEffector, m_elevator))),
35+
36+
OUTTAKE_CORAL(
37+
new SequentialCommandGroup(new IntakeOuttakeCoralCommand(m_endEffector, IntakeState.OuttakeCoral))),
38+
LOWER_ELEVATOR(
39+
new SequentialCommandGroup(new CoralL1Command(m_endEffector, m_elevator))),
40+
41+
GET_ALGAE(
42+
new SequentialCommandGroup()),
43+
PLACE_ALGAE(
44+
new SequentialCommandGroup());
45+
46+
private final Command m_command;
47+
48+
private Commands(Command command) {
49+
m_command = command;
50+
}
51+
52+
public static void registerAutonCommands() {
53+
for (Commands command : values()) {
54+
NamedCommands.registerCommand(command.name(), command.m_command);
5955
}
56+
}
6057
}
6158
}

‎src/main/java/frc/robot/commands/IntakeOuttakeCoralCommand.java‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.commands;
6+
7+
import edu.wpi.first.wpilibj2.command.Command;
8+
import frc.robot.Robot;
9+
import frc.robot.subsystems.EndEffectorSubsystem;
10+
import frc.robot.subsystems.EndEffectorSubsystem.IntakeState;
11+
12+
public class IntakeOuttakeCoralCommand extends Command {
13+
14+
EndEffectorSubsystem m_endEffectorSubsystem;
15+
IntakeState m_intakeState;
16+
17+
/** Creates a new CoralCommand. */
18+
public IntakeOuttakeCoralCommand(EndEffectorSubsystem endEffectorSubsystem, IntakeState intakeState) {
19+
/*
20+
* We aren't requiring the end effector subsystem here because moving the
21+
* intake wheels does not affect the safety of the bot in any other way,
22+
* and this allows us to also simultaneously run commands that pivot the end
23+
* effector
24+
*/
25+
26+
m_endEffectorSubsystem = endEffectorSubsystem;
27+
m_intakeState = intakeState;
28+
}
29+
30+
// Called when the command is initially scheduled.
31+
@Override
32+
public void initialize() {
33+
m_endEffectorSubsystem.setIntakeState(m_intakeState);
34+
}
35+
36+
// Called every time the scheduler runs while the command is scheduled.
37+
@Override
38+
public void execute() {
39+
}
40+
41+
// Called once the command ends or is interrupted.
42+
@Override
43+
public void end(boolean interrupted) {
44+
m_endEffectorSubsystem.setIntakeState(IntakeState.Idle);
45+
}
46+
47+
// Returns true when the command should end.
48+
@Override
49+
public boolean isFinished() {
50+
51+
if (Robot.isSimulation()) {
52+
return true;
53+
}
54+
55+
if (m_intakeState == IntakeState.IntakeCoral) {
56+
return m_endEffectorSubsystem.isHolding();
57+
} else if (m_intakeState == IntakeState.OuttakeCoral) {
58+
return !m_endEffectorSubsystem.isHolding();
59+
}
60+
61+
return m_intakeState != m_endEffectorSubsystem.getIntakeState();
62+
}
63+
}

‎src/main/java/frc/robot/commands/PlaceGrabCoralCommand.java‎

Lines changed: 0 additions & 44 deletions
This file was deleted.

‎src/main/java/frc/robot/commands/auton/BlueLeft.java‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,13 +7,14 @@
77
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
88
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
99
import edu.wpi.first.wpilibj2.command.WaitCommand;
10+
import frc.robot.commands.IntakeOuttakeCoralCommand;
1011
import frc.robot.commands.DriveToPose;
11-
import frc.robot.commands.PlaceGrabCoralCommand;
1212
import frc.robot.commands.scoring.L1Command;
1313
import frc.robot.commands.scoring.L4Command;
1414
import frc.robot.subsystems.DriveSubsystem;
1515
import frc.robot.subsystems.ElevatorSubsystem;
1616
import frc.robot.subsystems.EndEffectorSubsystem;
17+
import frc.robot.subsystems.EndEffectorSubsystem.IntakeState;
1718
import frc.robot.utils.FindNearest;
1819

1920
public class BlueLeft extends SequentialCommandGroup {
@@ -36,7 +37,7 @@ public BlueLeft(DriveSubsystem driveSubsystem, ElevatorSubsystem elevator, EndEf
3637
new DriveToPose(driveSubsystem, FindNearest.blueScoringLocations[9].plus(new Transform2d(startingPose, FindNearest.blueScoringLocations[9]).times(-0.15))),
3738
new L1Command(endEffector, elevator, () -> true),
3839
new DriveToPose(driveSubsystem, FindNearest.blueSources[1]),
39-
new PlaceGrabCoralCommand(endEffector, false),
40+
new IntakeOuttakeCoralCommand(endEffector, IntakeState.OuttakeCoral),
4041
new DriveToPose(driveSubsystem, FindNearest.blueScoringLocations[7].plus(new Transform2d(FindNearest.blueSources[1], FindNearest.blueScoringLocations[7]).times(-0.15))),
4142
new L4Command(endEffector, elevator, () -> true),
4243
new DriveToPose(driveSubsystem, FindNearest.blueScoringLocations[7]),

‎src/main/java/frc/robot/commands/auton/RedRight.java‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,13 +7,14 @@
77
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
88
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
99
import edu.wpi.first.wpilibj2.command.WaitCommand;
10+
import frc.robot.commands.IntakeOuttakeCoralCommand;
1011
import frc.robot.commands.DriveToPose;
11-
import frc.robot.commands.PlaceGrabCoralCommand;
1212
import frc.robot.commands.scoring.L1Command;
1313
import frc.robot.commands.scoring.L4Command;
1414
import frc.robot.subsystems.DriveSubsystem;
1515
import frc.robot.subsystems.ElevatorSubsystem;
1616
import frc.robot.subsystems.EndEffectorSubsystem;
17+
import frc.robot.subsystems.EndEffectorSubsystem.IntakeState;
1718
import frc.robot.utils.AllianceFlipUtil;
1819
import frc.robot.utils.FindNearest;
1920

@@ -36,7 +37,7 @@ public RedRight(DriveSubsystem driveSubsystem, ElevatorSubsystem elevator, EndEf
3637
new DriveToPose(driveSubsystem, FindNearest.redScoringLocations[9].plus(new Transform2d(startingPose, FindNearest.redScoringLocations[9]).times(0.15))),
3738
new L1Command(endEffector, elevator, () -> true),
3839
new DriveToPose(driveSubsystem, FindNearest.redSources[1]),
39-
new PlaceGrabCoralCommand(endEffector, false),
40+
new IntakeOuttakeCoralCommand(endEffector, IntakeState.OuttakeCoral),
4041
new DriveToPose(driveSubsystem, FindNearest.redScoringLocations[7].plus(new Transform2d(FindNearest.redSources[1], FindNearest.redScoringLocations[7]).times(-0.15))),
4142
new L4Command(endEffector, elevator, () -> true),
4243
new DriveToPose(driveSubsystem, FindNearest.redScoringLocations[7]),

‎src/main/java/frc/robot/commands/scoring/coral/CoralL1Command.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ public CoralL1Command(EndEffectorSubsystem endEffector, ElevatorSubsystem elevat
1515
new ParallelCommandGroup(
1616
new PivotCommand(endEffector, 0.36),
1717
new ElevatorNoPivotCommand(ElevatorConstants.kL1Height, elevator)),
18-
new PivotCommand(endEffector, 0.08)
18+
new PivotCommand(endEffector, 0.05)
1919
);
2020
}
2121

‎src/main/java/frc/robot/commands/scoring/coral/CoralL2Command.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package frc.robot.commands.scoring.coral;
22

3+
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
34
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
45
import frc.robot.Constants.ElevatorConstants;
56
import frc.robot.commands.ElevatorNoPivotCommand;
@@ -11,8 +12,9 @@ public class CoralL2Command extends SequentialCommandGroup {
1112

1213
public CoralL2Command(EndEffectorSubsystem endEffector, ElevatorSubsystem elevator) {
1314
super(
14-
new PivotCommand(endEffector, 0.36),
15-
new ElevatorNoPivotCommand(ElevatorConstants.kL2Height, elevator));
15+
new ParallelCommandGroup(
16+
new PivotCommand(endEffector, 0.36),
17+
new ElevatorNoPivotCommand(ElevatorConstants.kL2Height, elevator)));
1618
}
1719

1820
}

0 commit comments

Comments
 (0)