Skip to content

Commit 39b2128

Browse files
dabeycorndabeycorn
committed
HVR day 1
Signed-off-by: dabeycorn <[email protected]>
1 parent 3e871b7 commit 39b2128

File tree

17 files changed

+929
-149
lines changed

17 files changed

+929
-149
lines changed

‎src/main/java/wmironpatriots/Constants.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -55,10 +55,10 @@ public class Constants {
5555
: Optional.empty();
5656

5757
public static enum LevelTarget {
58-
L1(Elevator.POSE_L1, Tail.POSE_MIN_REVS),
59-
L2(Elevator.POSE_L2, Tail.POSE_MIN_REVS),
60-
L3(Elevator.POSE_L3, Tail.POSE_MIN_REVS),
61-
L4(Elevator.POSE_L4, Tail.POSE_MAX_REVS);
58+
L1(Elevator.POSE_L1, Tail.POSE_IN_ANGLE),
59+
L2(Elevator.POSE_L2, Tail.POSE_IN_ANGLE),
60+
L3(Elevator.POSE_L3, Tail.POSE_IN_ANGLE),
61+
L4(Elevator.POSE_L4, Tail.POSE_OUT_ANGLE);
6262
public final double elevatorPoseRevs, tailPoseRads;
6363

6464
private LevelTarget(double elevatorPoseRevs, double tailPoseRads) {

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

Lines changed: 120 additions & 95 deletions
Original file line numberDiff line numberDiff line change
@@ -6,15 +6,18 @@
66

77
package wmironpatriots;
88

9+
import static edu.wpi.first.units.Units.MetersPerSecond;
10+
import static edu.wpi.first.units.Units.RadiansPerSecond;
11+
import static edu.wpi.first.units.Units.RotationsPerSecond;
912
import static wmironpatriots.Constants.SWERVE_SIM_CONFIG;
1013

1114
import com.ctre.phoenix6.SignalLogger;
15+
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
16+
import com.ctre.phoenix6.swerve.SwerveRequest;
1217
import edu.wpi.first.math.geometry.Pose2d;
1318
import edu.wpi.first.math.geometry.Rotation2d;
1419
import edu.wpi.first.wpilibj.DriverStation;
1520
import edu.wpi.first.wpilibj.TimedRobot;
16-
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
17-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1821
import edu.wpi.first.wpilibj2.command.Command;
1922
import edu.wpi.first.wpilibj2.command.CommandScheduler;
2023
import edu.wpi.first.wpilibj2.command.Commands;
@@ -29,37 +32,54 @@
2932
import monologue.Monologue.MonologueConfig;
3033
import org.ironmaple.simulation.SimulatedArena;
3134
import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
32-
import wmironpatriots.commands.Autonomous;
35+
import wmironpatriots.generated.TunerConstants;
36+
import wmironpatriots.subsystems.CommandSwerveDrivetrain;
3337
import wmironpatriots.subsystems.Superstructure;
3438
import wmironpatriots.subsystems.Superstructure.Requests;
3539
import wmironpatriots.subsystems.chute.Chute;
3640
import wmironpatriots.subsystems.chute.ChuteIOComp;
3741
import wmironpatriots.subsystems.elevator.Elevator;
3842
import wmironpatriots.subsystems.elevator.ElevatorIOComp;
3943
import wmironpatriots.subsystems.elevator.ElevatorIOSim;
40-
import wmironpatriots.subsystems.swerve.Swerve;
4144
import wmironpatriots.subsystems.tail.Tail;
4245
import wmironpatriots.subsystems.tail.TailIOComp;
4346
import wmironpatriots.subsystems.tail.TailIOSim;
44-
import wmironpatriots.util.deviceUtil.InputStream;
4547
import wmironpatriots.util.deviceUtil.OperatorController;
4648

4749
public class Robot extends TimedRobot implements Logged {
4850
private final CommandScheduler scheduler = CommandScheduler.getInstance();
4951

50-
private final CommandXboxController driveController;
52+
private final CommandXboxController joystick;
5153
private final CommandXboxController operatorController;
5254

5355
private final OperatorController operatorController2;
5456

55-
private final Swerve swerve;
5657
private final Tail tail;
5758
private final Elevator elevator;
5859
private final Chute chute;
5960

6061
private final RobotVisualizer visualizer;
6162

62-
private final SendableChooser<Command> autonChooser;
63+
private double MaxSpeed =
64+
TunerConstants.kSpeedAt12Volts.in(MetersPerSecond)
65+
* 0.75; // kSpeedAt12Volts desired top speed
66+
private double MaxAngularRate =
67+
RotationsPerSecond.of(0.75)
68+
.in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
69+
70+
/* Setting up bindings for necessary control of the swerve drive platform */
71+
private final SwerveRequest.FieldCentric drive =
72+
new SwerveRequest.FieldCentric()
73+
.withDeadband(MaxSpeed * 0.1)
74+
.withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
75+
.withDriveRequestType(
76+
DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
77+
private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
78+
private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
79+
80+
private final Telemetry logger = new Telemetry(MaxSpeed);
81+
82+
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
6383

6484
public Robot() {
6585
// * MONOLOGUE SETUP
@@ -104,7 +124,7 @@ public Robot() {
104124
});
105125

106126
// * SUBSYSTEM INIT
107-
driveController = new CommandXboxController(0);
127+
joystick = new CommandXboxController(0);
108128
operatorController = new CommandXboxController(1);
109129

110130
operatorController2 = new OperatorController(3);
@@ -116,7 +136,6 @@ public Robot() {
116136
SWERVE_SIM_CONFIG.get(), new Pose2d(3, 3, new Rotation2d())))
117137
: Optional.empty();
118138

119-
swerve = new Swerve(swerveSim);
120139
if (Robot.isReal()) {
121140
tail = new TailIOComp();
122141
elevator = new ElevatorIOComp();
@@ -130,64 +149,36 @@ public Robot() {
130149
// Setup simulated arena if simulated
131150
if (Robot.isSimulation()) {
132151
SimulatedArena.getInstance().addDriveTrainSimulation(swerveSim.orElse(null));
133-
swerve.resetOdo(swerveSim.get().getSimulatedDriveTrainPose());
134152
}
135153

136154
// * DEFAULT COMMANDS
137155
// Set up driver input streams
138-
double maxSpeed = swerve.MAX_LINEAR_SPEED_MPS;
139-
double maxAngularSpeed = swerve.MAX_ANGULAR_SPEED_RADS_PER_SEC;
140-
141-
InputStream x = InputStream.of(driveController::getLeftY);
142-
InputStream y = InputStream.of(driveController::getLeftX);
143-
144-
InputStream hypot =
145-
InputStream.hypot(y, x).clamp(1).deadband(0.05, 1.0).signedPow(2).scale(maxSpeed);
146-
147-
InputStream theta = InputStream.arcTan(y, x);
148-
x = hypot.scale(hypot.scale(theta.map(Math::cos)));
149-
y = hypot.scale(hypot.scale(theta.map(Math::sin)));
150-
151-
InputStream omega =
152-
InputStream.of(driveController::getRightX)
153-
.clamp(1.0)
154-
.deadband(0.05, 1.0)
155-
.signedPow(2.0)
156-
.scale(maxAngularSpeed);
157-
158-
swerve.setDefaultCommand(swerve.teleopSwerveCmmd(x, y, omega));
159-
160-
elevator.setDefaultCommand(
161-
Commands.sequence(
162-
elevator.runPoseZeroingCmmd().onlyIf(() -> !elevator.isZeroed()),
163-
elevator
164-
.setTargetPoseCmmd(1.0)
165-
.until(
166-
() ->
167-
elevator.inSetpointRange() || elevator.getSetpoint() > elevator.getPose()),
168-
elevator.stopMotorInputCmmd()));
169-
170-
tail.setDefaultCommand(
171-
Commands.sequence(
172-
tail.setRollerSpeedCmmd(0.0),
173-
tail.runPoseZeroingCmmd()
174-
.onlyIf(() -> !tail.isZeroed() && Superstructure.isTailSafe(elevator, tail)),
175-
tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)));
176-
177-
chute.setDefaultCommand(chute.runChuteSpeedCmmd(0.0));
178-
156+
configureBindings();
157+
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+
179179
// .until(() -> Superstructure.isTailSafe(elevator, tail)),
180180
// tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS).until(() -> tail.inSetpointRange()),
181181
// 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));
191182
/*
192183
driveController
193184
.x()
@@ -206,66 +197,72 @@ public Robot() {
206197
207198
driveController.b().whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L3));
208199
*/
209-
// driveController.x().whileTrue(tail.setRollerSpeedCmmd(1)).onFalse(tail.setRollerSpeedCmmd(0.0));
200+
// joystick.x().whileTrue(tail.setRollerSpeedCmmd(1)).onFalse(tail.setRollerSpeedCmmd(0.0));
210201

211202
// * ELEVATOR LEVEL 1 COMMAND
212203
operatorController
213204
.a()
214205
.whileTrue(
215-
tail.setTargetPoseCmmd(Tail.POSE_MOVE_REVS)
206+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
216207
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
217-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
208+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_ANGLE)))
218209
.whileTrue(this.setElevatorToStowed().onlyIf(() -> tail.inSetpointRange()));
219210

220-
// * ELEVATOR LEVEL 2 COMMAND
211+
// // * ELEVATOR LEVEL 2 COMMAND
221212
operatorController
222213
.x()
223214
.whileTrue(
224-
tail.setTargetPoseCmmd(Tail.POSE_MOVE_REVS)
215+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
225216
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
226217
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L2)))
227218
.whileTrue(
228219
elevator.setTargetPoseCmmd(Elevator.POSE_L2).onlyIf(() -> tail.inSetpointRange()));
229220

230-
// * ELEVATOR LEVEL 3 COMMAND
221+
// // * ELEVATOR LEVEL 3 COMMAND
231222
operatorController
232223
.b()
233224
.whileTrue(
234-
tail.setTargetPoseCmmd(Tail.POSE_MOVE_REVS)
235-
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
236-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L3)))
225+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
226+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
227+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L3)))
237228
.whileTrue(
238-
elevator.setTargetPoseCmmd(Elevator.POSE_L3).onlyIf(() -> tail.inSetpointRange()));
229+
elevator.setTargetPoseCmmd(Elevator.POSE_L3).onlyIf(() -> tail.inSetpointRange()));
239230

