Skip to content

Commit 40e9b5c

Browse files
vox-eternalvox-eternal
committed
11-4 commit
1 parent 99ae816 commit 40e9b5c

File tree

6 files changed

+84
-57
lines changed

6 files changed

+84
-57
lines changed

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

Lines changed: 20 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -190,22 +190,22 @@ private void configureButtonBindings() {
190190
joyDrive.rightBumper().onTrue(setSlowMode(true)).onFalse(setSlowMode(false));
191191
joyDrive.start().onTrue(offsetGyro());
192192

193-
joyDrive
194-
.leftTrigger()
195-
.and(joyDrive.rightTrigger().negate())
196-
.onTrue(drivetrain.selectNearestScoringSpot(Side.LEFT))
197-
.whileTrue(drivetrain.alignSelectedSpot(Side.LEFT));
198-
joyDrive
199-
.rightTrigger()
200-
.and(joyDrive.leftTrigger().negate())
201-
.onTrue(drivetrain.selectNearestScoringSpot(Side.RIGHT))
202-
.whileTrue(drivetrain.alignSelectedSpot(Side.RIGHT));
193+
// joyDrive
194+
// .leftTrigger()
195+
// .and(joyDrive.rightTrigger().negate())
196+
// .onTrue(drivetrain.selectNearestScoringSpot(Side.LEFT))
197+
// .whileTrue(drivetrain.alignSelectedSpot(Side.LEFT));
198+
// joyDrive
199+
// .rightTrigger()
200+
// .and(joyDrive.leftTrigger().negate())
201+
// .onTrue(drivetrain.selectNearestScoringSpot(Side.RIGHT))
202+
// .whileTrue(drivetrain.alignSelectedSpot(Side.RIGHT));
203203

204-
joyDrive
205-
.leftTrigger()
206-
.and(joyDrive.rightTrigger())
207-
.onTrue(drivetrain.selectNearestScoringSpot(Side.CENTER))
208-
.whileTrue(drivetrain.alignSelectedSpot(Side.CENTER));
204+
// joyDrive
205+
// .leftTrigger()
206+
// .and(joyDrive.rightTrigger())
207+
// .onTrue(drivetrain.selectNearestScoringSpot(Side.CENTER))
208+
// .whileTrue(drivetrain.alignSelectedSpot(Side.CENTER));
209209

210210
/*
211211
* Manipulator button bindings:
@@ -222,6 +222,9 @@ private void configureButtonBindings() {
222222
* point up on dpad to toggle climber block
223223
// * point down on dpad and press B (L2) or X (L3) to clean algae, release to stow
224224
*/
225+
joyManip.a().onTrue(outtake.extend()).onFalse(outtake.retract());
226+
joyManip.b().onTrue(DriveCommands.vomit(arm, intakeWheel));
227+
joyManip.rightTrigger().onTrue(outtake.setLockOverride(true)).onFalse(outtake.setLockOverride(false));
225228

226229
}
227230

@@ -284,8 +287,8 @@ private Command offsetGyro() {
284287
public void teleopInit() {
285288
drivetrain.setTeleopCurrentLimit();
286289
arm.sensorTrigger
287-
.onTrue(outtake.setLock(true))
288-
.onFalse(Commands.sequence(Commands.waitSeconds(1), outtake.setLock(false)));
290+
.onTrue(outtake.setLock(true))
291+
.onFalse(Commands.sequence(Commands.waitSeconds(1), outtake.setLock(false)));
289292
// drivetrain.offsetGyro(Rotation2d.fromDegrees(-90));
290293
}
291294

‎src/main/java/frc/team5115/commands/AutoCommands.java‎

Lines changed: 20 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -9,28 +9,28 @@ public class AutoCommands {
99
private AutoCommands() {}
1010

1111
// command for raising while aligning
12-
public static Command getReefAlignCommand(Drivetrain drivetrain, Side side) {
13-
return Commands.sequence(drivetrain.autoAlignToScoringSpot(side).withTimeout(2.0));
14-
}
12+
// public static Command getReefAlignCommand(Drivetrain drivetrain, Side side) {
13+
// return Commands.sequence(drivetrain.autoAlignToScoringSpot(side).withTimeout(2.0));
14+
// }
1515

16-
/** Aligns left side, removes algae, and scores. */
17-
public static Command cleanAndScoreLeft(Drivetrain drivetrain) {
18-
return Commands.sequence(drivetrain.autoAlignToScoringSpot(Side.LEFT).withTimeout(3.0));
19-
}
16+
// /** Aligns left side, removes algae, and scores. */
17+
// public static Command cleanAndScoreLeft(Drivetrain drivetrain) {
18+
// return Commands.sequence(drivetrain.autoAlignToScoringSpot(Side.LEFT).withTimeout(3.0));
19+
// }
2020

21-
public static Command dealgify(Drivetrain drivetrain) {
22-
return Commands.sequence(
23-
Commands.print("Align"), drivetrain.autoAlignToScoringSpot(Side.CENTER).withTimeout(3.0));
24-
}
21+
// public static Command dealgify(Drivetrain drivetrain) {
22+
// return Commands.sequence(
23+
// Commands.print("Align"), drivetrain.autoAlignToScoringSpot(Side.CENTER).withTimeout(3.0));
24+
// }
2525

26-
public static Command scoreSequence(Drivetrain drivetrain, Side side) {
27-
return Commands.sequence(
28-
Commands.print("ScoreSequence!"), drivetrain.autoAlignToScoringSpot(side));
29-
}
26+
// public static Command scoreSequence(Drivetrain drivetrain, Side side) {
27+
// return Commands.sequence(
28+
// Commands.print("ScoreSequence!"), drivetrain.autoAlignToScoringSpot(side));
29+
// }
3030

31-
public static Command testingGetReefAlignCommand(Drivetrain drivetrain) {
32-
return Commands.sequence(
33-
// drivetrain.autoAlignToScoringSpot(side)
34-
);
35-
}
31+
// public static Command testingGetReefAlignCommand(Drivetrain drivetrain) {
32+
// return Commands.sequence(
33+
// // drivetrain.autoAlignToScoringSpot(side)
34+
// );
35+
// }
3636
}

‎src/main/java/frc/team5115/commands/DriveCommands.java‎

Lines changed: 16 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
import frc.team5115.subsystems.arm.Arm;
1313
import frc.team5115.subsystems.drive.Drivetrain;
1414
import frc.team5115.subsystems.intakewheel.IntakeWheel;
15+
import frc.team5115.subsystems.outtake.Outtake;
1516

1617
import java.util.function.BooleanSupplier;
1718
import java.util.function.DoubleSupplier;
@@ -71,7 +72,20 @@ public static Command joystickDrive(
7172
drivetrain);
7273
}
7374

