Skip to content

Commit d5f402e

Browse files
dabeycorndabeycorn
committed
meh
1 parent 415d640 commit d5f402e

File tree

6 files changed

+141
-153
lines changed

6 files changed

+141
-153
lines changed

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

Lines changed: 115 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -11,17 +11,17 @@
1111
import static edu.wpi.first.units.Units.RotationsPerSecond;
1212
import static wmironpatriots.Constants.SWERVE_SIM_CONFIG;
1313

14-
import choreo.auto.AutoChooser;
1514
import com.ctre.phoenix6.SignalLogger;
1615
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
1716
import com.ctre.phoenix6.swerve.SwerveRequest;
1817
import edu.wpi.first.math.geometry.Pose2d;
1918
import edu.wpi.first.math.geometry.Rotation2d;
2019
import edu.wpi.first.wpilibj.DriverStation;
2120
import edu.wpi.first.wpilibj.TimedRobot;
22-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2321
import edu.wpi.first.wpilibj2.command.Command;
2422
import edu.wpi.first.wpilibj2.command.CommandScheduler;
23+
import edu.wpi.first.wpilibj2.command.Commands;
24+
import edu.wpi.first.wpilibj2.command.WaitCommand;
2525
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
2626
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
2727
import java.util.Optional;
@@ -31,7 +31,6 @@
3131
import monologue.Monologue.MonologueConfig;
3232
import org.ironmaple.simulation.SimulatedArena;
3333
import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
34-
import wmironpatriots.commands.ChoreoFactory;
3534
import wmironpatriots.generated.TunerConstants;
3635
import wmironpatriots.subsystems.CommandSwerveDrivetrain;
3736
import wmironpatriots.subsystems.chute.Chute;
@@ -42,20 +41,17 @@
4241
import wmironpatriots.subsystems.tail.Tail;
4342
import wmironpatriots.subsystems.tail.TailIOComp;
4443
import wmironpatriots.subsystems.tail.TailIOSim;
45-
import wmironpatriots.util.deviceUtil.OperatorController;
4644

