Skip to content

Commit 0abe225

Browse files
Ishan1522Ishan1522
committed
Merge branch 'ExamplesYay' of https://github.com/TitaniumTigers4829/4829-BaseRobotCode into ExamplesYay
2 parents d6f17c8 + acb1721 commit 0abe225

File tree

17 files changed

+245
-188
lines changed

17 files changed

+245
-188
lines changed

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

Lines changed: 13 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,28 @@ 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(retSpeeds, dt);
154134

155135
double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt;
156136
double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt;
@@ -175,15 +155,12 @@ public SwerveSetpoint generateSetpoint(
175155
accelStates[m].speedMetersPerSecond);
176156
}
177157

178-
Logger.recordOutput("finalVars", vars);
179-
180-
return new SwerveSetpoint( // Logger.recordOutput("output",
181-
retSpeeds, outputStates);
158+
// log("output",
159+
return new SwerveSetpoint(retSpeeds, outputStates);
182160
}
183161

184162
public SwerveSetpoint generateSimpleSetpoint(
185163
final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) {
186-
187164
AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES];
188165
SwerveModuleState[] desiredModuleStates =
189166
kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds);
@@ -192,6 +169,7 @@ public SwerveSetpoint generateSimpleSetpoint(
192169
desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle);
193170
outputStates[m] = AdvancedSwerveModuleState.fromBase(desiredModuleStates[m]);
194171
}
172+
195173
return new SwerveSetpoint(kinematics.toChassisSpeeds(desiredModuleStates), outputStates);
196174
}
197175

@@ -309,38 +287,25 @@ private void solveDriving(LocalVars vars) {
309287
// battery is sagging down to 11v, which will affect the max torque output
310288
double currentDraw =
311289
driveMotor.getCurrent(Math.abs(lastVelRadPerSec), RobotController.getInputVoltage());
312-
double reverseCurrentDraw =
313-
Math.abs(
314-
driveMotor.getCurrent(
315-
Math.abs(lastVelRadPerSec), -RobotController.getInputVoltage()));
316290
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);
291+
double moduleTorque = driveMotor.getTorque(currentDraw);
322292

323293
double prevSpeed = vars.prevModuleStates[m].speedMetersPerSecond;
324294
vars.desiredModuleStates[m].optimize(vars.prevModuleStates[m].angle);
325295
double desiredSpeed = vars.desiredModuleStates[m].speedMetersPerSecond;
326296

327297
int forceSign;
328298
Rotation2d forceAngle = vars.prevModuleStates[m].angle;
329-
double moduleTorque;
330-
331299
if (epsilonEquals(prevSpeed, 0.0)
332300
|| (prevSpeed > 0 && desiredSpeed >= prevSpeed)
333301
|| (prevSpeed < 0 && desiredSpeed <= prevSpeed)) {
334-
moduleTorque = forwardModuleTorque;
335-
336302
// Torque loss will be fighting motor
337303
moduleTorque -= torqueLoss;
338304
forceSign = 1; // Force will be applied in direction of module
339305
if (prevSpeed < 0) {
340306
forceAngle = forceAngle.plus(Rotation2d.k180deg);
341307
}
342308
} else {
343-
moduleTorque = reverseModuleTorque;
344309
// Torque loss will be helping the motor
345310
moduleTorque += torqueLoss;
346311
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: 6 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1717
import frc.robot.extras.setpointGen.SwerveSetpoint;
1818
import frc.robot.extras.setpointGen.SwerveSetpointGenerator;
19+
import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
1920
import frc.robot.extras.util.DeviceCANBus;
2021
import frc.robot.extras.util.TimeUtil;
2122
import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
@@ -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

@@ -105,13 +106,6 @@ public SwerveDrive(
105106
gyroDisconnectedAlert.set(false);
106107
}
107108

108-
/** Updates the pose estimator with the pose calculated from the swerve modules. */
109-
// public void addPoseEstimatorSwerveMeasurement() {
110-
// for (int timestampIndex = 0;
111-
// timestampIndex < odometryThreadInputs.measurementTimeStamps.length;
112-
// timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex);
113-
// }
114-
115109
/*
116110
* Updates the pose estimator with the pose calculated from the april tags. How much it
117111
* contributes to the pose estimation is set by setPoseEstimatorVisionConfidence.
@@ -183,6 +177,7 @@ private void fetchOdometryInputs() {
183177
odometryThread.unlockOdometry();
184178
}
185179

180+
/** Runs the SwerveModules periodic methods */
186181
private void modulesPeriodic() {
187182
for (SwerveModule module : swerveModules) module.periodic();
188183
}
@@ -202,7 +197,7 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi
202197
xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d())
203198
: new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
204199

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

