Skip to content

Commit 3e871b7

Browse files
arsamulyakarsamulyak
committed
Merge branch 'manual-controls' into 1.0.0
2 parents 516a9a5 + b889648 commit 3e871b7

File tree

3 files changed

+129
-16
lines changed

3 files changed

+129
-16
lines changed

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

Lines changed: 117 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,9 @@ public class Robot extends TimedRobot implements Logged {
4848
private final CommandScheduler scheduler = CommandScheduler.getInstance();
4949

5050
private final CommandXboxController driveController;
51-
private final OperatorController operatorController;
51+
private final CommandXboxController operatorController;
52+
53+
private final OperatorController operatorController2;
5254

5355
private final Swerve swerve;
5456
private final Tail tail;
@@ -103,7 +105,9 @@ public Robot() {
103105

104106
// * SUBSYSTEM INIT
105107
driveController = new CommandXboxController(0);
106-
operatorController = new OperatorController(1);
108+
operatorController = new CommandXboxController(1);
109+
110+
operatorController2 = new OperatorController(3);
107111

108112
Optional<SwerveDriveSimulation> swerveSim =
109113
Robot.isSimulation()
@@ -165,12 +169,103 @@ public Robot() {
165169

166170
tail.setDefaultCommand(
167171
Commands.sequence(
172+
tail.setRollerSpeedCmmd(0.0),
168173
tail.runPoseZeroingCmmd()
169174
.onlyIf(() -> !tail.isZeroed() && Superstructure.isTailSafe(elevator, tail)),
170-
tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)
171-
.until(() -> Superstructure.isTailSafe(elevator, tail)),
172-
tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS).until(() -> tail.inSetpointRange()),
173-
tail.stopMotorInputCmmd()));
175+
tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)));
176+
177+
chute.setDefaultCommand(chute.runChuteSpeedCmmd(0.0));
178+
179+
// .until(() -> Superstructure.isTailSafe(elevator, tail)),
180+
// tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS).until(() -> tail.inSetpointRange()),
181+
// tail.stopMotorInputCmmd()));
182+
183+
// driveController
184+
// .a()
185+
// .whileTrue(chute.runChuteSpeedCmmd(Chute.INTAKE_SPEED))
186+
// .onFalse(chute.runChuteSpeedCmmd(0.0));
187+
188+
// driveController.a().whileTrue(tail.setTargetPoseCmmd(0.0));
189+
190+
// driveController.x().whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS));
191+
/*
192+
driveController
193+
.x()
194+
.whileTrue(tail.setRollerSpeedCmmd(1.4))
195+
.whileFalse(tail.setRollerSpeedCmmd(0));
196+
197+
driveController
198+
.a()
199+
.whileTrue(tail.setRollerSpeedCmmd(-1.4))
200+
.whileFalse(tail.setRollerSpeedCmmd(0));
201+
202+
driveController
203+
.y()
204+
.whileTrue(chute.runChuteSpeedCmmd(-1).alongWith(tail.setRollerSpeedCmmd(1.3)))
205+
.whileFalse(chute.runChuteSpeedCmmd(0).alongWith(tail.setRollerSpeedCmmd(0)));
206+
207+
driveController.b().whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L3));
208+
*/
209+
// driveController.x().whileTrue(tail.setRollerSpeedCmmd(1)).onFalse(tail.setRollerSpeedCmmd(0.0));
210+
211+
// * ELEVATOR LEVEL 1 COMMAND
212+
operatorController
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()));
219+
220+
// * ELEVATOR LEVEL 2 COMMAND
221+
operatorController
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()));
229+
230+
// * ELEVATOR LEVEL 3 COMMAND
231+
operatorController
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()));
239+
240+
// * ELEVATOR LEVEL 4 COMMAND
241+
operatorController
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()));
249+
250+
// * INTAKING CORAL COMMAND
251+
operatorController
252+
.leftBumper()
253+
.whileTrue(chute.runChuteSpeedCmmd(Chute.INTAKE_SPEED)
254+
.alongWith(tail.setRollerSpeedCmmd(Tail.INTAKING_SPEEDS)));
255+
256+
// * OUTTAKING CORAL COMMAND
257+
operatorController
258+
.rightBumper()
259+
.whileTrue(
260+
chute
261+
.runChuteSpeedCmmd(Chute.OUTAKE_SPEED)
262+
.alongWith(tail.setRollerSpeedCmmd(Tail.OUTTAKING_SPEEDS)));
263+
264+
265+
// * SCORING CORAL COMMAND
266+
driveController
267+
.rightBumper()
268+
.whileTrue(tail.setRollerSpeedCmmd(Tail.OUTPUTTING_SPEEDS));
174269

175270
// * SUPERSTRUCTURE INIT
176271
Map<Requests, Trigger> triggerMap = new HashMap<Superstructure.Requests, Trigger>();
@@ -181,14 +276,15 @@ public Robot() {
181276
// * AUTON INIT
182277
autonChooser = Autonomous.configureAutons(swerve);
183278
SmartDashboard.putData("Select Auton", autonChooser);
184-
185-
new Superstructure(
186-
elevator,
187-
tail,
188-
chute,
189-
triggerMap,
190-
operatorController.getBranchTarget(),
191-
operatorController.getLevelTarget());
279+
/*
280+
new Superstructure(
281+
elevator,
282+
tail,
283+
chute,
284+
triggerMap
285+
operatorController2.getBranchTarget(),
286+
operatorController2.getLevelTarget());
287+
*/
192288
}
193289

194290
@Override
@@ -224,4 +320,11 @@ public void teleopInit() {}
224320
public void testInit() {
225321
CommandScheduler.getInstance().cancelAll();
226322
}
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+
}
227330
}

‎src/main/java/wmironpatriots/subsystems/chute/Chute.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ public abstract class Chute implements LoggedSubsystem {
1616
@Log protected double chuteSupplyCurrent;
1717

1818
public static final double INTAKE_SPEED = -0.13;
19+
public static final double OUTAKE_SPEED = 0.13;
1920

2021
/** Checks to see if current is spiking for chute motors */
2122
public boolean hasCoral() {

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

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -24,10 +24,17 @@ public abstract class Tail implements LoggedSubsystem {
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;
28+
29+
public static final double POSE_L1 = -4;
30+
public static final double POSE_L2 = -4;
31+
public static final double POSE_L3 = -4;
32+
public static final double POSE_L4 = -4;
2733

2834
// Roller speeds
29-
public static final double INTAKING_SPEEDS = 5;
30-
public static final double OUTTAKING_SPEEDS = 5;
35+
public static final double INTAKING_SPEEDS = 1;
36+
public static final double OUTTAKING_SPEEDS = -1;
37+
public static final double OUTPUTTING_SPEEDS = 2;
3138

3239
public static final double PIVOT_P = 1.5;
3340
public static final double PIVOT_I = 0.0;
@@ -60,6 +67,8 @@ public Command setTargetPoseCmmd(double poseRes) {
6067
});
6168
}
6269

70+
71+
6372
/** Runs rollers at specific speed */
6473
public Command setRollerSpeedCmmd(double speed) {
6574
return this.run(

0 commit comments

Comments
 (0)