240-
// * ELEVATOR LEVEL 4 COMMAND
231+
// // * ELEVATOR LEVEL 4 COMMAND
241232
operatorController
242233
.y()
243234
.whileTrue(
244-
tail.setTargetPoseCmmd(Tail.POSE_MOVE_REVS)
245-
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
246-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L4)))
235+
tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
236+
.until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
237+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_L4)))
247238
.whileTrue(
248-
elevator.setTargetPoseCmmd(Elevator.POSE_L4).onlyIf(() -> tail.inSetpointRange()));
239+
elevator.setTargetPoseCmmd(Elevator.POSE_L4).onlyIf(() -> tail.inSetpointRange()));
249240

250-
// * INTAKING CORAL COMMAND
241+
// // * INTAKING CORAL COMMAND
251242
operatorController
252243
.leftBumper()
253-
.whileTrue(chute.runChuteSpeedCmmd(Chute.INTAKE_SPEED)
254-
.alongWith(tail.setRollerSpeedCmmd(Tail.INTAKING_SPEEDS)));
244+
.whileTrue(
245+
chute
246+
.runChuteSpeedCmmd(Chute.INTAKE_SPEED)
247+
.alongWith(tail.setRollerSpeedCmmd(Tail.INTAKING_SPEEDS)))
248+
.whileFalse(tail.setRollerSpeedCmmd(0))
249+
.whileFalse(chute.runChuteSpeedCmmd(0.0));
255250