207202
setModuleStates(setpoint.moduleStates());
208203
Logger.recordOutput("SwerveStates/DesiredStates", setpoint.moduleStates());
@@ -289,23 +284,6 @@ public void setModuleStates(SwerveModuleState[] desiredStates) {
289284
*
290285
* @param timestampIndex index of the timestamp to sample the pose at
291286
*/
292-
// private void addPoseEstimatorSwerveMeasurement(int timestampIndex) {
293-
// final SwerveModulePosition[] modulePositions = getModulePositions(),
294-
// moduleDeltas = getModulesDelta(modulePositions);
295-
296-
// if (gyroInputs.isConnected) {
297-
// rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex];
298-
// } else {
299-
// Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas);
300-
// rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
301-
// }
302-
303-
// poseEstimator.updateWithTime(
304-
// odometryThreadInputs.measurementTimeStamps[timestampIndex],
305-
// rawGyroRotation,
306-
// modulePositions);
307-
// }
308-
309287
public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex
310288
final SwerveModulePosition[] modulePositions = getModulePositions(),
311289
moduleDeltas = getModulesDelta(modulePositions);
@@ -324,21 +302,9 @@ public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex
324302
}
325303

326304
/**
327-
* Gets the modules positions, sampled at the indexed timestamp.
328-
*
329-
* @param timestampIndex the timestamp index to sample.
330-
* @return a list of SwerveModulePosition, containing relative drive position and absolute turn
331-
* rotation at the sampled timestamp.
305+
* @param freshModulesPosition Latest module positions
306+
* @return The change of the module positions between the current and last update
332307
*/
333-
// private SwerveModulePosition[] getModulesPosition(int timestampIndex) {
334-
// SwerveModulePosition[] swerveModulePositions = new
335-
// SwerveModulePosition[swerveModules.length];
336-
// for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++)
337-
// swerveModulePositions[moduleIndex] =
338-
// swerveModules[moduleIndex].getOdometryPositions()[timestampIndex];
339-
// return swerveModulePositions;
340-
// }
341-
342308
private SwerveModulePosition[] getModulesDelta(SwerveModulePosition[] freshModulesPosition) {
343309
SwerveModulePosition[] deltas = new SwerveModulePosition[swerveModules.length];
344310
for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {

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

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ public SwerveModule(ModuleInterface io, String name) {
3232
CommandScheduler.getInstance().unregisterSubsystem(this);
3333
}
3434

35+
/** Updates the module's odometry inputs. */
3536
public void updateOdometryInputs() {
3637
io.updateInputs(inputs);
3738
Logger.processInputs("Drive/Module-" + name, inputs);
@@ -41,15 +42,18 @@ public void updateOdometryInputs() {
4142
@Override
4243
public void periodic() {}
4344

45+
/** Sets the drive voltage of the module. */
4446
public void setVoltage(Voltage volts) {
4547
io.setDriveVoltage(volts);
4648
io.setTurnVoltage(Volts.zero());
4749
}
4850

51+
/** Gets the drive voltage of the module. */
4952
public double getDriveVoltage() {
5053
return inputs.driveAppliedVolts;
5154
}
5255

56+
/** Sets the drive velocity of the module. */
5357
public double getCharacterizationVelocity() {
5458
return inputs.driveVelocity;
5559
}
@@ -61,6 +65,7 @@ public void runSetPoint(SwerveModuleState state) {
6165
io.stopModule();
6266
}
6367
io.setDesiredState(state);
68+
Logger.recordOutput("Drive/desired turn angle", state.angle.getRotations());
6469
}
6570

6671
/** 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)