Skip to content

Commit 04351c7

Browse files
Ishan1522Ishan1522
committed
noice
1 parent e736457 commit 04351c7

File tree

17 files changed

+181
-152
lines changed

17 files changed

+181
-152
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
plugins {
22
id "java"
3-
id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
3+
id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3"
44
id "com.peterabeles.gversion" version "1.10"
55
id "com.diffplug.spotless" version "6.25.0"
66
id "pmd"

‎src/main/java/frc/robot/extras/setpointGen/SwerveSetpointGenerator.java‎

Lines changed: 15 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,6 @@
99
import edu.wpi.first.math.kinematics.SwerveModuleState;
1010
import edu.wpi.first.math.system.plant.DCMotor;
1111
import edu.wpi.first.wpilibj.RobotController;
12-
import frc.robot.extras.setpointGen.SPGCalcs.LocalVars;
13-
import org.littletonrobotics.junction.Logger;
1412

1513
/**
1614
* Swerve setpoint generatoR that has been passed around so many times its hard to keep track, just
@@ -24,7 +22,6 @@ public class SwerveSetpointGenerator {
2422
private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds();
2523

2624
private static final int NUM_MODULES = SPGCalcs.NUM_MODULES;
27-
// private final double brownoutVoltage;
2825

2926
private final SwerveDriveKinematics kinematics;
3027
private final Translation2d[] moduleLocations;
@@ -63,7 +60,6 @@ public SwerveSetpointGenerator(
6360
this.moiKgMetersSquared = moiKgMetersSquared;
6461
this.wheelRadiusMeters = wheelDiameterMeters / 2;
6562
this.maxDriveVelocityMPS = driveMotor.freeSpeedRadPerSec * wheelRadiusMeters;
66-
// this.brownoutVoltage = RobotController.getBrownoutVoltage();
6763

6864
wheelFrictionForce = wheelCoF * ((massKg / 4) * 9.81);
6965
// maxTorqueFriction = this.wheelFrictionForce * wheelRadiusMeters;
@@ -89,13 +85,6 @@ public SwerveSetpointGenerator(
8985
*/
9086
public SwerveSetpoint generateSetpoint(
9187
final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) {
92-
// Add logging for inputs
93-
// if (Double.isNaN(inputVoltage)) {
94-
// inputVoltage = 12.0;
95-
// } else {
96-
// inputVoltage = Math.max(inputVoltage, brownoutVoltage);
97-
// }
98-
9988
// https://github.com/wpilibsuite/allwpilib/issues/7332
10089
SwerveModuleState[] desiredModuleStates =
10190
kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds);
@@ -120,37 +109,30 @@ public SwerveSetpoint generateSetpoint(
120109
- prevSetpoint.chassisSpeeds().omegaRadiansPerSecond;
121110
vars.minS = 1.0;
122111

123-
Logger.recordOutput("beginningVars", vars);
124-
125112
checkNeedToSteer(vars);
126-
127-
Logger.recordOutput("postCheckNeedToSteer", vars);
128-
129113
makeVectors(vars);
130114

131-
Logger.recordOutput("pastMakeVectors", vars);
132-
133-
if (vars.allModulesShouldFlip
134-
&& !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS)
135-
&& !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) {
136-
// It will (likely) be faster to stop the robot, rotate the modules in place to the
137-
// complement
138-
// of the desired angle, and accelerate again.
139-
return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt);
140-
}
115+
// if (vars.allModulesShouldFlip
116+
// && !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS)
117+
// && !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) {
118+
// // It will (likely) be faster to stop the robot, rotate the modules in place to the
119+
// complement
120+
// // of the desired angle, and accelerate again.
121+
// return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt);
122+
// }
141123

142124
solveSteering(vars);
143-
Logger.recordOutput("postSolveSteering", vars);
144125

145126
solveDriving(vars);
146-
Logger.recordOutput("postSolveDriving", vars);
147127

148128
ChassisSpeeds retSpeeds =
149129
new ChassisSpeeds(
150130
vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx,
151131
vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy,
152132
vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta);
153-
retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt);
133+
retSpeeds.discretize(
134+
retSpeeds,
135+
dt);
154136

155137
double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt;
156138
double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt;
@@ -175,15 +157,12 @@ public SwerveSetpoint generateSetpoint(
175157
accelStates[m].speedMetersPerSecond);
176158
}
177159

178-
Logger.recordOutput("finalVars", vars);
179-
180-
return new SwerveSetpoint( // Logger.recordOutput("output",
181-
retSpeeds, outputStates);
160+
// log("output",
161+
return new SwerveSetpoint(retSpeeds, outputStates);
182162
}
183163

184164
public SwerveSetpoint generateSimpleSetpoint(
185165
final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) {
186-
187166
AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES];
188167
SwerveModuleState[] desiredModuleStates =
189168
kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds);
@@ -192,6 +171,7 @@ public SwerveSetpoint generateSimpleSetpoint(
192171
desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle);
193172
outputStates[m] = AdvancedSwerveModuleState.fromBase(desiredModuleStates[m]);
194173
}
174+
195175
return new SwerveSetpoint(kinematics.toChassisSpeeds(desiredModuleStates), outputStates);
196176
}
197177

