Skip to content

Commit c728412

Browse files
admlvntvadmlvntv
authored
Merge pull request #99 from team467/dcmp
Changes from DCMP
2 parents 6186dc2 + 6fdeb7d commit c728412

17 files changed

Lines changed: 162 additions & 77 deletions

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

Lines changed: 7 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -39,13 +39,7 @@
3939
import frc.robot.commands.arm.ArmShelfCMD;
4040
import frc.robot.commands.arm.ArmStopCMD;
4141
import frc.robot.commands.auto.NewAlignToNode;
42-
import frc.robot.commands.auto.complex.BackUpAndBalance;
43-
import frc.robot.commands.auto.complex.OnlyBackup;
44-
import frc.robot.commands.auto.complex.OnlyBalance;
45-
import frc.robot.commands.auto.complex.OnlyScore;
46-
import frc.robot.commands.auto.complex.ScoreAndBackUp;
47-
import frc.robot.commands.auto.complex.ScoreAndBackUpAndBalance;
48-
import frc.robot.commands.auto.complex.ScoreAndBalance;
42+
import frc.robot.commands.auto.complex.*;
4943
import frc.robot.commands.drive.DriveWithDpad;
5044
import frc.robot.commands.drive.DriveWithJoysticks;
5145
import frc.robot.commands.intakerelease.HoldCMD;
@@ -195,13 +189,13 @@ public RobotContainer() {
195189

196190
// AprilTag 3 or 6
197191
autoChooser.addOption(
198-
"Tag 3/6: Only Back Up", new OnlyBackup(6, "Right", drive, arm, intakeRelease));
192+
"Tag 3/6: Only Back Up", new OnlyBackupClearSide(6, "Right", drive, arm, intakeRelease));
199193
autoChooser.addOption(
200194
"Tag 3/6: Only Score Cone",
201195
new OnlyScore(6, "Right", "Cone", "High", drive, arm, intakeRelease));
202196
autoChooser.addOption(
203197
"Tag 3/6: Score Cone and Back Up",
204-
new ScoreAndBackUp(6, "Right", "Cone", "High", drive, arm, intakeRelease));
198+
new ScoreAndBackUpClearSide(6, "Right", "Cone", "High", drive, arm, intakeRelease));
205199

206200
// AprilTag 2 or 7
207201
autoChooser.addOption(
@@ -219,13 +213,13 @@ public RobotContainer() {
219213

220214
// AprilTag 1 or 8
221215
autoChooser.addOption(
222-
"Tag 1/8: Only Back Up", new OnlyBackup(8, "Left", drive, arm, intakeRelease));
216+
"Tag 1/8: Only Back Up", new OnlyBackupBumpSide(8, "Left", drive, arm, intakeRelease));
223217
autoChooser.addOption(
224218
"Tag 1/8: Only Score Cone",
225219
new OnlyScore(8, "Left", "Cone", "High", drive, arm, intakeRelease));
226220
autoChooser.addOption(
227221
"Tag 1/8: Score Cone and Back Up",
228-
new ScoreAndBackUp(8, "Left", "Cone", "High", drive, arm, intakeRelease));
222+
new ScoreAndBackUpBumpSide(8, "Left", "Cone", "High", drive, arm, intakeRelease));
229223

