Skip to content

Commit b889648

Browse files
arsamulyakarsamulyak
committed
refactored commands to be more simpler
Signed-off-by: arsamulyak <[email protected]>
1 parent 1d3585e commit b889648

File tree

2 files changed

+65
-64
lines changed

2 files changed

+65
-64
lines changed

‎src/main/java/wmironpatriots/Robot.java‎

Lines changed: 64 additions & 64 deletions
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ public class Robot extends TimedRobot implements Logged {
4949

5050
private final CommandXboxController driveController;
5151
private final CommandXboxController operatorController;
52-
52+
5353
private final OperatorController operatorController2;
5454

5555
private final Swerve swerve;
@@ -169,9 +169,13 @@ public Robot() {
169169

170170
tail.setDefaultCommand(
171171
Commands.sequence(
172+
tail.setRollerSpeedCmmd(0.0),
172173
tail.runPoseZeroingCmmd()
173174
.onlyIf(() -> !tail.isZeroed() && Superstructure.isTailSafe(elevator, tail)),
174-
tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
175+
tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)));
176+
177+
chute.setDefaultCommand(chute.runChuteSpeedCmmd(0.0));
178+
175179
// .until(() -> Superstructure.isTailSafe(elevator, tail)),
176180
// tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS).until(() -> tail.inSetpointRange()),
177181
// tail.stopMotorInputCmmd()));
@@ -184,7 +188,7 @@ public Robot() {
184188
// driveController.a().whileTrue(tail.setTargetPoseCmmd(0.0));
185189

186190
// driveController.x().whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS));
187-
/*
191+
/*
188192
driveController
189193
.x()
190194
.whileTrue(tail.setRollerSpeedCmmd(1.4))
@@ -204,74 +208,64 @@ public Robot() {
204208
*/
205209
// driveController.x().whileTrue(tail.setRollerSpeedCmmd(1)).onFalse(tail.setRollerSpeedCmmd(0.0));
206210

207-
208211
// * ELEVATOR LEVEL 1 COMMAND
209212
operatorController
210-
.a()
211-
.whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)
212-
.until(() -> tail.inSetpointRange())
213-
.andThen(elevator.setTargetPoseCmmd(Elevator.POSE_L1))
214-
.alongWith(tail.setTargetPoseCmmd(Tail.POSE_L1)))
215-
.whileFalse(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING)
216-
.until(() -> elevator.inSetpointRange())
217-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
213+
.a()
214+
.whileTrue(
215+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_REVS)
216+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
217+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
218+
.whileTrue(this.setElevatorToStowed().onlyIf(() -> tail.inSetpointRange()));
218219

219220
// * ELEVATOR LEVEL 2 COMMAND
220221
operatorController
221-
.x()
222-
.whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)
223-
.until(() -> tail.inSetpointRange())
224-
.andThen(elevator.setTargetPoseCmmd(Elevator.POSE_L2))
225-
.alongWith(tail.setTargetPoseCmmd(Tail.POSE_L2)))
226-
.whileFalse(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING)
227-
.until(() -> elevator.inSetpointRange())
228-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
222+
.x()
223+
.whileTrue(
224+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_REVS)
225+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
226+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L2)))
227+
.whileTrue(
228+
elevator.setTargetPoseCmmd(Elevator.POSE_L2).onlyIf(() -> tail.inSetpointRange()));
229229

230230
// * ELEVATOR LEVEL 3 COMMAND
231231
operatorController
232-
.a()
233-
.whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)
234-
.until(() -> tail.inSetpointRange())
235-
.andThen(elevator.setTargetPoseCmmd(Elevator.POSE_L3))
236-
.alongWith(tail.setTargetPoseCmmd(Tail.POSE_L3)))
237-
.whileFalse(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING)
238-
.until(() -> elevator.inSetpointRange())
239-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
232+
.b()
233+
.whileTrue(
234+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_REVS)
235+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
236+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L3)))
237+
.whileTrue(
238+
elevator.setTargetPoseCmmd(Elevator.POSE_L3).onlyIf(() -> tail.inSetpointRange()));
240239

