Skip to content

Commit 3e19834

Browse files
dabeycorndabeycorn
committed
f
1 parent 76e6507 commit 3e19834

File tree

9 files changed

+191
-209
lines changed

9 files changed

+191
-209
lines changed

‎src/main/java/lib/devices/SimKraken.java‎

Lines changed: 38 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,9 @@
1+
// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
2+
// https://github.com/FIRSTTeam6423
3+
//
4+
// Open Source Software; you can modify and/or share it under the terms of
5+
// MIT license file in the root directory of this project
6+
17
package lib.devices;
28

39
import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -8,7 +14,6 @@
814
import com.ctre.phoenix6.hardware.TalonFX;
915
import com.ctre.phoenix6.signals.InvertedValue;
1016
import com.ctre.phoenix6.sim.ChassisReference;
11-
1217
import edu.wpi.first.math.system.plant.DCMotor;
1318
import edu.wpi.first.math.system.plant.LinearSystemId;
1419
import edu.wpi.first.wpilibj.Notifier;
@@ -33,32 +38,33 @@ public class SimKraken {
3338
private Notifier simNotifier = null;
3439
private double lastUpdateTimestamp = 0.0;
3540

36-
public SimKraken(CanDeviceId talonId, TalonFXConfiguration talonConfig, double JKgMetersSquared, double gearing) {
41+
public SimKraken(
42+
CanDeviceId talonId,
43+
TalonFXConfiguration talonConfig,
44+
double JKgMetersSquared,
45+
double gearing) {
3746
talon = new TalonFX(talonId.getId(), talonId.getBusName());
3847
talon.getConfigurator().apply(talonConfig);
3948

4049
// Configure sim state to follow the same orientation as talon config
41-
this.talon.getSimState().Orientation =
42-
talonConfig.MotorOutput.Inverted == InvertedValue.CounterClockwise_Positive
43-
? ChassisReference.CounterClockwise_Positive
44-
: ChassisReference.Clockwise_Positive;
45-
46-
motorSim = new DCMotorSim(
47-
LinearSystemId.createDCMotorSystem(
48-
motor,
49-
JKgMetersSquared,
50-
gearing),
51-
motor,
52-
0.0,
53-
0.0);
54-
55-
this.gearing = gearing;
56-
57-
// Run sim at a faster rate so PID gains behave better
58-
simNotifier = new Notifier(() -> {
59-
updateSimState();
60-
});
61-
simNotifier.startPeriodic(0.005);
50+
this.talon.getSimState().Orientation =
51+
talonConfig.MotorOutput.Inverted == InvertedValue.CounterClockwise_Positive
52+
? ChassisReference.CounterClockwise_Positive
53+
: ChassisReference.Clockwise_Positive;
54+
55+
motorSim =
56+
new DCMotorSim(
57+
LinearSystemId.createDCMotorSystem(motor, JKgMetersSquared, gearing), motor, 0.0, 0.0);
58+
59+
this.gearing = gearing;
60+
61+
// Run sim at a faster rate so PID gains behave better
62+
simNotifier =
63+
new Notifier(
64+
() -> {
65+
updateSimState();
66+
});
67+
simNotifier.startPeriodic(0.005);
6268
}
6369

6470
public void setAppliedVolts(double volts, boolean focEnabled) {
@@ -77,23 +83,21 @@ public void stop() {
7783
talon.stopMotor();
7884
}
7985

80-
81-
8286
private double addFriction(double motorVoltage, double frictionVoltage) {
83-
if (Math.abs(motorVoltage) < frictionVoltage) {
84-
motorVoltage = 0.0;
85-
} else if (motorVoltage > 0.0) {
86-
motorVoltage -= frictionVoltage;
87-
} else {
88-
motorVoltage += frictionVoltage;
89-
}
90-
return motorVoltage;
87+
if (Math.abs(motorVoltage) < frictionVoltage) {
88+
motorVoltage = 0.0;
89+
} else if (motorVoltage > 0.0) {
90+
motorVoltage -= frictionVoltage;
91+
} else {
92+
motorVoltage += frictionVoltage;
9193
}
94+
return motorVoltage;
95+
}
9296

9397
/** Updates simulated motor */
9498
private void updateSimState() {
9599
var simState = talon.getSimState();
96-
double simVolts = addFriction(simState.getMotorVoltage(), 0.25);
100+
double simVolts = addFriction(simState.getMotorVoltage(), 0.25);
97101

98102
motorSim.setInput(simVolts);
99103
double timestamp = RobotController.getFPGATime();

‎src/main/java/wmironpatriots/Robot.java‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -75,8 +75,12 @@ public Robot() {
7575
public void configureBindings() {}
7676

7777
public void configureGameBehavior() {
78+
swerve.setDefaultCommand(swerveCmdFactory.defaultCommand());
79+
7880
inTeleoperated.whileTrue(
79-
swerveCmdFactory.teleopDrive(driver::getLeftY, driver::getLeftY, driver::getRightX));
81+
swerveCmdFactory
82+
.teleopDrive(driver::getLeftX, driver::getLeftY, driver::getRightX)
83+
.repeatedly());
8084
}
8185

8286
/** Command for driver controller rumble */

‎src/main/java/wmironpatriots/commands/factories/SwerveCommandFactory.java‎

Lines changed: 24 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -6,10 +6,15 @@
66

77
package wmironpatriots.commands.factories;
88

9+
import static edu.wpi.first.units.Units.MetersPerSecond;
10+
import static edu.wpi.first.units.Units.RadiansPerSecond;
11+
12+
import edu.wpi.first.math.kinematics.ChassisSpeeds;
913
import edu.wpi.first.wpilibj2.command.Command;
14+
import edu.wpi.first.wpilibj2.command.Commands;
1015
import java.util.function.DoubleSupplier;
11-
import lib.swerve.ChassisVelocity;
1216
import wmironpatriots.subsystems.Swerve.Swerve;
17+
import wmironpatriots.subsystems.Swerve.SwerveConstants;
1318

1419
public class SwerveCommandFactory {
1520
private final Swerve swerve;
@@ -18,6 +23,10 @@ public SwerveCommandFactory(Swerve swerve) {
1823
this.swerve = swerve;
1924
}
2025

26+
public Command defaultCommand() {
27+
return Commands.runOnce(() -> swerve.stop(), swerve);
28+
}
29+
2130
/**
2231
* Drives robot based on input streams
2332
*
@@ -30,11 +39,19 @@ public Command teleopDrive(
3039
DoubleSupplier xSpeedMagnitude,
3140
DoubleSupplier ySpeedMagnitude,
3241
DoubleSupplier angularRateMagnitude) {
33-
return swerve.setChassisVelocity(
34-
ChassisVelocity.fromFieldRelativeSpeeds(
35-
xSpeedMagnitude.getAsDouble(),
36-
ySpeedMagnitude.getAsDouble(),
37-
angularRateMagnitude.getAsDouble(),
38-
swerve.getHeadingRotation2d().getRadians()));
42+
return Commands.run(
43+
() ->
44+
swerve.setChassisSpeeds(
45+
ChassisSpeeds.fromFieldRelativeSpeeds(
46+
xSpeedMagnitude.getAsDouble()
47+
* SwerveConstants.MAX_LINEAR_SPEED.in(MetersPerSecond)
48+
* -1,
49+
ySpeedMagnitude.getAsDouble()
50+
* SwerveConstants.MAX_LINEAR_SPEED.in(MetersPerSecond),
51+
angularRateMagnitude.getAsDouble()
52+
* SwerveConstants.MAX_ANGULAR_RATE.in(RadiansPerSecond)
53+
* -1,
54+
swerve.getHeadingRotation2d())),
55+
swerve);
3956
}
4057
}

‎src/main/java/wmironpatriots/subsystems/Swerve/Swerve.java‎

Lines changed: 45 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -15,10 +15,13 @@
1515
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
1616
import edu.wpi.first.math.kinematics.SwerveModulePosition;
1717
import edu.wpi.first.math.kinematics.SwerveModuleState;
18+
import edu.wpi.first.networktables.NetworkTableInstance;
19+
import edu.wpi.first.networktables.StructArrayPublisher;
1820
import edu.wpi.first.wpilibj.DriverStation;
21+
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
22+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1923
import edu.wpi.first.wpilibj2.command.Command;
2024
import edu.wpi.first.wpilibj2.command.Subsystem;
21-
import lib.swerve.ChassisVelocity;
2225
import wmironpatriots.Constants;
2326
import wmironpatriots.Robot;
2427
import wmironpatriots.subsystems.Swerve.gyro.GyroHardware;
@@ -56,20 +59,32 @@ public static Swerve create() {
5659
private final SwerveDriveKinematics kinematics = SwerveConstants.KINEMATICS;
5760
private final SwerveDrivePoseEstimator odometry;
5861

62+
private final Field2d f2d = new Field2d();
63+
private final StructArrayPublisher<SwerveModuleState> swervePublisher =
64+
NetworkTableInstance.getDefault()
65+
.getStructArrayTopic("SwerveStates", SwerveModuleState.struct)
66+
.publish();
67+
private final StructArrayPublisher<SwerveModuleState> setpoint =
68+
NetworkTableInstance.getDefault()
69+
.getStructArrayTopic("SwerveStateSetpoints", SwerveModuleState.struct)
70+
.publish();
71+
5972
public Swerve(GyroHardware gyro, Module... modules) {
6073
this.modules = modules;
6174
this.gyro = gyro;
6275

76+
SmartDashboard.putData("SwerveField", f2d);
77+
6378
odometry =
6479
new SwerveDrivePoseEstimator(
6580
kinematics, getGyroRotation2d(), getSwerveModulePositions(), new Pose2d());
66-
67-
setDefaultCommand(stop());
6881
}
6982

7083
@Override
7184
public void periodic() {
7285
odometry.update(getGyroRotation2d(), getSwerveModulePositions());
86+
f2d.setRobotPose(getPose2d());
87+
swervePublisher.set(getSwerveModuleStates());
7388

7489
for (Module module : modules) {
7590
module.periodic();
@@ -82,51 +97,47 @@ public void periodic() {
8297

8398
@Override
8499
public void simulationPeriodic() {
85-
var angularRate = getVelocity().getRobotRelative().omegaRadiansPerSecond;
100+
var angularRate = getChassisSpeeds().omegaRadiansPerSecond;
86101
simulatedHeading =
87102
simulatedHeading.rotateBy(
88103
Rotation2d.fromRadians(
89104
!Double.isNaN(angularRate) ? angularRate * Constants.LOOPTIME.in(Seconds) : 0));
90105
}
91106

92107
/**
93-
* Converts a robot centric velocity into {@link SwerveModuleState} setpoint for each module
108+
* Converts a {@link ChassisSpeeds} object representing robot relative speeds into {@link
109+
* SwerveModuleState} setpoint for each module
94110
*
95-
* @param velocity {@link ChassisVelocity} representing desired robot centric velocity setpoint
111+
* @param velocity {@link ChassisSpeeds} representing desired robot centric velocity setpoint
96112
* @return {@link Command}
97113
*/
98-
public Command setChassisVelocity(ChassisVelocity velocity) {
99-
return run(
100-
() -> {
101-
// https://github.com/wpilibsuite/allwpilib/issues/7332
102-
var states = kinematics.toSwerveModuleStates(velocity.getRobotRelative());
103-
SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_LINEAR_SPEED);
104-
105-
var speeds = kinematics.toChassisSpeeds(states);
106-
speeds = ChassisSpeeds.discretize(speeds, Constants.LOOPTIME.in(Seconds));
107-
108-
states = kinematics.toSwerveModuleStates(speeds);
109-
SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_LINEAR_SPEED);
110-
111-
for (int i = 0; i < modules.length; i++) {
112-
modules[i].setSetpoints(states[i]);
113-
}
114-
});
114+
public void setChassisSpeeds(ChassisSpeeds speeds) {
115+
// https://github.com/wpilibsuite/allwpilib/issues/7332
116+
// var states = kinematics.toSwerveModuleStates(velocity.getRobotRelative());
117+
// SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_LINEAR_SPEED);
118+
119+
// var speeds = kinematics.toChassisSpeeds(states);
120+
// speeds = ChassisSpeeds.discretize(speeds, Constants.LOOPTIME.in(Seconds));
121+
speeds = ChassisSpeeds.discretize(speeds, Constants.LOOPTIME.in(Seconds));
122+
123+
var states = kinematics.toSwerveModuleStates(speeds);
124+
SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_LINEAR_SPEED);
125+
setpoint.set(states);
126+
127+
for (int i = 0; i < modules.length; i++) {
128+
modules[i].setSetpoints(states[i]);
129+
}
115130
}
116131

117132
/**
118133
* Stops all swerve motors
119134
*
120135
* @return {@link Command}
121136
*/
122-
public Command stop() {
123-
return runOnce(
124-
() -> {
125-
for (Module module : modules) {
126-
module.stop();
127-
}
128-
})
129-
.withName("Stop");
137+
public void stop() {
138+
for (Module module : modules) {
139+
module.stop();
140+
}
130141
}
131142

132143
/**
@@ -151,16 +162,15 @@ public Rotation2d getHeadingRotation2d() {
151162
}
152163

153164
/**
154-
* @return {@link ChassisVelocity} representing the measured chassis velocity derived from module
155-
* states
165+
* @return {@link ChassisSpeeds} representing the measured chassis speeds from modules states
156166
*/
157-
public ChassisVelocity getVelocity() {
167+
public ChassisSpeeds getChassisSpeeds() {
158168
var moduleStates = new SwerveModuleState[modules.length];
159169
for (int i = 0; i < 4; i++) {
160170
moduleStates[i] = modules[i].getSwerveModuleState();
161171
}
162172

163-
return ChassisVelocity.fromRobotRelativeSpeeds(kinematics.toChassisSpeeds(moduleStates));
173+
return kinematics.toChassisSpeeds(moduleStates);
164174
}
165175

166176
/**

‎src/main/java/wmironpatriots/subsystems/Swerve/SwerveConstants.java‎

Lines changed: 16 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
package wmironpatriots.subsystems.Swerve;
88

99
import static edu.wpi.first.units.Units.FeetPerSecond;
10+
import static edu.wpi.first.units.Units.Inches;
1011
import static edu.wpi.first.units.Units.KilogramSquareMeters;
1112
import static edu.wpi.first.units.Units.Kilograms;
1213
import static edu.wpi.first.units.Units.Meters;
@@ -52,7 +53,8 @@ public static record ModuleConfig(
5253

5354
public static final Distance BUMPER_WIDTH = Meters.of(0.0889);
5455
public static final Distance TRACK_WIDTH = Meters.of(0.596201754);
55-
public static final Distance RADIUS = Meters.of(Math.hypot(TRACK_WIDTH.in(Meters) / 2.0, TRACK_WIDTH.in(Meters) / 2.0));
56+
public static final Distance RADIUS =
57+
Meters.of(Math.hypot(TRACK_WIDTH.in(Meters) / 2.0, TRACK_WIDTH.in(Meters) / 2.0));
5658

5759
public static final LinearVelocity MAX_LINEAR_SPEED = FeetPerSecond.of(16.5);
5860
public static final AngularVelocity MAX_ANGULAR_RATE =
@@ -64,44 +66,26 @@ public static record ModuleConfig(
6466
public static final ProfiledPIDController ANGULAR_RATE_FEEDBACK_CONTROLLER =
6567
new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0));
6668

67-
public static final Distance WHEEL_RADIUS = Meters.of(0.0);
69+
public static final Distance WHEEL_RADIUS = Inches.of(3);
6870

69-
public static final Translation2d[] MODULE_OFFSETS = new Translation2d[4];
71+
public static final Translation2d[] MODULE_OFFSETS =
72+
new Translation2d[] {
73+
new Translation2d(TRACK_WIDTH.in(Meters) / 2.0, TRACK_WIDTH.in(Meters) / 2.0),
74+
new Translation2d(TRACK_WIDTH.in(Meters) / 2.0, -TRACK_WIDTH.in(Meters) / 2.0),
75+
new Translation2d(-TRACK_WIDTH.in(Meters) / 2.0, TRACK_WIDTH.in(Meters) / 2.0),
76+
new Translation2d(-TRACK_WIDTH.in(Meters) / 2.0, -TRACK_WIDTH.in(Meters) / 2.0)
77+
};
7078

71-
public static final ModuleConfig[] MODULE_CONFIGS =
79+
public static final ModuleConfig[] MODULE_CONFIGS =
7280
new ModuleConfig[] {
7381
new ModuleConfig(
74-
0,
75-
MATRIXID.FL_PIVOT,
76-
MATRIXID.FL_DRIVE,
77-
MATRIXID.FL_CANCODER,
78-
-0.16,
79-
false,
80-
false),
82+
0, MATRIXID.FL_PIVOT, MATRIXID.FL_DRIVE, MATRIXID.FL_CANCODER, -0.16, false, false),
8183
new ModuleConfig(
82-
1,
83-
MATRIXID.FR_PIVOT,
84-
MATRIXID.FR_DRIVE,
85-
MATRIXID.FR_CANCODER,
86-
0.01,
87-
true,
88-
false),
84+
1, MATRIXID.FR_PIVOT, MATRIXID.FR_DRIVE, MATRIXID.FR_CANCODER, 0.01, true, false),
8985
new ModuleConfig(
90-
2,
91-
MATRIXID.BL_PIVOT,
92-
MATRIXID.BL_DRIVE,
93-
MATRIXID.BL_CANCODER,
94-
0.36,
95-
true,
96-
false),
86+
2, MATRIXID.BL_PIVOT, MATRIXID.BL_DRIVE, MATRIXID.BL_CANCODER, 0.36, true, false),
9787
new ModuleConfig(
98-
3,
99-
MATRIXID.BR_PIVOT,
100-
MATRIXID.BR_DRIVE,
101-
MATRIXID.BR_CANCODER,
102-
-0.26,
103-
true,
104-
false),
88+
3, MATRIXID.BR_PIVOT, MATRIXID.BR_DRIVE, MATRIXID.BR_CANCODER, -0.26, true, false),
10589
};
10690

10791
public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(MODULE_OFFSETS);

0 commit comments

Comments
 (0)