@@ -309,38 +289,25 @@ private void solveDriving(LocalVars vars) {
309289
// battery is sagging down to 11v, which will affect the max torque output
310290
double currentDraw =
311291
driveMotor.getCurrent(Math.abs(lastVelRadPerSec), RobotController.getInputVoltage());
312-
double reverseCurrentDraw =
313-
Math.abs(
314-
driveMotor.getCurrent(
315-
Math.abs(lastVelRadPerSec), -RobotController.getInputVoltage()));
316292
currentDraw = Math.min(currentDraw, driveCurrentLimitAmps);
317-
318-
reverseCurrentDraw = Math.min(reverseCurrentDraw, driveCurrentLimitAmps);
319-
double forwardModuleTorque = driveMotor.getTorque(currentDraw);
320-
double reverseModuleTorque = driveMotor.getTorque(reverseCurrentDraw);
321-
// double moduleTorque = driveMotor.getTorque(currentDraw);
293+
double moduleTorque = driveMotor.getTorque(currentDraw);
322294

323295
double prevSpeed = vars.prevModuleStates[m].speedMetersPerSecond;
324296
vars.desiredModuleStates[m].optimize(vars.prevModuleStates[m].angle);
325297
double desiredSpeed = vars.desiredModuleStates[m].speedMetersPerSecond;
326298

327299
int forceSign;
328300
Rotation2d forceAngle = vars.prevModuleStates[m].angle;
329-
double moduleTorque;
330-
331301
if (epsilonEquals(prevSpeed, 0.0)
332302
|| (prevSpeed > 0 && desiredSpeed >= prevSpeed)
333303
|| (prevSpeed < 0 && desiredSpeed <= prevSpeed)) {
334-
moduleTorque = forwardModuleTorque;
335-
336304
// Torque loss will be fighting motor
337305
moduleTorque -= torqueLoss;
338306
forceSign = 1; // Force will be applied in direction of module
339307
if (prevSpeed < 0) {
340308
forceAngle = forceAngle.plus(Rotation2d.k180deg);
341309
}
342310
} else {
343-
moduleTorque = reverseModuleTorque;
344311
// Torque loss will be helping the motor
345312
moduleTorque += torqueLoss;
346313
forceSign = -1; // Force will be applied in opposite direction of module

‎src/main/java/frc/robot/extras/simulation/field/SimulatedField.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
import frc.robot.extras.simulation.mechanismSim.swerve.AbstractDriveTrainSimulation;
1414
import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim;
1515
import frc.robot.extras.util.GeomUtil;
16-
import java.lang.ref.WeakReference;
16+
// import java.lang.ref.WeakReference;
1717
import java.util.*;
1818
import org.dyn4j.dynamics.Body;
1919
import org.dyn4j.dynamics.BodyFixture;
@@ -130,7 +130,7 @@ public static void overrideSimulationTimings(
130130
protected final Set<AbstractDriveTrainSimulation> driveTrainSimulations;
131131
protected final Set<GamePieceSimulation> gamePieces;
132132
protected final List<Runnable> simulationSubTickActions;
133-
protected final List<WeakReference<BrushlessMotorSim>> motors;
133+
protected final List<BrushlessMotorSim> motors;
134134
private final List<IntakeSimulation> intakeSimulations;
135135

136136
/**
@@ -284,7 +284,7 @@ public void simulationPeriodic() {
284284
}
285285

286286
public void addMotor(BrushlessMotorSim motor) {
287-
motors.add(new WeakReference<>(motor));
287+
motors.add(motor);
288288
}
289289

290290
/**
@@ -306,7 +306,7 @@ public void addMotor(BrushlessMotorSim motor) {
306306
private void simulationSubTick() {
307307
ArrayList<Double> motorCurrents = new ArrayList<>();
308308
for (var motor : motors) {
309-
BrushlessMotorSim motorRef = motor.get();
309+
BrushlessMotorSim motorRef = motor;
310310
if (motorRef != null) {
311311
motorRef.update();
312312
}

‎src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -170,7 +170,7 @@ public static GyroSimulation createPigeon2() {
170170
* IMU</a>
171171
*/
172172
public static GyroSimulation createNavX2() {
173-
return new GyroSimulation(2, 0.04);
173+
return new GyroSimulation(0.00208333333, 0.04);
174174
}
175175

176176
/**

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
99
import edu.wpi.first.math.util.Units;
1010

11-
/** Add your docs here. */
11+
/** Swerve Constants */
1212
public class SwerveConstants {
1313

1414
public static final class DriveConstants {

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,7 @@
1414
import edu.wpi.first.wpilibj.Alert;
1515
import edu.wpi.first.wpilibj.DriverStation;
1616
import edu.wpi.first.wpilibj2.command.SubsystemBase;
17-
import frc.robot.extras.setpointGen.SwerveSetpoint;
18-
import frc.robot.extras.setpointGen.SwerveSetpointGenerator;
17+
import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
1918
import frc.robot.extras.util.DeviceCANBus;
2019
import frc.robot.extras.util.TimeUtil;
2120
import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
@@ -25,6 +24,8 @@
2524
import frc.robot.subsystems.swerve.module.ModuleInterface;
2625
import frc.robot.subsystems.swerve.odometryThread.OdometryThread;
2726
import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged;
27+
import frc.robot.extras.setpointGen.SwerveSetpoint;
28+
import frc.robot.extras.setpointGen.SwerveSetpointGenerator;
2829
import frc.robot.subsystems.vision.VisionConstants;
2930
import java.util.Optional;
3031
import org.littletonrobotics.junction.AutoLogOutput;
@@ -49,7 +50,7 @@ public class SwerveDrive extends SubsystemBase {
4950
58,
5051
7,
5152
ModuleConstants.WHEEL_DIAMETER_METERS,
52-
1.5,
53+
WHEEL_GRIP.TIRE_WHEEL.cof,
5354
0.0);
5455
private SwerveSetpoint setpoint = SwerveSetpoint.zeroed();
5556

@@ -139,7 +140,6 @@ public void setPoseEstimatorVisionConfidence(
139140
VecBuilder.fill(xStandardDeviation, yStandardDeviation, thetaStandardDeviation));
140141
}
141142

142-
@Override
143143
public void periodic() {
144144
final double t0 = TimeUtil.getRealTimeSeconds();
145145
fetchOdometryInputs();
@@ -202,7 +202,7 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi
202202
xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d())
203203
: new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
204204

205-
setpoint = setpointGenerator.generateSimpleSetpoint(setpoint, desiredSpeeds, 0.02);
205+
setpoint = setpointGenerator.generateSetpoint(setpoint, desiredSpeeds, 0.02);
206206

207207
setModuleStates(setpoint.moduleStates());
208208
Logger.recordOutput("SwerveStates/DesiredStates", setpoint.moduleStates());

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

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,10 @@ public void updateOdometryInputs() {
3939
}
4040

4141
@Override
42-
public void periodic() {}
42+
public void periodic() {
43+
// updateOdometryPositions();
44+
Logger.recordOutput("Drive/current turn angle", getTurnRotation().getRotations());
45+
}
4346

4447
public void setVoltage(Voltage volts) {
4548
io.setDriveVoltage(volts);
@@ -61,6 +64,7 @@ public void runSetPoint(SwerveModuleState state) {
6164
io.stopModule();
6265
}
6366
io.setDesiredState(state);
67+
Logger.recordOutput("Drive/desired turn angle", state.angle.getRotations());
6468
}
6569

6670
/** Returns the current turn angle of the module. */

‎src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,6 @@
44
import org.littletonrobotics.junction.AutoLog;
55

66
public interface GyroInterface {
7-
87
@AutoLog
98
public static class GyroInputs {
109
public boolean isConnected = false;

‎src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,6 @@
66
import org.littletonrobotics.junction.AutoLog;
77

88
public interface ModuleInterface {
9-
109
@AutoLog
1110
class ModuleInputs {
1211

‎src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java‎

Lines changed: 5 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -48,9 +48,9 @@ public class PhysicalModule implements ModuleInterface {
4848
private final BaseStatusSignal[] periodicallyRefreshedSignals;
4949

5050
public PhysicalModule(ModuleConfig moduleConfig) {
51-
driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.RIO.name);
52-
turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.RIO.name);
53-
turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.RIO.name);
51+
driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.CANIVORE.name);
52+
turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.CANIVORE.name);
53+
turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.CANIVORE.name);
5454

5555
CANcoderConfiguration turnEncoderConfig = new CANcoderConfiguration();
5656
turnEncoderConfig.MagnetSensor.MagnetOffset = -moduleConfig.angleZero();
@@ -170,29 +170,15 @@ public void setTurnVoltage(Voltage volts) {
170170

171171
@Override
172172
public void setDesiredState(SwerveModuleState desiredState) {
173-
double turnRotations = getTurnRotations();
174-
// Optimize the reference state to avoid spinning further than 90 degrees
175-
SwerveModuleState setpoint =
176-
new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
177-
178-
setpoint.optimize(Rotation2d.fromRotations(turnRotations));
179-
setpoint.cosineScale(Rotation2d.fromRotations(turnRotations));
180-
181-
if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) {
182-
driveMotor.set(0);
183-
turnMotor.set(0);
184-
return;
185-
}
186-
187173
// Converts meters per second to rotations per second
188174
double desiredDriveRPS =
189-
setpoint.speedMetersPerSecond
175+
desiredState.speedMetersPerSecond
190176
* ModuleConstants.DRIVE_GEAR_RATIO
191177
/ ModuleConstants.WHEEL_CIRCUMFERENCE_METERS;
192178

193179
driveMotor.setControl(velocityRequest.withVelocity(RotationsPerSecond.of(desiredDriveRPS)));
194180
turnMotor.setControl(
195-
mmPositionRequest.withPosition(Rotations.of(setpoint.angle.getRotations())));
181+
mmPositionRequest.withPosition(Rotations.of(desiredState.angle.getRotations())));
196182
}
197183

198184
public double getTurnRotations() {

0 commit comments

Comments
 (0)