74-
public Command xferLunite(Arm arm, IntakeWheel intakeWheel) {
75-
return Commands.sequence(arm.deploy(), intakeWheel.intake(), arm.waitForSensorState(true, Double.POSITIVE_INFINITY), arm.stow(), intakeWheel.setSpeed(-1), Commands.waitSeconds(1));
75+
public static Command xferLunite(Outtake outtake, Arm arm, IntakeWheel intakeWheel) {
76+
return Commands.sequence(
77+
outtake.setLock(true),
78+
arm.deploy(),
79+
intakeWheel.intake(),
80+
arm.waitForSensorState(true, Double.POSITIVE_INFINITY),
81+
arm.stow(),
82+
arm.waitForSetpoint(Double.POSITIVE_INFINITY),
83+
intakeWheel.setSpeed(-1),
84+
Commands.waitSeconds(1),
85+
outtake.setLock(false));
86+
}
87+
88+
public static Command vomit(Arm arm, IntakeWheel intakeWheel) {
89+
return Commands.sequence(arm.deploy(), arm.waitForSetpoint(Double.POSITIVE_INFINITY), intakeWheel.vomit());
7690
}
7791
}

‎src/main/java/frc/team5115/subsystems/drive/Drivetrain.java‎

Lines changed: 17 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -291,30 +291,30 @@ public Trigger aligningToGoal() {
291291
}
292292

293293
/** Drives to nearest scoring spot until all pids at goal */
294-
public Command autoAlignToScoringSpot(AutoConstants.Side side) {
295-
return Commands.sequence(
296-
Commands.print("AutoDriving!"),
297-
selectNearestScoringSpot(side),
298-
alignSelectedSpot(side).until(() -> alignedAtGoal()));
299-
}
294+
// public Command autoAlignToScoringSpot(AutoConstants.Side side) {
295+
// return Commands.sequence(
296+
// Commands.print("AutoDriving!"),
297+
// selectNearestScoringSpot(side),
298+
// alignSelectedSpot(side).until(() -> alignedAtGoal()));
299+
// }
300300

301301
/**
302302
* Choose the scoring spot based on nearest scoring spot. Will also reset the pids.
303303
*
304304
* @param side the side to score on
305305
* @return an Instant Command
306306
*/
307-
public Command selectNearestScoringSpot(AutoConstants.Side side) {
308-
return Commands.runOnce(
309-
() -> {
310-
selectedPose = AutoConstants.getNearestScoringSpot(getPose(), side);
311-
final var pose = getPose();
312-
anglePid.reset(pose.getRotation().getRadians());
313-
xPid.reset(pose.getX());
314-
yPid.reset(pose.getY());
315-
},
316-
this);
317-
}
307+
// public Command selectNearestScoringSpot(AutoConstants.Side side) {
308+
// return Commands.runOnce(
309+
// () -> {
310+
// selectedPose = AutoConstants.getNearestScoringSpot(getPose(), side);
311+
// final var pose = getPose();
312+
// anglePid.reset(pose.getRotation().getRadians());
313+
// xPid.reset(pose.getX());
314+
// yPid.reset(pose.getY());
315+
// },
316+
// this);
317+
// }
318318

319319
/**
320320
* Drive by auto aim pids using an already chosen `selectedPose`

‎src/main/java/frc/team5115/subsystems/outtake/Outtake.java‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,4 +38,8 @@ public Command retract() {
3838
public Command setLock(boolean lock) {
3939
return Commands.runOnce(() -> locked = lock);
4040
}
41+
42+
public Command setLockOverride(boolean lockOverridee) {
43+
return Commands.runOnce(() -> lockOverride = lockOverridee);
44+
}
4145
}

‎the evil table.txt‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,4 +7,10 @@ Intake Arm
77

88
Defacator
99
- Arm - Pnumatic
10-
- Wait till three Lunites (Sensor? Count Variable?), Move up arm
10+
- Wait till three Lunites (Sensor? Count Variable?), Move up arm
11+
12+
Commands for binding:
13+
button to score (done)
14+
movement (done)
15+
vomit on arm (done)
16+
override lock (press / trigger hold) (done)

0 commit comments

Comments
 (0)