256-
// * OUTTAKING CORAL COMMAND
251+
// // * OUTTAKING CORAL COMMAND
257252
operatorController
258253
.rightBumper()
259254
.whileTrue(
260255
chute
261256
.runChuteSpeedCmmd(Chute.OUTAKE_SPEED)
262-
.alongWith(tail.setRollerSpeedCmmd(Tail.OUTTAKING_SPEEDS)));
263-
257+
.alongWith(tail.setRollerSpeedCmmd(Tail.OUTTAKING_SPEEDS)))
258+
.whileFalse(tail.setRollerSpeedCmmd(0))
259+
.whileFalse(chute.runChuteSpeedCmmd(0.0));
264260

265-
// * SCORING CORAL COMMAND
266-
driveController
261+
// // * SCORING CORAL COMMAND
262+
joystick
267263
.rightBumper()
268-
.whileTrue(tail.setRollerSpeedCmmd(Tail.OUTPUTTING_SPEEDS));
264+
.whileTrue(tail.setRollerSpeedCmmd(Tail.OUTPUTTING_SPEEDS))
265+
.onFalse(tail.setRollerSpeedCmmd(0.0));
269266

270267
// * SUPERSTRUCTURE INIT
271268
Map<Requests, Trigger> triggerMap = new HashMap<Superstructure.Requests, Trigger>();
@@ -274,8 +271,6 @@ public Robot() {
274271
visualizer = new RobotVisualizer(elevator, tail);
275272

276273
// * AUTON INIT
277-
autonChooser = Autonomous.configureAutons(swerve);
278-
SmartDashboard.putData("Select Auton", autonChooser);
279274
/*
280275
new Superstructure(
281276
elevator,
@@ -287,6 +282,44 @@ public Robot() {
287282
*/
288283
}
289284