241240
// * ELEVATOR LEVEL 4 COMMAND
242241
operatorController
243-
.x()
244-
.whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)
245-
.until(() -> tail.inSetpointRange())
246-
.andThen(elevator.setTargetPoseCmmd(Elevator.POSE_L4))
247-
.alongWith(tail.setTargetPoseCmmd(Tail.POSE_L4)))
248-
.whileFalse(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING)
249-
.until(() -> elevator.inSetpointRange())
250-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
251-
242+
.y()
243+
.whileTrue(
244+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_REVS)
245+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
246+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L4)))
247+
.whileTrue(
248+
elevator.setTargetPoseCmmd(Elevator.POSE_L4).onlyIf(() -> tail.inSetpointRange()));
252249

253250
// * INTAKING CORAL COMMAND
254-
operatorController.leftBumper()
255-
.whileTrue(chute.runChuteSpeedCmmd(Chute.INTAKE_SPEED)
256-
.alongWith(tail.setRollerSpeedCmmd(Tail.INTAKING_SPEEDS)))
257-
.whileFalse(chute.runChuteSpeedCmmd(0)
258-
.alongWith(tail.setRollerSpeedCmmd(0)));
259-
251+
operatorController
252+
.leftBumper()
253+
.whileTrue(chute.runChuteSpeedCmmd(Chute.INTAKE_SPEED)
254+
.alongWith(tail.setRollerSpeedCmmd(Tail.INTAKING_SPEEDS)));
255+
260256
// * OUTTAKING CORAL COMMAND
261-
operatorController.rightBumper()
262-
.whileTrue(chute.runChuteSpeedCmmd(Chute.OUTAKE_SPEED)
263-
.alongWith(tail.setRollerSpeedCmmd(Tail.OUTTAKING_SPEEDS)))
264-
.whileFalse(chute.runChuteSpeedCmmd(0)
265-
.alongWith(tail.setRollerSpeedCmmd(0)));
257+
operatorController
258+
.rightBumper()
259+
.whileTrue(
260+
chute
261+
.runChuteSpeedCmmd(Chute.OUTAKE_SPEED)
262+
.alongWith(tail.setRollerSpeedCmmd(Tail.OUTTAKING_SPEEDS)));
263+
266264

267-
268265
// * SCORING CORAL COMMAND
269266
driveController
270-
.rightBumper()
271-
.whileTrue(tail.setRollerSpeedCmmd(Tail.OUTPUTTING_SPEEDS))
272-
.whileFalse(tail.setRollerSpeedCmmd(0));
273-
274-
267+
.rightBumper()
268+
.whileTrue(tail.setRollerSpeedCmmd(Tail.OUTPUTTING_SPEEDS));
275269

276270
// * SUPERSTRUCTURE INIT
277271
Map<Requests, Trigger> triggerMap = new HashMap<Superstructure.Requests, Trigger>();
@@ -282,17 +276,16 @@ public Robot() {
282276
// * AUTON INIT
283277
autonChooser = Autonomous.configureAutons(swerve);
284278
SmartDashboard.putData("Select Auton", autonChooser);
285-
/*
286-
new Superstructure(
287-
elevator,
288-
tail,
289-
chute,
290-
triggerMap
291-
operatorController2.getBranchTarget(),
292-
operatorController2.getLevelTarget());
293-
*/
279+
/*
280+
new Superstructure(
281+
elevator,
282+
tail,
283+
chute,
284+
triggerMap
285+
operatorController2.getBranchTarget(),
286+
operatorController2.getLevelTarget());
287+
*/
294288
}
295-
296289

297290
@Override
298291
public void robotPeriodic() {
@@ -327,4 +320,11 @@ public void teleopInit() {}
327320
public void testInit() {
328321
CommandScheduler.getInstance().cancelAll();
329322
}
323+
324+
private Command setElevatorToStowed() {
325+
return elevator
326+
.setTargetPoseCmmd(1.0)
327+
.until(() -> elevator.inSetpointRange() || elevator.getSetpoint() > elevator.getPose())
328+
.andThen(elevator.stopMotorInputCmmd());
329+
}
330330
}

‎src/main/java/wmironpatriots/subsystems/tail/Tail.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ public abstract class Tail implements IronSubsystem {
2424
// Poses
2525
public static final double POSE_MIN_REVS = -10.4;
2626
public static final double POSE_MAX_REVS = 0.1;
27+
public static final double POSE_MOVE_REVS = -5;
2728

2829
public static final double POSE_L1 = -4;
2930
public static final double POSE_L2 = -4;

0 commit comments

Comments
 (0)