4745
public class Robot extends TimedRobot implements Logged {
4846
private final CommandScheduler scheduler = CommandScheduler.getInstance();
4947

5048
private final CommandXboxController joystick;
5149
private final CommandXboxController operatorController;
52-
53-
private final OperatorController operatorController2;
50+
private final CommandXboxController debugController;
5451

5552
private final Tail tail;
5653
private final Elevator elevator;
5754
private final Chute chute;
58-
private final ChoreoFactory factory;
5955
private final RobotVisualizer visualizer;
6056

6157
private double MaxSpeed =
@@ -79,8 +75,6 @@ public class Robot extends TimedRobot implements Logged {
7975

8076
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
8177

82-
private final AutoChooser chooser;
83-
8478
public Robot() {
8579
// * MONOLOGUE SETUP
8680
DriverStation.silenceJoystickConnectionWarning(true);
@@ -126,8 +120,7 @@ public Robot() {
126120
// * SUBSYSTEM INIT
127121
joystick = new CommandXboxController(0);
128122
operatorController = new CommandXboxController(1);
129-
130-
operatorController2 = new OperatorController(3);
123+
debugController = new CommandXboxController(4);
131124

132125
Optional<SwerveDriveSimulation> swerveSim =
133126
Robot.isSimulation()
@@ -155,30 +148,29 @@ public Robot() {
155148
// Set up driver input streams
156149
configureBindings();
157150

158-
// elevator.setDefaultCommand(
159-
// Commands.sequence(
160-
// elevator.runPoseZeroingCmmd().onlyIf(() -> !elevator.isZeroed()),
161-
// elevator
162-
// .setTargetPoseCmmd(3.4)
163-
// .until(
164-
// () ->
165-
// elevator.inSetpointRange() || elevator.getSetpoint() >
166-
// elevator.getPose()),
167-
// elevator.stopMotorInputCmmd()));
168-
169-
// tail.setDefaultCommand(
170-
// Commands.sequence(
171-
// // tail.setRollerSpeedCmmd(0.0),
172-
// tail.runPoseZeroingCmmd().onlyIf(() -> !tail.isZeroed()),
173-
// tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
174-
// .until(
175-
// () -> elevator.inSetpointRange() || elevator.getSetpoint() >
176-
// elevator.getPose())
177-
// .andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_ANGLE))));
178-
179-
// .until(() -> Superstructure.isTailSafe(elevator, tail)),
180-
// tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS).until(() -> tail.inSetpointRange()),
181-
// tail.stopMotorInputCmmd()));
151+
debugController.a().whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE));
152+
debugController.rightBumper().whileTrue(tail.setTargetPoseCmmd(Tail.POSE_OUT_ANGLE));
153+
debugController.leftBumper().whileTrue(tail.setTargetPoseCmmd(Tail.POSE_IN_ANGLE));
154+
155+
elevator.setDefaultCommand(
156+
Commands.sequence(
157+
elevator.runPoseZeroingCmmd().onlyIf(() -> !elevator.isZeroed()),
158+
elevator
159+
.setTargetPoseCmmd(3.4)
160+
.until(
161+
() ->
162+
elevator.inSetpointRange() || elevator.getSetpoint() > elevator.getPose()),
163+
elevator.stopMotorInputCmmd()));
164+
165+
tail.setDefaultCommand(
166+
Commands.sequence(
167+
// tail.setRollerSpeedCmmd(0.0),
168+
tail.runPoseZeroingCmmd().onlyIf(() -> !tail.isZeroed()),
169+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
170+
.until(
171+
() -> elevator.inSetpointRange() || elevator.getSetpoint() > elevator.getPose())
172+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_ANGLE))));
173+
182174
/*
183175
driveController
184176
.x()
@@ -204,7 +196,7 @@ public Robot() {
204196
.a()
205197
.whileTrue(
206198
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
207-
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
199+
.until(() -> elevator.inSetpointRange())
208200
.andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_ANGLE)))
209201
.whileTrue(this.setElevatorToStowed().onlyIf(() -> tail.inSetpointRange()));
210202

@@ -213,7 +205,7 @@ public Robot() {
213205
.x()
214206
.whileTrue(
215207
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
216-
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
208+
.until(() -> elevator.inSetpointRange())
217209
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L2)))
218210
.whileTrue(
219211
elevator.setTargetPoseCmmd(Elevator.POSE_L2).onlyIf(() -> tail.inSetpointRange()));
@@ -223,7 +215,7 @@ public Robot() {
223215
.y()
224216
.whileTrue(
225217
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
226-
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
218+
.until(() -> elevator.inSetpointRange())
227219
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L3)))
228220
.whileTrue(
229221
elevator.setTargetPoseCmmd(Elevator.POSE_L3).onlyIf(() -> tail.inSetpointRange()));
@@ -233,13 +225,16 @@ public Robot() {
233225
.b()
234226
.whileTrue(
235227
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
236-
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
228+
.until(() -> elevator.inSetpointRange())
237229
.andThen(
238230
tail.setTargetPoseCmmd(Tail.POSE_L4)
239231
.until(joystick.leftBumper())
240232
.andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_ANGLE))))
241233
.whileTrue(
242-
elevator.setTargetPoseCmmd(Elevator.POSE_L4).onlyIf(() -> tail.inSetpointRange()));
234+
elevator
235+
.setTargetPoseCmmd(Elevator.POSE_L4)
236+
.onlyIf(() -> tail.inSetpointRange())
237+
.repeatedly());
243238

244239
// // * INTAKING CORAL COMMAND
245240
operatorController
@@ -252,15 +247,22 @@ public Robot() {
252247
.whileFalse(chute.runChuteSpeedCmmd(0.0));
253248

254249
// * AUTO CENTERING COMMAND
255-
256250
operatorController
257251
.povRight()
258252
.onTrue(
259-
tail.setRollerSpeedCmmd(.5)
260-
.alongWith(chute.runChuteSpeedCmmd(-.1))
261-
.until(() -> tail.beamTripped = true)
262-
.andThen(tail.setRollerTimecmmd(.5, 1))
263-
.alongWith(chute.runChuteSpeedCmmd(0)));
253+
tail.setRollerSpeedCmmd(1)
254+
.alongWith(chute.runChuteSpeedCmmd(-.2))
255+
.until(() -> tail.beamTripped == true)
256+
.andThen(tail.setRollerTimecmmd(1, 1).alongWith(chute.runChuteSpeedCmmd(0))));
257+
258+
joystick.x().whileTrue(tail.setRollerSpeedCmmd(-3)).whileFalse(tail.setRollerSpeedCmmd(0));
259+
// operatorController
260+
// .povRight()
261+
// .onTrue(
262+
// tail.setRollerSpeedCmmd(1)
263+
// .alongWith(chute.runChuteSpeedCmmd(-.2))
264+
// .until(() -> tail.beamTripped = true)
265+
// .andThen(tail.setRollerTimecmmd(.5, 1).alongWith(chute.runChuteSpeedCmmd(0))));
264266

265267
// // * OUTTAKING CORAL COMMAND
266268
operatorController
@@ -279,39 +281,76 @@ public Robot() {
279281
.onFalse(tail.setRollerSpeedCmmd(0.0));
280282

281283
// * ALGAE DESCORING
284+
285+
/*
282286
operatorController
283287
.povUp()
284288
.whileTrue(
285-
tail.setRollerSpeedCmmd(1)
286-
.alongWith(
287-
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
288-
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
289-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_HIGH))))
289+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
290+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
291+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_HIGH)))
292+
.whileTrue(tail.setRollerSpeedCmmd(1))
290293
.whileTrue(
291294
elevator
292295
.setTargetPoseCmmd(Elevator.POSE_ALGAE_HIGH)
293296
.onlyIf(() -> tail.inSetpointRange()))
294297
.whileFalse(tail.setRollerSpeedCmmd(0));
295-
298+
*/
296299
operatorController
297-
.povDown()
300+
.povUp()
298301
.whileTrue(
299-
tail.setRollerSpeedCmmd(1)
300-
.alongWith(
301-
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
302-
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
303-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_LOW))))
302+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
303+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
304+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_HIGH)))
304305
.whileTrue(
305306
elevator
306-
.setTargetPoseCmmd(Elevator.POSE_ALGAE_LOW)
307-
.onlyIf(() -> tail.inSetpointRange()))
308-
.whileFalse(tail.setRollerSpeedCmmd(0));
307+
.setTargetPoseCmmd(Elevator.POSE_ALGAE_HIGH)
308+
.onlyIf(() -> tail.inSetpointRange()));
309309

