Skip to content

Commit 789b9d9

Browse files
ohowe1ohowe1
committed
Let's get Kraken
1 parent 3b0dc8a commit 789b9d9

File tree

9 files changed

+172
-257
lines changed

9 files changed

+172
-257
lines changed

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

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,6 @@
2020
import frc.robot.subsystems.swerve.SwerveDriveSubsystem;
2121
import frc.robot.subsystems.transport.TransportSubsystem;
2222
import frc.robot.subsystems.wrist.WristSubsystem;
23-
import frc.robot.utils.RaiderUtils;
2423

2524
public class Autos {
2625
private final SwerveDriveSubsystem driveSubsystem;
@@ -133,11 +132,13 @@ public Command ampSpeakerCloseAmpTwoPieceAuto() {
133132
}
134133

135134
public Command mobilitySourceSideAuto() {
136-
return Commands.sequence(autoStart(), followPathCommand("MobilitySourceSide", true, driveSubsystem));
135+
return Commands.sequence(
136+
autoStart(), followPathCommand("MobilitySourceSide", true, driveSubsystem));
137137
}
138138

139139
public Command mobilityAmpSideAuto() {
140-
return Commands.sequence(autoStart(), followPathCommand("MobilityAmpSide", true, driveSubsystem));
140+
return Commands.sequence(
141+
autoStart(), followPathCommand("MobilityAmpSide", true, driveSubsystem));
141142
}
142143

143144
private Command centerSpeakerCloseMidNote(boolean firstPath) {

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

Lines changed: 16 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -114,18 +114,27 @@ private SwerveConstants() {}
114114
public static final double STEER_GEAR_REDUCTION = 150.0 / 7.0;
115115

116116
public static final double DRIVE_CURRENT_LIMIT = 60.0;
117-
public static final int STEER_STALL_CURRENT_LIMIT = 45;
118-
public static final int STEER_FREE_CURRENT_LIMIT = 25;
117+
public static final double STEER_CURRENT_LIMIT = 35.0;
119118

120119
// 0.47
121120
public static final TunablePIDGains DRIVE_VELOCITY_PID_GAINS =
122121
new TunablePIDGains("/gains/drive", 0.2, 0.0, 0.0, MiscConstants.TUNING_MODE);
123-
122+
// Meters per rotation
123+
private static final double DRIVE_CONVERSION =
124+
(Math.PI * WHEEL_DIAMETER_METERS) / DRIVE_GEAR_REDUCTION;
125+
// Volts/(Meter/Second) to Volts/(Rotation/Second)
124126
public static final TunableFFGains DRIVE_VELOCITY_FF_GAINS =
125-
new TunableFFGains("/gains/drive", 0.064022, 1.9843, 0.9255, MiscConstants.TUNING_MODE);
127+
new TunableFFGains(
128+
"/gains/drive",
129+
0.064022 * DRIVE_CONVERSION,
130+
1.9843 * DRIVE_CONVERSION,
131+
0.9255 * DRIVE_CONVERSION,
132+
MiscConstants.TUNING_MODE);
126133

127134
public static final TunablePIDGains STEER_POSITION_PID_GAINS =
128-
new TunablePIDGains("/gains/steer", 1.0, 0.0, 0.1, MiscConstants.TUNING_MODE);
135+
new TunablePIDGains("/gains/steer", 7.207, 0.0, 0.002, MiscConstants.TUNING_MODE);
136+
public static final TunableFFGains STEER_VELOCITY_FF_GAINS =
137+
new TunableFFGains("/gains/steer", 0.0, 0.0, 0.0, MiscConstants.TUNING_MODE);
129138

130139
// Left right distance between center of wheels
131140
public static final double TRACKWIDTH_METERS = Units.inchesToMeters(22.75);
@@ -154,9 +163,9 @@ private SwerveConstants() {}
154163
DRIVE_GEAR_REDUCTION,
155164
STEER_GEAR_REDUCTION,
156165
DRIVE_CURRENT_LIMIT,
157-
STEER_FREE_CURRENT_LIMIT,
158-
STEER_STALL_CURRENT_LIMIT,
166+
STEER_CURRENT_LIMIT,
159167
WHEEL_DIAMETER_METERS,
168+
MAX_VELOCITY_METERS_SECOND,
160169
ODOMETRY_FREQUENCY,
161170
DRIVE_VELOCITY_PID_GAINS,
162171
DRIVE_VELOCITY_FF_GAINS,

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

Lines changed: 2 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -93,8 +93,7 @@ private void configureAutos() {
9393
autoCommand.addOption(
9494
"Center Speaker Two Piece Close Mid", autos.centerSpeakerCloseMidTwoPieceAuto());
9595
autoCommand.addOption(
96-
"Center Speaker Three Piece Close Amp Mid",
97-
autos.centerSpeakerCloseAmpMidThreePieceAuto());
96+
"Center Speaker Three Piece Close Amp Mid", autos.centerSpeakerCloseAmpMidThreePieceAuto());
9897
autoCommand.addOption(
9998
"Center Speaker Three Piece Close Source Mid",
10099
autos.centerSpeakerCloseSourceMidThreePieceAuto());
@@ -230,9 +229,7 @@ private void configureDriving() {
230229
TunableDouble maxMaxAngularSpeedPercent = new TunableDouble("/speed/maxAngular", 0.6, true);
231230

232231
DoubleSupplier maxTranslationalSpeedSuppler =
233-
() ->
234-
maxTranslationSpeedPercent.get()
235-
* SwerveConstants.MAX_VELOCITY_METERS_SECOND;
232+
() -> maxTranslationSpeedPercent.get() * SwerveConstants.MAX_VELOCITY_METERS_SECOND;
236233
DoubleSupplier maxAngularSpeedSupplier =
237234
() -> maxMaxAngularSpeedPercent.get() * SwerveConstants.MAX_ANGULAR_VELOCITY_RADIANS_SECOND;
238235

‎src/main/java/frc/robot/commands/drive/auto/FollowPathCommand.java‎

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -41,12 +41,16 @@ public class FollowPathCommand extends Command {
4141
private final Timer timer = new Timer();
4242
private ChoreoTrajectory currentPath;
4343

44-
public FollowPathCommand(ChoreoTrajectory path, boolean resetOdometry, SwerveDriveSubsystem driveSubsystem) {
44+
public FollowPathCommand(
45+
ChoreoTrajectory path, boolean resetOdometry, SwerveDriveSubsystem driveSubsystem) {
4546
this(path, resetOdometry, true, driveSubsystem);
4647
}
4748

4849
public FollowPathCommand(
49-
ChoreoTrajectory path, boolean resetOdometry, boolean shouldFlipIfRed, SwerveDriveSubsystem driveSubsystem) {
50+
ChoreoTrajectory path,
51+
boolean resetOdometry,
52+
boolean shouldFlipIfRed,
53+
SwerveDriveSubsystem driveSubsystem) {
5054
this(
5155
() -> {
5256
if (shouldFlipIfRed && (RaiderUtils.shouldFlip())) {
@@ -59,7 +63,9 @@ public FollowPathCommand(
5963
}
6064

6165
public FollowPathCommand(
62-
Supplier<ChoreoTrajectory> pathSupplier, boolean resetOdometry, SwerveDriveSubsystem driveSubsystem) {
66+
Supplier<ChoreoTrajectory> pathSupplier,
67+
boolean resetOdometry,
68+
SwerveDriveSubsystem driveSubsystem) {
6369
this.pathSupplier = pathSupplier;
6470
this.resetOdometry = resetOdometry;
6571
this.driveSubsystem = driveSubsystem;

‎src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,10 @@
1919
public class ClimberSubsystem extends SubsystemBase {
2020
private final TelemetryTalonFX climberMotor =
2121
new TelemetryTalonFX(
22-
CLIMBER_MOTOR_ID, "/climber/motor", MiscConstants.CANIVORE_NAME, MiscConstants.TUNING_MODE);
22+
CLIMBER_MOTOR_ID,
23+
"/climber/motor",
24+
MiscConstants.CANIVORE_NAME,
25+
MiscConstants.TUNING_MODE);
2326
private final EventTelemetryEntry eventEntry = new EventTelemetryEntry("/climber/events");
2427
private final DoubleTelemetryEntry voltageReqEntry =
2528
new DoubleTelemetryEntry("/climber/voltageReq", MiscConstants.TUNING_MODE);

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

Lines changed: 20 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,8 @@ enum DriveMode {
9898
private final Function<Pose2d, List<EstimatedRobotPose>> cameraPoseDataSupplier;
9999

100100
private final TelemetryPigeon2 pigeon =
101-
new TelemetryPigeon2(PIGEON_ID, "/drive/gyro", MiscConstants.CANIVORE_NAME, MiscConstants.TUNING_MODE);
101+
new TelemetryPigeon2(
102+
PIGEON_ID, "/drive/gyro", MiscConstants.CANIVORE_NAME, MiscConstants.TUNING_MODE);
102103

103104
private StatusSignal<Double> yawSignal;
104105

@@ -113,11 +114,15 @@ enum DriveMode {
113114
null,
114115
this));
115116

117+
private final SysIdRoutine steerPositionSysId =
118+
new SysIdRoutine(
119+
new SysIdRoutine.Config(
120+
null, null, null, (state) -> SignalLogger.writeString("State", state.toString())),
121+
new SysIdRoutine.Mechanism(
122+
(Measure<Voltage> voltage) -> setRawVolts(0.0, voltage.in(Volts)), null, this));
123+
116124
private final Alert pigeonConfigurationAlert =
117125
new Alert("Pigeon failed to initialize", Alert.AlertType.ERROR);
118-
private final BooleanTelemetryEntry allModulesAtAbsoluteZeroEntry =
119-
new BooleanTelemetryEntry("/drive/allModulesAtAbsoluteZero", true);
120-
121126
private final DoubleTelemetryEntry gyroEntry =
122127
new DoubleTelemetryEntry("/drive/gyroRadians", true);
123128

@@ -327,20 +332,6 @@ public double[] getActualDriveVoltages() {
327332
return voltages;
328333
}
329334

330-
public void setAllModulesToAbsolute() {
331-
for (SwerveModule module : modules) {
332-
module.resetSteerToAbsolute();
333-
}
334-
}
335-
336-
private boolean allModulesAtAbsolute() {
337-
boolean allSet = true;
338-
for (SwerveModule module : modules) {
339-
allSet &= module.isSetToAbsolute();
340-
}
341-
return allSet;
342-
}
343-
344335
/**
345336
* Should only be used for characterization
346337
*
@@ -376,14 +367,22 @@ public SwerveModulePosition[] getModulePositions() {
376367
return actualPositions;
377368
}
378369

379-
public Command quasistaticSysIDCommand(SysIdRoutine.Direction direction) {
370+
public Command driveQuasistaticSysIDCommand(SysIdRoutine.Direction direction) {
380371
return driveVelocitySysId.quasistatic(direction).beforeStarting(SignalLogger::start);
381372
}
382373

383-
public Command dynamicSysIDCommand(SysIdRoutine.Direction direction) {
374+
public Command driveDynamicSysIDCommand(SysIdRoutine.Direction direction) {
384375
return driveVelocitySysId.dynamic(direction).beforeStarting(SignalLogger::start);
385376
}
386377

378+
public Command steerQuasistaticSysIDCommand(SysIdRoutine.Direction direction) {
379+
return steerPositionSysId.quasistatic(direction).beforeStarting(SignalLogger::start);
380+
}
381+
382+
public Command steerDynamicSysIDCommand(SysIdRoutine.Direction direction) {
383+
return steerPositionSysId.dynamic(direction).beforeStarting(SignalLogger::start);
384+
}
385+
387386
@Override
388387
public void periodic() {
389388
switch (driveMode) {
@@ -401,7 +400,7 @@ public void periodic() {
401400
}
402401
case CHARACTERIZATION -> {
403402
for (SwerveModule module : modules) {
404-
module.setCharacterizationVoltage(rawDriveVolts);
403+
module.setDriveCharacterizationVoltage(rawDriveVolts);
405404
}
406405
}
407406
}
@@ -418,7 +417,6 @@ public void periodic() {
418417
}
419418

420419
private void logValues() {
421-
allModulesAtAbsoluteZeroEntry.append(allModulesAtAbsolute());
422420
gyroEntry.append(getGyroRotation().getRadians());
423421
chassisSpeedsEntry.append(getCurrentChassisSpeeds());
424422
actualSwerveStatesEntry.append(getActualStates());

0 commit comments

Comments
 (0)