Skip to content

Commit 0c9af5f

Browse files
caparomulaPez357
andauthored
Vahay (#138)
* Switch to field April tags * Added Aapril tag json, removed reef offset, updated climber servo constants * Remove reef offset todo * morning tweaks * Fix elevator rocket * Move zorro controller command into teleop init. * sycronized blue and red auto logic * removed withtimout on elevator * added timout to both red and blue L4autos * Set default stop command for swerve in autonomous * Prevent elevator rocket in joystick command * made the elevator and robot move at the same time. * Tweaked the choro by 3 inches * Widen tolerance for pose seek display * Remove init from lifter joystick command * Tweaked the redCenterToL4G choreo * commented out the createjoystickcontrolcommand and tweaked choreo. * Another attempt to prevent skyrocket on teleop init * Adjusted path for 1 piece auto * Add a few metrics to smart dashboard * Slowed down the red Auto in Choero * Adjust Al;gae L3 height, comment out algae wrist set angle on grab * spotless --------- Co-authored-by: Pez357 <[email protected]>
1 parent 80a68bb commit 0c9af5f

File tree

15 files changed

+666
-228
lines changed

15 files changed

+666
-228
lines changed

src/main/deploy/2025-reefscape-andymark.json

Lines changed: 404 additions & 0 deletions
Large diffs are not rendered by default.

src/main/deploy/choreo/blueCenterToL4G.traj

Lines changed: 94 additions & 91 deletions
Large diffs are not rendered by default.

src/main/deploy/choreo/redCenterToL4G.traj

Lines changed: 105 additions & 91 deletions
Large diffs are not rendered by default.

src/main/java/frc/game/Reef.java

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,7 @@
1818
* to a pose.
1919
*/
2020
public enum Reef {
21-
// TODO remove reef ceter offset
22-
Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5 + 4.5), Rotation2d.kZero)),
21+
Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5), Rotation2d.kZero)),
2322
Red(new Pose2d(Inches.of(514.125), Inches.of(158.5), Rotation2d.kPi));
2423

2524
/** The radius of the reef hexagon */
@@ -75,10 +74,10 @@ public int getSector(Pose2d atPose) {
7574
var delta = centerPose.getTranslation().minus(atPose.getTranslation());
7675
int vectorAngle = (int) delta.getAngle().getDegrees();
7776
int rayAngle = (vectorAngle + 30 + 360 + (int) centerPose.getRotation().getDegrees()) % 360;
78-
SmartDashboard.putNumber("Sector delta X", delta.getX());
79-
SmartDashboard.putNumber("Sector delta Y", delta.getY());
80-
SmartDashboard.putNumber("Sector vector delta", vectorAngle);
81-
SmartDashboard.putNumber("Sector ray angle", rayAngle);
77+
// SmartDashboard.putNumber("Sector delta X", delta.getX());
78+
// SmartDashboard.putNumber("Sector delta Y", delta.getY());
79+
// SmartDashboard.putNumber("Sector vector delta", vectorAngle);
80+
// SmartDashboard.putNumber("Sector ray angle", rayAngle);
8281
return rayAngle / 60;
8382
}
8483

src/main/java/frc/robot/Constants.java

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,6 @@
1414
import edu.wpi.first.units.measure.Distance;
1515
import edu.wpi.first.units.measure.LinearVelocity;
1616
import edu.wpi.first.units.measure.Time;
17-
import edu.wpi.first.wpilibj.Filesystem;
1817
import edu.wpi.first.wpilibj.TimedRobot;
1918
import edu.wpi.first.wpilibj.util.Color;
2019
import frc.game.FeederStation;
@@ -39,9 +38,6 @@ public static final class NEOVortexConstants {
3938
}
4039