310310
operatorController
311311
.povDown()
312312
.whileTrue(
313-
tail.setTargetPoseCmmd(Tail.POSE_ALGAE_LOW).alongWith(tail.setRollerSpeedCmmd(1)))
314-
.whileFalse(tail.setRollerSpeedCmmd(0));
313+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
314+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
315+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_LOW)))
316+
.whileTrue(
317+
elevator
318+
.setTargetPoseCmmd(Elevator.POSE_ALGAE_LOW)
319+
.onlyIf(() -> tail.inSetpointRange()));
320+
321+
// operatorController
322+
// .povUp()
323+
// .whileTrue(
324+
// tail.setRollerSpeedCmmd(1)
325+
// .alongWith(
326+
// tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
327+
// .until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
328+
// .andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_HIGH))))
329+
// .whileTrue(
330+
// elevator
331+
// .setTargetPoseCmmd(Elevator.POSE_ALGAE_HIGH)
332+
// .onlyIf(() -> tail.inSetpointRange()))
333+
// .whileFalse(tail.setRollerSpeedCmmd(0));
334+
335+
// operatorController
336+
// .povDown()
337+
// .whileTrue(
338+
// tail.setRollerSpeedCmmd(1)
339+
// .alongWith(
340+
// tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
341+
// .until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
342+
// .andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_LOW))))
343+
// .whileTrue(
344+
// elevator
345+
// .setTargetPoseCmmd(Elevator.POSE_ALGAE_LOW)
346+
// .onlyIf(() -> tail.inSetpointRange()))
347+
// .whileFalse(tail.setRollerSpeedCmmd(0));
348+
349+
// operatorController
350+
// .povDown()
351+
// .whileTrue(
352+
// tail.setTargetPoseCmmd(Tail.POSE_ALGAE_LOW).alongWith(tail.setRollerSpeedCmmd(1)))
353+
// .whileFalse(tail.setRollerSpeedCmmd(0));
315354

