Skip to content

Commit 87df4ba

Browse files
bobopop787bobopop787
committed
post-competition cleanup
- increase current limits appropriately for swerve and climber - nicer default temp pose heading for practice fields - lower elevator at home - don't overslow movement on preset pos from elevator factor - clean up error pose is aligned check
1 parent 0df5e2c commit 87df4ba

3 files changed

Lines changed: 40 additions & 16 deletions

File tree

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

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -94,6 +94,10 @@ public static class SwerveConstants { // all swerve on CAN ID range 1-9
9494

9595
public static final double kSlowedMult = 0.12;
9696

97+
public static final double kReefAlignXTolerance = 0.02; // meters
98+
public static final double kReefAlignYTolerance = 0.015; // meters
99+
public static final double kReefAlignHeadingTolerance = 2; // degrees
100+
97101
public static final SwerveDriveKinematics kSwerveKinematics = new SwerveDriveKinematics( //! make sure these are the right order, FRONT left right, BACK left right
98102
new Translation2d(-SwerveConstants.kWheelDistanceMeters / 2, SwerveConstants.kWheelDistanceMeters / 2),
99103
new Translation2d(SwerveConstants.kWheelDistanceMeters / 2, SwerveConstants.kWheelDistanceMeters / 2), // I REALLY DONT KNOW ANYMORE
@@ -113,7 +117,7 @@ public static class SwerveConstants { // all swerve on CAN ID range 1-9
113117

114118
public static final ControlConstants kPresetPosControlConstants = new ControlConstants(
115119
"SwervePresetPos",
116-
2.5,
120+
2.5, // TODO probably turn up
117121
0,
118122
0,
119123
0,
@@ -126,7 +130,7 @@ public static class SwerveConstants { // all swerve on CAN ID range 1-9
126130
// Preferences.initDouble("kSwerveTestDrive", 0);
127131
Preferences.initDouble("odometry/setPoseX", 0);
128132
Preferences.initDouble("odometry/setPoseY", 0);
129-
Preferences.initDouble("odometry/setPoseRot", 0);
133+
Preferences.initDouble("odometry/setPoseRot", Units.degreesToRadians(120));
130134
System.out.println("SwerveConstants initialized");
131135
}
132136
}
@@ -180,7 +184,7 @@ public static class SwerveModuleConstants {
180184

181185
driveConfig
182186
.idleMode(IdleMode.kBrake)
183-
.smartCurrentLimit(30)
187+
.smartCurrentLimit(40)
184188
.voltageCompensation(12)
185189
.closedLoopRampRate(0.01)
186190
; driveConfig.closedLoop
@@ -347,7 +351,7 @@ public static class ClimberConstants { // CAN ID range 25-29
347351
static {
348352
climberConfig
349353
.idleMode(IdleMode.kBrake)
350-
.smartCurrentLimit(30)
354+
.smartCurrentLimit(40)
351355
.voltageCompensation(12)
352356
;
353357
}
@@ -382,6 +386,7 @@ public static class FieldConstants {
382386
public static final Pose2d redFarLeftReef = blueCloseLeftReef.plus(betweenReefsTransform);
383387
public static final Pose2d redFarRightReef = blueCloseRightReef.plus(betweenReefsTransform);
384388

389+
// station
385390
public static final Pose2d blueCoralY = new Pose2d(1.59336668626, 7.44378938214, Rotation2d.fromDegrees(306));
386391
public static final Pose2d blueCoralZ = new Pose2d(1.59336668626, fieldHeight - 7.44378938214, Rotation2d.fromDegrees(54));
387392
public static final Pose2d redCoralY = blueCoralY.rotateAround(fieldCenter, Rotation2d.k180deg);

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
package frc.robot.subsystems.elevator;
22

33
public enum ElevatorPosition {
4-
HOME("Home", 0.011),
4+
HOME("Home", 0.007),
55
L1("L1", 0.327),
66
L2("L2", 0.452),
77
L3("L3", 0.609),

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

Lines changed: 30 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -322,7 +322,7 @@ public void injectPresetPosition(ChassisSpeeds chassisSpeeds, boolean fieldRelat
322322
Logger.recordOutput("Swerve/desiredPose", desiredPose);
323323
Logger.recordOutput("Swerve/errorPose", errorPose);
324324
double vyReefRelative = presetPosController.calculate(errorPose.getY(), 0);
325-
if(vyReefRelative > 0.4) vyReefRelative *= elevatorSpeedFactor;
325+
if(vyReefRelative > 0.4) vyReefRelative *= elevatorSpeedFactor; // TODO double check when higher kP
326326
Logger.recordOutput("Swerve/vyReefRelative", vyReefRelative);
327327

328328
fieldRelativeSpeeds = fieldRelative ?
@@ -651,25 +651,44 @@ public void periodic() {
651651
if(fieldZone.equals(FieldZones.OPPOSITE)) {
652652
isAligned = false;
653653
} 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()));
654+
Pose2d leftErrorPose = new Pose2d(
655+
getPose().getX() - fieldZone.leftReefPose.getX(),
656+
getPose().getY() - fieldZone.leftReefPose.getY(),
657+
getPose().getRotation().minus(fieldZone.leftReefPose.getRotation())
658+
);
659+
Pose2d rightErrorPose = new Pose2d(
660+
getPose().getX() - fieldZone.rightReefPose.getX(),
661+
getPose().getY() - fieldZone.rightReefPose.getY(),
662+
getPose().getRotation().minus(fieldZone.rightReefPose.getRotation())
663+
);
656664
if(desiredPresetPosition.equals(PresetPositionType.LEFTREEF) || desiredPresetPosition.equals(PresetPositionType.RIGHTREEF)) {
657665
Pose2d desiredPose = desiredPresetPosition.equals(PresetPositionType.LEFTREEF) ? fieldZone.leftReefPose : fieldZone.rightReefPose;
658-
Pose2d errorPose = new Pose2d(getPose().getX() - desiredPose.getX(), getPose().getY() - desiredPose.getY(), getPose().getRotation().minus(desiredPose.getRotation()));
659-
if(Math.abs(errorPose.getX()) < 0.05 && Math.abs(errorPose.getY()) < 0.02) isAligned = true;
660-
else isAligned = false;
666+
Pose2d errorPose = new Pose2d(
667+
getPose().getX() - desiredPose.getX(),
668+
getPose().getY() - desiredPose.getY(),
669+
getPose().getRotation().minus(desiredPose.getRotation())
670+
);
671+
isAligned =
672+
Math.abs(errorPose.getX()) < SwerveConstants.kReefAlignXTolerance &&
673+
Math.abs(errorPose.getY()) < SwerveConstants.kReefAlignYTolerance &&
674+
Math.abs(errorPose.getRotation().getDegrees()) < SwerveConstants.kReefAlignHeadingTolerance;
661675
Logger.recordOutput("Swerve/isAlignedErrorPose", errorPose);
662676
errorX = errorPose.getX();
663677
errorY = errorPose.getY();
664678
errorHeading = errorPose.getRotation().getDegrees();
665679
reefAimed = true;
666680
} 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());
681+
errorX = Math.abs(leftErrorPose.getX()) < Math.abs(rightErrorPose.getX()) ? leftErrorPose.getX() : rightErrorPose.getX();
682+
errorY = Math.abs(leftErrorPose.getY()) < Math.abs(rightErrorPose.getY()) ? leftErrorPose.getY() : rightErrorPose.getY();
672683
errorHeading = Math.min(leftErrorPose.getRotation().getDegrees(), rightErrorPose.getRotation().getDegrees());
684+
isAligned =
685+
(Math.abs(leftErrorPose.getX()) < SwerveConstants.kReefAlignXTolerance &&
686+
Math.abs(leftErrorPose.getY()) < SwerveConstants.kReefAlignYTolerance &&
687+
Math.abs(leftErrorPose.getRotation().getDegrees()) < SwerveConstants.kReefAlignHeadingTolerance)
688+
||
689+
(Math.abs(rightErrorPose.getX()) < SwerveConstants.kReefAlignXTolerance &&
690+
Math.abs(rightErrorPose.getY()) < SwerveConstants.kReefAlignYTolerance &&
691+
Math.abs(leftErrorPose.getRotation().getDegrees()) < SwerveConstants.kReefAlignHeadingTolerance);
673692
reefAimed = false;
674693
}
675694
Logger.recordOutput("Swerve/isAlignedErrorPoseLeft", leftErrorPose);

0 commit comments

Comments
 (0)