230224
autoChooser.addOption(
231225
"Drive Characterization",
@@ -314,8 +308,8 @@ private void configureButtonBindings() {
314308
.recordOutput("CustomController/HomeButton", operatorController.x().getAsBoolean());
315309

316310
// Home will be for movement
317-
operatorController.x().onTrue(new ArmHomeCMD(arm));
318-
driverController.x().onTrue(new ArmHomeCMD(arm));
311+
operatorController.x().onTrue(new ArmHomeCMD(arm, intakeRelease::wantsCone));
312+
driverController.x().onTrue(new ArmHomeCMD(arm, intakeRelease::wantsCone));
319313

320314
// Need to set to use automated movements, should be set in Autonomous init.
321315
driverController.back().onTrue(new ArmCalibrateCMD(arm));

‎src/main/java/frc/robot/commands/arm/ArmDropCMD.java‎

Lines changed: 41 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -2,42 +2,56 @@
22

33
import edu.wpi.first.wpilibj.Timer;
44
import edu.wpi.first.wpilibj2.command.CommandBase;
5+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
6+
import edu.wpi.first.wpilibj2.command.WaitCommand;
57
import frc.robot.subsystems.arm.Arm;
68
import java.util.function.Supplier;
79

8-
public class ArmDropCMD extends CommandBase {
9-
private static final double ROTATE_DROP_METERS = 0.025;
10-
private Arm arm;
11-
private Supplier<Boolean> haveCone;
12-
private Supplier<Boolean> wantsCone;
13-
private Timer timer = new Timer();
10+
public class ArmDropCMD extends SequentialCommandGroup {
11+
private boolean hasDropped = false;
1412

1513
public ArmDropCMD(Supplier<Boolean> haveCone, Supplier<Boolean> wantsCone, Arm arm) {
16-
this.arm = arm;
17-
this.haveCone = haveCone;
18-
this.wantsCone = wantsCone;
19-
addRequirements(arm);
14+
addCommands(
15+
new InternalArmDropCMD(haveCone, wantsCone, arm),
16+
new WaitCommand(0.5).unless(() -> !hasDropped));
2017
}
2118

22-
@Override
23-
public void initialize() {
24-
timer.reset();
25-
timer.start();
26-
if (haveCone.get()
27-
&& wantsCone.get()
28-
&& arm.getExtention() >= 0.2
29-
&& arm.getRotation() >= 0.1) {
30-
arm.setTargetPositionRotate(arm.getRotation() - ROTATE_DROP_METERS);
19+
private class InternalArmDropCMD extends CommandBase {
20+
private static final double ROTATE_DROP_METERS = 0.025;
21+
private Arm arm;
22+
private Supplier<Boolean> haveCone;
23+
private Supplier<Boolean> wantsCone;
24+
private Timer timer = new Timer();
25+
26+
public InternalArmDropCMD(Supplier<Boolean> haveCone, Supplier<Boolean> wantsCone, Arm arm) {
27+
this.arm = arm;
28+
this.haveCone = haveCone;
29+
this.wantsCone = wantsCone;
30+
addRequirements(arm);
3131
}
32-
}
3332

34-
@Override
35-
public void end(boolean interrupted) {
36-
arm.hold();
37-
}
33+
@Override
34+
public void initialize() {
35+
timer.reset();
36+
timer.start();
37+
hasDropped = false;
38+
if (haveCone.get()
39+
&& wantsCone.get()
40+
&& arm.getExtention() >= 0.2
41+
&& arm.getRotation() >= 0.1) {
42+
arm.setTargetPositionRotate(arm.getRotation() - ROTATE_DROP_METERS);
43+
hasDropped = true;
44+
}
45+
}
3846

39-
@Override
40-
public boolean isFinished() {
41-
return timer.hasElapsed(0.5) || arm.isFinished();
47+
@Override
48+
public void end(boolean interrupted) {
49+
arm.hold();
50+
}
51+
52+
@Override
53+
public boolean isFinished() {
54+
return timer.hasElapsed(0.5) || arm.isFinished();
55+
}
4256
}
4357
}

‎src/main/java/frc/robot/commands/arm/ArmHomeCMD.java‎

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,17 @@
11
package frc.robot.commands.arm;
22

3+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
34
import frc.robot.subsystems.arm.Arm;
45
import frc.robot.subsystems.arm.ArmPositionConstants;
6+
import java.util.function.Supplier;
57

6-
public class ArmHomeCMD extends ArmPositionCMD {
8+
public class ArmHomeCMD extends SequentialCommandGroup {
79

8-
public ArmHomeCMD(Arm arm) {
9-
super(arm, ArmPositionConstants.HOME);
10+
public ArmHomeCMD(Arm arm, Supplier<Boolean> wantsCone) {
11+
addCommands(
12+
new ArmPositionCMD(
13+
arm,
14+
wantsCone.get() ? ArmPositionConstants.CONE_HOME : ArmPositionConstants.CUBE_HOME)
15+
.withTimeout(5.0));
1016
}
1117
}

‎src/main/java/frc/robot/commands/auto/StraightDriveToPose.java‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,10 @@ public class StraightDriveToPose extends CommandBase {
2929
new ProfiledPIDController(
3030
// 2.0, 0, 0.0, new Constraints(Units.inchesToMeters(150),
3131
// Units.inchesToMeters(450.0)));
32-
2.0, 0, 0.0, new Constraints(Units.inchesToMeters(90), Units.inchesToMeters(450.0)));
32+
2.0,
33+
0,
34+
0.0,
35+
new Constraints(Units.inchesToMeters(85), Units.inchesToMeters(450.0))); // 90
3336
private final ProfiledPIDController thetaController =
3437
new ProfiledPIDController(
3538
5.0, 0, 0.0, new Constraints(Units.degreesToRadians(360), Units.degreesToRadians(720.0)));

‎src/main/java/frc/robot/commands/auto/complex/BackUpAndBalance.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
import edu.wpi.first.math.util.Units;
44
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
5+
import frc.robot.FieldConstants;
56
import frc.robot.commands.auto.BetterBalancing;
67
import frc.robot.commands.auto.Initialize;
78
import frc.robot.commands.auto.StraightDriveToPose;
@@ -13,6 +14,7 @@ public BackUpAndBalance(String relativePosition, Drive drive, Arm arm) {
1314
int aprilTag = 7;
1415
addCommands(
1516
new Initialize(aprilTag, relativePosition, drive, arm),
17+
new StraightDriveToPose(0.0, -FieldConstants.Grids.nodeSeparationY, 0.0, drive),
1618
new StraightDriveToPose(Units.inchesToMeters(175.0), 0.0, 0.0, drive),
1719
new StraightDriveToPose(Units.inchesToMeters(-75.0), 0.0, 0.0, drive).withTimeout(2.5),
1820
new BetterBalancing(drive));

src/main/java/frc/robot/commands/auto/complex/OnlyBackup.java renamed to src/main/java/frc/robot/commands/auto/complex/OnlyBackupBumpSide.java

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,12 +8,12 @@
88
import frc.robot.subsystems.drive.Drive;
99
import frc.robot.subsystems.intakerelease.IntakeRelease;
1010

11-
public class OnlyBackup extends SequentialCommandGroup {
11+
public class OnlyBackupBumpSide extends SequentialCommandGroup {
1212

13-
public OnlyBackup(
13+
public OnlyBackupBumpSide(
1414
int aprilTag, String relativePosition, Drive drive, Arm arm, IntakeRelease intakeRelease) {
1515
addCommands(
1616
new Initialize(aprilTag, relativePosition, drive, arm),
17-
new StraightDriveToPose(Units.inchesToMeters(156.0), 0.0, 0.0, drive));
17+
new StraightDriveToPose(Units.inchesToMeters(175.0), 0.0, 0.0, drive));
1818
}
1919
}

‎src/main/java/frc/robot/commands/auto/complex/OnlyBackupClearSide.java‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
package frc.robot.commands.auto.complex;
2+
3+
import edu.wpi.first.math.util.Units;
4+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
5+
import frc.robot.commands.auto.Initialize;
6+
import frc.robot.commands.auto.StraightDriveToPose;
7+
import frc.robot.subsystems.arm.Arm;
8+
import frc.robot.subsystems.drive.Drive;
9+
import frc.robot.subsystems.intakerelease.IntakeRelease;
10+
11+
public class OnlyBackupClearSide extends SequentialCommandGroup {
12+
13+
public OnlyBackupClearSide(
14+
int aprilTag, String relativePosition, Drive drive, Arm arm, IntakeRelease intakeRelease) {
15+
addCommands(
16+
new Initialize(aprilTag, relativePosition, drive, arm),
17+
new StraightDriveToPose(Units.inchesToMeters(160.0), 0.0, 0.0, drive));
18+
}
19+
}

‎src/main/java/frc/robot/commands/auto/complex/OnlyBalance.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
import edu.wpi.first.math.util.Units;
44
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
5+
import frc.robot.FieldConstants;
56
import frc.robot.commands.auto.BetterBalancing;
67
import frc.robot.commands.auto.Initialize;
78
import frc.robot.commands.auto.StraightDriveToPose;
@@ -14,6 +15,7 @@ public OnlyBalance(String relativePosition, Drive drive, Arm arm) {
1415
int aprilTag = 7;
1516
addCommands(
1617
new Initialize(aprilTag, relativePosition, drive, arm),
18+
new StraightDriveToPose(0.0, -FieldConstants.Grids.nodeSeparationY, 0.0, drive),
1719
new StraightDriveToPose(Units.inchesToMeters(95.25), 0.0, 0.0, drive),
1820
new BetterBalancing(drive));
1921
}

‎src/main/java/frc/robot/commands/auto/complex/OnlyScore.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,6 @@ public OnlyScore(
2020
addCommands(
2121
new Initialize(aprilTag, relativePosition, drive, arm),
2222
new Score(gamePieceType, location, arm, intakeRelease),
23-
new ArmHomeCMD(arm));
23+
new ArmHomeCMD(arm, intakeRelease::wantsCone));
2424
}
2525
}

‎src/main/java/frc/robot/commands/auto/complex/ScoreAndBackUpAndBalance.java‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import edu.wpi.first.math.util.Units;
44
import edu.wpi.first.wpilibj2.command.Commands;
55
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
6+
import frc.robot.FieldConstants;
67
import frc.robot.commands.arm.ArmHomeCMD;
78
import frc.robot.commands.auto.BetterBalancing;
89
import frc.robot.commands.auto.Initialize;
@@ -25,8 +26,10 @@ public ScoreAndBackUpAndBalance(
2526
new Initialize(aprilTag, relativePosition, drive, arm),
2627
new Score(gamePieceType, location, arm, intakeRelease),
2728
Commands.parallel(
28-
new ArmHomeCMD(arm).withTimeout(3.5),
29-
new StraightDriveToPose(Units.inchesToMeters(175.0), 0.0, 0.0, drive)),
29+
new ArmHomeCMD(arm, intakeRelease::wantsCone).withTimeout(3.5),
30+
Commands.sequence(
31+
new StraightDriveToPose(0.0, -FieldConstants.Grids.nodeSeparationY, 0.0, drive),
32+
new StraightDriveToPose(Units.inchesToMeters(175.0), 0.0, 0.0, drive))),
3033
new StraightDriveToPose(Units.inchesToMeters(-75.0), 0.0, 0.0, drive).withTimeout(2.5),
3134
new BetterBalancing(drive));
3235
}

0 commit comments

Comments
 (0)