4140
public static final class VisionConstants {
42-
public static final String kAprilTagLayoutPath =
43-
Filesystem.getDeployDirectory() + "/" + "stemgym.json";
44-
4541
// Define the standard deviations for the pose estimator, which determine how fast the pose
4642
// estimate converges to the vision measurement. This should depend on the vision measurement
4743
// noise and how many or how frequently vision measurements are applied to the pose estimator.
@@ -228,8 +224,8 @@ public static final class RotationControllerGains {
228224
public static final class ClimberConstants {
229225
public static final int kClimberPort = 17;
230226
public static final int kRatchetServoPort = 1;
231-
public static final double kEngagedPosition = 0 / 1024.0;
232-
public static final double kDisengedPosition = 1024.0 / 1024.0;
227+
public static final double kEngagedPosition = 800 / 1024.0;
228+
public static final double kDisengedPosition = 950 / 1024.0;
233229

234230
public static final int kCageSensorPort = 6;
235231

src/main/java/frc/robot/LEDs/LEDs.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -326,10 +326,10 @@ public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) {
326326
Segments.MIDDLE);
327327

328328
var x = delta.getTranslation().getMeasureX().in(Centimeters);
329-
fill(Math.abs(x) < 3 ? Color.kWhite : x > 0 ? Color.kGreen : Color.kRed, Segments.TOP);
329+
fill(Math.abs(x) < 5 ? Color.kWhite : x > 0 ? Color.kGreen : Color.kRed, Segments.TOP);
330330

331331
var y = delta.getTranslation().getMeasureY().in(Centimeters);
332-
if (Math.abs(y) < 3) {
332+
if (Math.abs(y) < 6) {
333333
fill(Color.kWhite, Segments.BOTTOM);
334334
} else {
335335
fill(Color.kGreen, y > 0 ? Segments.leftBottom : Segments.rightBottom);

src/main/java/frc/robot/Robot.java

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,6 @@
4848
import frc.robot.elevator.AlgaeWrist;
4949
import frc.robot.elevator.CoralRoller;
5050
import frc.robot.elevator.Elevator;
51-
import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
5251
import frc.robot.elevator.Lifter;
5352
import frc.robot.vision.Vision;
5453
import java.util.HashMap;
@@ -125,9 +124,6 @@ public void robotInit() {
125124
DataLogManager.start();
126125
DriverStation.startDataLog(DataLogManager.getLog());
127126

128-
swerve.setDefaultCommand(
129-
new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID()));
130-
131127
reefTargetPositionsPublisher.set(DriveConstants.kReefTargetPoses);
132128
}
133129

@@ -199,6 +195,7 @@ public void disabledPeriodic() {
199195
@Override
200196
public void autonomousInit() {
201197
autoSelector.scheduleAuto();
198+
swerve.setDefaultCommand(swerve.createStopCommand());
202199
lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand());
203200
leds.replaceDefaultCommandImmediately(
204201
leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
@@ -210,7 +207,13 @@ public void autonomousPeriodic() {}
210207
@Override
211208
public void teleopInit() {
212209
autoSelector.cancelAuto();
210+
swerve.setDefaultCommand(
211+
new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID()));
212+
213+
lifter.matchHeight();
214+
lifter.resetController();
213215
lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
216+
// lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
214217
leds.replaceDefaultCommandImmediately(
215218
leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
216219

@@ -343,7 +346,7 @@ public boolean getAsBoolean() {
343346
// Force joystick operation of the elevator
344347
Trigger elevatorTriggerHigh = operator.axisGreaterThan(Axis.kLeftY.value, 0.9, loop).debounce(0.1);
345348
Trigger elevatorTriggerLow = operator.axisGreaterThan(Axis.kLeftY.value, -0.9, loop).debounce(0.1);
346-
elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.createJoystickControlCommand(operator.getHID()));
349+
// elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.createJoystickControlCommand(operator.getHID()));
347350

348351
// Actuate climber winch
349352
// Trigger climbTrigger = operator.axisGreaterThan(Axis.kRightY.value, -0.9, loop).debounce(0.1);
@@ -383,8 +386,8 @@ private void configureEventBindings() {
383386

384387
algaeRoller.hasAlgae
385388
.whileTrue(algaeRoller.createHoldAlgaeCommand());
386-
algaeRoller.hasAlgae
387-
.onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
389+
// algaeRoller.hasAlgae
390+
// .onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
388391
coralRoller.isRolling.or(algaeRoller.isRolling).whileTrue(createRollerAnimationCommand());
389392
}
390393

src/main/java/frc/robot/auto/BlueL4Auto.java

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -48,9 +48,7 @@ public AutoRoutine getAutoRoutine() {
4848
blueL4AutoRoutine.active().onTrue(
4949
Commands.parallel(
5050
blueCenterToL4G.cmd(),
51-
Commands.sequence(
52-
Commands.waitSeconds(1.0),
53-
elevator.coralL4PositionCG())));
51+
elevator.coralL4PositionCG().withTimeout(2.0)));
5452

5553
blueCenterToL4G.done().onTrue(
5654
Commands.sequence(

src/main/java/frc/robot/auto/RedL4Auto.java

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -45,13 +45,15 @@ public Optional<Pose2d> getInitialPose() {
4545
public AutoRoutine getAutoRoutine() {
4646

4747
// spotless:off
48-
redL4AutoRoutine.active().onTrue(redCenterToL4G.cmd());
48+
redL4AutoRoutine.active().onTrue(
49+
Commands.parallel(
50+
redCenterToL4G.cmd(),
51+
elevator.coralL4PositionCG().withTimeout(2.0)));
52+
4953

5054
redCenterToL4G.done().onTrue(
5155
Commands.sequence(
52-
Commands.waitSeconds(0.1),
53-
elevator.coralL4PositionCG(),
54-
Commands.waitSeconds(0.1),
56+
Commands.waitSeconds(0.1),
5557
coralRoller.createOuttakeCommand().withTimeout(0.2),
5658
Commands.waitSeconds(0.2)));
5759

src/main/java/frc/robot/drivetrain/Drivetrain.java

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -192,6 +192,10 @@ public void setHeadingOffset() {
192192
fieldRotatedSupplier.getAsBoolean() ? getHeading().rotateBy(Rotation2d.kPi) : getHeading();
193193
}
194194

195+
public Command createStopCommand() {
196+
return this.run(() -> setRobotRelativeChassisSpeeds(new ChassisSpeeds()));
197+
}
198+
195199
/**
196200
* @return Array of swerve module positions
197201
*/

0 commit comments

Comments
 (0)