Skip to content

Commit 5a716e0

Browse files
bobopop787bobopop787
committed
some optimizations
- temp pose to combo - reconfigure some main controls to allow for preset x button - preset x button with left bumper - tweak control constants for preset poses - make pid profiled
1 parent cb308c5 commit 5a716e0

4 files changed

Lines changed: 61 additions & 19 deletions

File tree

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

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

108108
public static final ControlConstants kPresetRotControlConstants = new ControlConstants(
109109
"SwervePresetRot",
110-
10,
110+
7,
111111
0,
112-
1,
112+
0.1,
113113
0,
114114
0,
115115
0
116116
);
117117

118118
public static final ControlConstants kPresetPosControlConstants = new ControlConstants(
119119
"SwervePresetPos",
120-
2.5, // TODO probably turn up
121-
0,
120+
10, // TODO probably turn up
122121
0,
122+
0.2,
123123
0,
124124
0,
125125
0

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

Lines changed: 15 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -105,9 +105,9 @@ private void configureBindings() {
105105
.withSize(1, 1);
106106

107107
swerve.setToAimSuppliers(
108-
driverController.leftTrigger()::getAsBoolean, // aim reef
109-
driverController.b()::getAsBoolean, // aim processor
110-
driverController.a()::getAsBoolean // aim station
108+
driverController.leftTrigger(), // aim reef
109+
driverController.b(), // aim processor
110+
driverController.a() // aim station
111111
);
112112
swerve.setReefChooserSuppliers(
113113
driverController::getRightX,
@@ -119,17 +119,26 @@ private void configureBindings() {
119119
driverController::getLeftY, // vy
120120
driverController::getRightX, // omega
121121
driverController::getRightTriggerAxis, // raw slow input
122-
driverController.leftBumper()::getAsBoolean, // robot centric
123-
driverController.rightBumper()::getAsBoolean // no optimize
122+
driverController.rightBumper(), // robot centric
123+
driverController.start() // no optimize
124124
));
125125

126126
driverController.y().onTrue(swerve.runZeroGyro());
127127
driverController.back().onTrue(swerve.runToggleToXPosition(true));
128128
driverController.b().onTrue(swerve.runUpdateControlConstants().andThen(elevator.runUpdateControlConstants()));
129-
driverController.start().onTrue(swerve.runSetTempPose());
130129
driverController.povLeft().onTrue(swerve.runTogglePresetPosition(PresetPositionType.LEFTREEF));
131130
driverController.povRight().onTrue(swerve.runTogglePresetPosition(PresetPositionType.RIGHTREEF));
132131
driverController.povUp().onTrue(swerve.runTogglePresetPosition(PresetPositionType.PROCESSOR));
132+
driverController.leftBumper().onTrue(swerve.runSetPresetXEnabled(true));
133+
driverController.leftBumper().onFalse(swerve.runSetPresetXEnabled(false));
134+
135+
Combo setTempPoseCombo = new Combo("setTempPose Combo", 0.5,
136+
driverController.rightBumper(),
137+
driverController.rightBumper().negate(),
138+
driverController.rightBumper(),
139+
driverController.start()
140+
);
141+
setTempPoseCombo.getTrigger().onTrue(swerve.runSetTempPose());
133142

134143
// if(Constants.simMode.equals(RobotMode.SIM)) {
135144
// swerve.setDefaultCommand(swerve.runSimOdometryMoveBy(

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -240,6 +240,8 @@ public Command runIntakeEffector(double effectorVolts, double funnelVolts, Boole
240240
desiredFunnelVolts = funnelVolts;
241241
elevatorIO.setEffectorVolts(-effectorVolts, effectorVolts);
242242
} else if(isAlignedSupplier.getAsBoolean()) {
243+
// TODO make it hold down still work
244+
// } else if(isAlignedSupplier.getAsBoolean() && Math.abs(elevatorIOInputs.elevatorHeight - elevatorIOInputs.desiredHeight) < 0.0015) {
243245
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.L1)) {
244246
elevatorIO.setEffectorVolts(-effectorVolts * 6 / 7, effectorVolts * 3 / 7); // over 5 to over 7
245247
} else {

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

Lines changed: 40 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -66,9 +66,9 @@ public class SwerveDrive extends SubsystemBase {
6666
private PIDController trajVYController;
6767
private PIDController trajHeadingController;
6868

69-
private PIDController presetRotController;
69+
private ProfiledPIDController presetRotController;
7070
private ProfiledPIDController presetXController;
71-
private PIDController presetYController;
71+
private ProfiledPIDController presetYController;
7272

7373
private FieldZones fieldZone;
7474

@@ -82,6 +82,7 @@ public class SwerveDrive extends SubsystemBase {
8282
private PresetPositionType desiredPresetPosition;
8383
private DoubleSupplier xReefChooser;
8484
private DoubleSupplier yReefChooser;
85+
private boolean presetXEnabled;
8586

8687
private DoubleSupplier elevatorHeightSupplier;
8788
private BooleanSupplier elevatorFactorOverrideSupplier;
@@ -136,14 +137,24 @@ private void initMathModels() {
136137
SwerveConstants.kPresetPosControlConstants.kP(),
137138
SwerveConstants.kPresetPosControlConstants.kI(),
138139
SwerveConstants.kPresetPosControlConstants.kD(),
139-
new Constraints(SwerveConstants.kMagVelLimit, SwerveConstants.kMagVelLimit * 2)
140+
new Constraints(SwerveConstants.kMagVelLimit, SwerveConstants.kMagVelLimit * 2.5)
140141
);
141-
presetYController = new PIDController(
142+
presetXController.setTolerance(0.01);
143+
presetYController = new ProfiledPIDController(
142144
SwerveConstants.kPresetPosControlConstants.kP(),
143145
SwerveConstants.kPresetPosControlConstants.kI(),
144-
SwerveConstants.kPresetPosControlConstants.kD()
146+
SwerveConstants.kPresetPosControlConstants.kD(),
147+
new Constraints(SwerveConstants.kMagVelLimit, SwerveConstants.kMagVelLimit * 2.5)
145148
);
146149
presetYController.setTolerance(0.01);
150+
presetRotController = new ProfiledPIDController(
151+
SwerveConstants.kPresetRotControlConstants.kP(),
152+
SwerveConstants.kPresetRotControlConstants.kI(),
153+
SwerveConstants.kPresetRotControlConstants.kD(),
154+
new Constraints(SwerveConstants.kRotVelLimit, SwerveConstants.kRotVelLimit * 2.5)
155+
);
156+
presetRotController.setTolerance(0.5);
157+
presetRotController.enableContinuousInput(0, 2 * Math.PI);
147158

148159
trajVXController = new PIDController(10, 0, 0);
149160
trajVYController = new PIDController(10, 0, 0);
@@ -156,6 +167,7 @@ private void initMathModels() {
156167
fieldZone = FieldZones.BLUE_CLOSE;
157168

158169
desiredPresetPosition = PresetPositionType.NONE;
170+
presetXEnabled = false;
159171

160172
errorX = 10;
161173
errorY = 10;
@@ -312,7 +324,7 @@ public void injectPresetRotation(ChassisSpeeds chassisSpeeds, boolean fieldRelat
312324

313325
public void toPresetRotation(ChassisSpeeds chassisSpeeds, Rotation2d heading, boolean fieldRelative, boolean optimize) {
314326
ChassisSpeeds desiredSpeeds = chassisSpeeds;
315-
desiredSpeeds.omegaRadiansPerSecond = trajHeadingController.calculate(getPose().getRotation().getRadians(), heading.getRadians());
327+
desiredSpeeds.omegaRadiansPerSecond = presetRotController.calculate(getPose().getRotation().getRadians(), heading.getRadians());
316328
injectPresetPosition(desiredSpeeds, fieldRelative, optimize);
317329
}
318330

@@ -330,6 +342,11 @@ public void injectPresetPosition(ChassisSpeeds chassisSpeeds, boolean fieldRelat
330342
errorPose = errorPose.rotateAround(Translation2d.kZero, desiredPose.getRotation().unaryMinus());
331343
Logger.recordOutput("Swerve/desiredPose", desiredPose);
332344
Logger.recordOutput("Swerve/errorPose", errorPose);
345+
346+
double vxReefRelative = presetXController.calculate(errorPose.getX(), 0);
347+
if(vxReefRelative > 0.4) vxReefRelative *= elevatorSpeedFactor; // TODO double check when higher kP
348+
Logger.recordOutput("Swerve/vxReefRelative", vxReefRelative);
349+
333350
double vyReefRelative = presetYController.calculate(errorPose.getY(), 0);
334351
if(vyReefRelative > 0.4) vyReefRelative *= elevatorSpeedFactor; // TODO double check when higher kP
335352
Logger.recordOutput("Swerve/vyReefRelative", vyReefRelative);
@@ -338,8 +355,12 @@ public void injectPresetPosition(ChassisSpeeds chassisSpeeds, boolean fieldRelat
338355
(Constants.isRed() ? flipChassisSpeeds(chassisSpeeds) : chassisSpeeds) :
339356
ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, getPose().getRotation());
340357
Logger.recordOutput("Swerve/fieldRelativeSpeedsNoFilter", fieldRelativeSpeeds);
358+
341359
ChassisSpeeds reefRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, fieldZone.rotation());
360+
Logger.recordOutput("Swerve/presetXEnabled", presetXEnabled);
361+
if(presetXEnabled) reefRelativeSpeeds.vxMetersPerSecond = vxReefRelative;
342362
reefRelativeSpeeds.vyMetersPerSecond = vyReefRelative;
363+
343364
fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(reefRelativeSpeeds, fieldZone.rotation());
344365
Logger.recordOutput("Swerve/fieldRelativeSpeedsReef", fieldRelativeSpeeds);
345366
runChassisSpeeds(fieldRelativeSpeeds, true, optimize, true);
@@ -511,9 +532,15 @@ public Command runUpdateControlConstants() {
511532
for(SDSSwerveModule module : modules) {
512533
module.updateControlConstants();
513534
}
514-
presetYController.setP(SwerveConstants.kPresetRotControlConstants.kP());
515-
presetYController.setI(SwerveConstants.kPresetRotControlConstants.kI());
516-
presetYController.setD(SwerveConstants.kPresetRotControlConstants.kD());
535+
presetXController.setP(SwerveConstants.kPresetPosControlConstants.kP());
536+
presetXController.setI(SwerveConstants.kPresetPosControlConstants.kI());
537+
presetXController.setD(SwerveConstants.kPresetPosControlConstants.kD());
538+
presetYController.setP(SwerveConstants.kPresetPosControlConstants.kP());
539+
presetYController.setI(SwerveConstants.kPresetPosControlConstants.kI());
540+
presetYController.setD(SwerveConstants.kPresetPosControlConstants.kD());
541+
presetRotController.setP(SwerveConstants.kPresetRotControlConstants.kP());
542+
presetRotController.setI(SwerveConstants.kPresetRotControlConstants.kI());
543+
presetRotController.setD(SwerveConstants.kPresetRotControlConstants.kD());
517544
System.out.println("Swerve control constants updated");
518545
});
519546
}
@@ -543,6 +570,10 @@ public Command runTogglePresetPosition(PresetPositionType type) {
543570
});
544571
}
545572

573+
public Command runSetPresetXEnabled(boolean presetXEnabled) {
574+
return runOnce(() -> this.presetXEnabled = presetXEnabled);
575+
}
576+
546577
@AutoLogOutput(key = "Odometry/Pose")
547578
public Pose2d getPose() {
548579
return poseEstimator.getEstimatedPosition();

0 commit comments

Comments
 (0)