316355
// * SUPERSTRUCTURE INIT
317356
// Map<Requests, Trigger> triggerMap = new HashMap<Superstructure.Requests, Trigger>();
@@ -330,10 +369,18 @@ public Robot() {
330369
operatorController2.getLevelTarget());
331370
*/
332371

333-
factory = new ChoreoFactory(drivetrain);
334-
chooser = factory.getChooser();
335-
SmartDashboard.putData("sdf", chooser);
336-
RobotModeTriggers.autonomous().whileTrue(chooser.selectedCommandScheduler());
372+
// factory = new ChoreoFactory(drivetrain);
373+
// chooser = factory.getChooser();
374+
Command auton =
375+
drivetrain
376+
.runOnce(() -> drivetrain.seedFieldCentric())
377+
.andThen(
378+
drivetrain
379+
.applyRequest(() -> drive.withVelocityX(MaxSpeed * 0.3))
380+
.withDeadline(new WaitCommand(1)));
381+
RobotModeTriggers.autonomous().whileTrue(auton);
382+
// SmartDashboard.putData("sdf", chooser);
383+
// RobotModeTriggers.autonomous().whileTrue(chooser.selectedCommandScheduler());
337384
}
338385

339386
private void configureBindings() {
@@ -386,13 +433,7 @@ public void robotPeriodic() {
386433
}
387434

388435
@Override
389-
public void autonomousInit() {
390-
Command auton =
391-
factory
392-
.getMove(); // .withDeadline(Commands.waitUntil(() -> DriverStation.isTeleopEnabled()));
393-
// if (auton != null) {
394-
auton.schedule();
395-
}
436+
public void autonomousInit() {}
396437

397438
@Override
398439
public void autonomousPeriodic() {}

‎src/main/java/wmironpatriots/commands/Autonomous.java‎

Lines changed: 21 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
import static wmironpatriots.Constants.DT_TIME;
1010

1111
import com.pathplanner.lib.auto.AutoBuilder;
12+
import com.pathplanner.lib.auto.NamedCommands;
1213
import com.pathplanner.lib.config.ModuleConfig;
1314
import com.pathplanner.lib.config.PIDConstants;
1415
import com.pathplanner.lib.config.RobotConfig;
@@ -18,11 +19,16 @@
1819
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
1920
import edu.wpi.first.wpilibj2.command.Command;
2021
import edu.wpi.first.wpilibj2.command.Commands;
22+
import edu.wpi.first.wpilibj2.command.ScheduleCommand;
23+
import edu.wpi.first.wpilibj2.command.WaitCommand;
24+
import wmironpatriots.subsystems.elevator.Elevator;
2125
import wmironpatriots.subsystems.swerve.Swerve;
2226
import wmironpatriots.subsystems.swerve.module.Module;
27+
import wmironpatriots.subsystems.tail.Tail;
2328

2429
public class Autonomous {
25-
public static SendableChooser<Command> configureAutons(Swerve swerve) {
30+
public static SendableChooser<Command> configureAutons(
31+
Swerve swerve, Tail tail, Elevator elevator) {
2632
RobotConfig robotConfig =
2733
new RobotConfig(
2834
Swerve.MASS_KG,
@@ -53,6 +59,20 @@ public static SendableChooser<Command> configureAutons(Swerve swerve) {
5359
},
5460
swerve);
5561

62+
Command algaeHigh =
63+
Commands.parallel(
64+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
65+
.repeatedly()
66+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
67+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_HIGH)),
68+
elevator
69+
.setTargetPoseCmmd(Elevator.POSE_ALGAE_HIGH)
70+
.repeatedly()
71+
.onlyIf(() -> tail.inSetpointRange()))
72+
.withDeadline(new WaitCommand(2));
73+
74+
NamedCommands.registerCommand("removeHigh", new ScheduleCommand(algaeHigh));
75+
5676
SendableChooser<Command> choser = AutoBuilder.buildAutoChooser();
5777
choser.addOption("play dead", Commands.none());
5878
return choser;

0 commit comments

Comments
 (0)