285+
private void configureBindings() {
286+
// Note that X is defined as forward according to WPILib convention,
287+
// and Y is defined as to the left according to WPILib convention.
288+
drivetrain.setDefaultCommand(
289+
// Drivetrain will execute this command periodically
290+
drivetrain.applyRequest(
291+
() ->
292+
drive
293+
.withVelocityX(-joystick.getLeftY() * MaxSpeed)
294+
.withVelocityY(
295+
-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left)
296+
.withRotationalRate(
297+
joystick.getRightX()
298+
* MaxAngularRate) // Drive counterclockwise with negative X (left)
299+
));
300+
301+
joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
302+
joystick
303+
.b()
304+
.whileTrue(
305+
drivetrain.applyRequest(
306+
() ->
307+
point.withModuleDirection(
308+
new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
309+
310+
// Run SysId routines when holding back/start and X/Y.
311+
// Note that each routine should be run exactly once in a single log.
312+
// joystick.povDown().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
313+
// joystick.povDown().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
314+
// joystick.povUp().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
315+
// joystick.povUp().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
316+
317+
// reset the field-centric heading on left bumper press
318+
joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
319+
320+
drivetrain.registerTelemetry(logger::telemeterize);
321+
}
322+
290323
@Override
291324
public void robotPeriodic() {
292325
CommandScheduler.getInstance().run();
@@ -300,14 +333,6 @@ public void robotPeriodic() {
300333

301334
@Override
302335
public void autonomousInit() {
303-
Command auton =
304-
autonChooser
305-
.getSelected()
306-
.withDeadline(Commands.waitUntil(() -> DriverStation.isEnabled()));
307-
308-
if (auton != null) {
309-
auton.schedule();
310-
}
311336
}
312337

313338
@Override

0 commit comments

Comments
 (0)