Skip to content

Commit df668dc

Browse files
bobopop787bobopop787
committed
more info on shuffleboard
- move stuff around to make it easier to read for drivers - log error heading properly - isaligned logic fix - easier boolean log of reefaimed - elevator factor don't impact already slow aiming speeds, higher P
1 parent 1bae822 commit df668dc

File tree

4 files changed

+36
-26
lines changed

4 files changed

+36
-26
lines changed

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ public static enum FieldType {
4848
.add("Field Type", kFieldType)
4949
.withWidget(BuiltInWidgets.kComboBoxChooser)
5050
.withSize(2, 1)
51-
.withPosition(0, 0);
51+
.withPosition(0, 3);
5252
}
5353

5454
/**
@@ -112,8 +112,8 @@ public static class SwerveConstants { // all swerve on CAN ID range 1-9
112112
);
113113

114114
public static final ControlConstants kPresetPosControlConstants = new ControlConstants(
115-
"SwervePresetRot",
116-
2,
115+
"SwervePresetPos",
116+
2.5,
117117
0,
118118
0,
119119
0,

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,7 @@ private void configureBindings() {
101101
overrideElevatorFactorCombo.getTrigger().onTrue(Commands.runOnce(() -> {}));
102102
Shuffleboard.getTab("Teleoperated")
103103
.addBoolean("EleFact Override", overrideElevatorFactorCombo.getTrigger()).
104-
withPosition(8, 1)
104+
withPosition(8, 0)
105105
.withSize(1, 1);
106106

107107
swerve.setToAimSuppliers(

‎src/main/java/frc/robot/subsystems/elevator/Elevator.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ public Elevator(ElevatorIO io) {
5151
.withPosition(2, 2)
5252
.withSize(1, 1);
5353
teleopTab.addBoolean("Home Bounce", () -> bouncing)
54-
.withPosition(7, 1)
54+
.withPosition(7, 0)
5555
.withSize(1, 1);
5656
}
5757

‎src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java‎

Lines changed: 31 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -94,6 +94,7 @@ public class SwerveDrive extends SubsystemBase {
9494
private double errorX;
9595
private double errorY;
9696
private double errorHeading;
97+
private boolean reefAimed;
9798

9899
private SendableChooser<FieldZones> tempZoneViewerChooser;
99100

@@ -150,6 +151,7 @@ private void initMathModels() {
150151
errorX = 10;
151152
errorY = 10;
152153
errorHeading = 1000;
154+
reefAimed = false;
153155
}
154156

155157
private void initAutoDashboards() {
@@ -161,7 +163,10 @@ private void initAutoDashboards() {
161163
for(FieldZones zone : FieldZones.values()) {
162164
if(!zone.equals(FieldZones.OPPOSITE)) tempZoneViewerChooser.addOption(zone.name(), zone);
163165
}
164-
Shuffleboard.getTab("Teleoperated").add(tempZoneViewerChooser).withWidget(BuiltInWidgets.kComboBoxChooser).withPosition(7, 0).withSize(2, 1);
166+
Shuffleboard.getTab("Teleoperated").add(tempZoneViewerChooser)
167+
.withWidget(BuiltInWidgets.kComboBoxChooser)
168+
.withPosition(7, 3)
169+
.withSize(2, 1);
165170

166171
ShuffleboardTab teleopTab = Shuffleboard.getTab("Teleoperated");
167172
teleopTab.addBoolean("toX", () -> toX)
@@ -171,11 +176,21 @@ private void initAutoDashboards() {
171176
.withPosition(0, 1)
172177
.withSize(2, 1);
173178
teleopTab.addString("Desired Preset Position", () -> desiredPresetPosition.name())
174-
.withPosition(0, 3)
179+
.withPosition(0, 0)
175180
.withSize(2, 1);
176181
teleopTab.addBoolean("Aligned", this::isAligned)
177182
.withPosition(3, 3)
178183
.withSize(1, 1);
184+
teleopTab.addNumber("xError", () -> errorX)
185+
.withPosition(7, 1);
186+
teleopTab.addNumber("yError", () -> errorY)
187+
.withPosition(8, 1);
188+
teleopTab.addNumber("headingError", () -> errorHeading)
189+
.withPosition(7, 2)
190+
.withSize(1, 1);
191+
teleopTab.addBoolean("reefAimed", () -> reefAimed)
192+
.withPosition(8, 2)
193+
.withSize(1, 1);
179194

180195
presetVisualizerField = new Field2d();
181196
odometryField = new Field2d();
@@ -306,7 +321,8 @@ public void injectPresetPosition(ChassisSpeeds chassisSpeeds, boolean fieldRelat
306321
errorPose = errorPose.rotateAround(Translation2d.kZero, desiredPose.getRotation().unaryMinus());
307322
Logger.recordOutput("Swerve/desiredPose", desiredPose);
308323
Logger.recordOutput("Swerve/errorPose", errorPose);
309-
double vyReefRelative = presetPosController.calculate(errorPose.getY(), 0) * elevatorSpeedFactor;
324+
double vyReefRelative = presetPosController.calculate(errorPose.getY(), 0);
325+
if(vyReefRelative > 0.4) vyReefRelative *= elevatorSpeedFactor;
310326
Logger.recordOutput("Swerve/vyReefRelative", vyReefRelative);
311327

312328
fieldRelativeSpeeds = fieldRelative ?
@@ -528,18 +544,6 @@ public boolean isAligned() {
528544
return isAligned;
529545
}
530546

531-
public double getErrorX() {
532-
return errorX;
533-
}
534-
535-
public double getErrorY() {
536-
return errorY;
537-
}
538-
539-
public double getErrorHeading() {
540-
return errorHeading;
541-
}
542-
543547
private ChassisSpeeds flipChassisSpeeds(ChassisSpeeds chassisSpeeds) {
544548
ChassisSpeeds flipped = chassisSpeeds;
545549
flipped.vxMetersPerSecond *= -1;
@@ -647,21 +651,27 @@ public void periodic() {
647651
if(fieldZone.equals(FieldZones.OPPOSITE)) {
648652
isAligned = false;
649653
} else {
654+
Pose2d leftErrorPose = new Pose2d(getPose().getX() - fieldZone.leftReefPose.getX(), getPose().getY() - fieldZone.leftReefPose.getY(), getPose().getRotation().minus(fieldZone.leftReefPose.getRotation()));
655+
Pose2d rightErrorPose = new Pose2d(getPose().getX() - fieldZone.rightReefPose.getX(), getPose().getY() - fieldZone.rightReefPose.getY(), getPose().getRotation().minus(fieldZone.rightReefPose.getRotation()));
650656
if(desiredPresetPosition.equals(PresetPositionType.LEFTREEF) || desiredPresetPosition.equals(PresetPositionType.RIGHTREEF)) {
651657
Pose2d desiredPose = desiredPresetPosition.equals(PresetPositionType.LEFTREEF) ? fieldZone.leftReefPose : fieldZone.rightReefPose;
652-
Pose2d errorPose = new Pose2d(getPose().getX() - desiredPose.getX(), getPose().getY() - desiredPose.getY(), getPose().getRotation());
658+
Pose2d errorPose = new Pose2d(getPose().getX() - desiredPose.getX(), getPose().getY() - desiredPose.getY(), getPose().getRotation().minus(desiredPose.getRotation()));
653659
if(Math.abs(errorPose.getX()) < 0.05 && Math.abs(errorPose.getY()) < 0.02) isAligned = true;
654660
else isAligned = false;
655661
Logger.recordOutput("Swerve/isAlignedErrorPose", errorPose);
656662
errorX = errorPose.getX();
657663
errorY = errorPose.getY();
658664
errorHeading = errorPose.getRotation().getDegrees();
665+
reefAimed = true;
666+
} else {
667+
if((Math.abs(leftErrorPose.getX()) < 0.04 && Math.abs(leftErrorPose.getY()) < 0.02)
668+
|| Math.abs(rightErrorPose.getX()) < 0.04 && Math.abs(rightErrorPose.getY()) < 0.02) isAligned = true;
669+
else isAligned = false;
670+
errorX = Math.min(leftErrorPose.getX(), rightErrorPose.getX());
671+
errorY = Math.min(leftErrorPose.getY(), rightErrorPose.getY());
672+
errorHeading = Math.min(leftErrorPose.getRotation().getDegrees(), rightErrorPose.getRotation().getDegrees());
673+
reefAimed = false;
659674
}
660-
Pose2d leftErrorPose = new Pose2d(getPose().getX() - fieldZone.leftReefPose.getX(), getPose().getY() - fieldZone.leftReefPose.getY(), getPose().getRotation());
661-
Pose2d rightErrorPose = new Pose2d(getPose().getX() - fieldZone.rightReefPose.getX(), getPose().getY() - fieldZone.rightReefPose.getY(), getPose().getRotation());
662-
if((Math.abs(leftErrorPose.getX()) < 0.04 && Math.abs(leftErrorPose.getY()) < 0.02)
663-
|| Math.abs(rightErrorPose.getX()) < 0.04 && Math.abs(rightErrorPose.getY()) < 0.02) isAligned = true;
664-
else isAligned = false;
665675
Logger.recordOutput("Swerve/isAlignedErrorPoseLeft", leftErrorPose);
666676
Logger.recordOutput("Swerve/isAlignedErrorPoseRight", rightErrorPose);
667677
}

0 commit comments

Comments
 (0)