Skip to content

Commit

Permalink
Merge pull request #13 from Iron-Panthers/beta-vendordeps
Browse files Browse the repository at this point in the history
2025 beta versions implemented in code
  • Loading branch information
audreypj authored Dec 4, 2024
2 parents 19d149d + 681b4f8 commit 1b11b2a
Show file tree
Hide file tree
Showing 15 changed files with 250 additions and 255 deletions.
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
id "com.peterabeles.gversion" version "1.10"
id "com.diffplug.spotless" version "6.12.0"
}
Expand Down
2 changes: 1 addition & 1 deletion gradle/wrapper/gradle-wrapper.properties
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
distributionBase=GRADLE_USER_HOME
distributionPath=permwrapper/dists
distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip
distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
networkTimeout=10000
validateDistributionUrl=true
zipStoreBase=GRADLE_USER_HOME
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,23 +9,29 @@
import com.ctre.phoenix6.controls.NeutralOut;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Temperature;
import edu.wpi.first.units.measure.Voltage;

public class FlywheelsIOTalonFX implements FlywheelsIO {
private final TalonFX topTalon;
private final TalonFX bottomTalon;

private final StatusSignal<Double> topPositionRads;
private final StatusSignal<Double> topVelocityRPM;
private final StatusSignal<Double> topAppliedVolts;
private final StatusSignal<Double> topSupplyCurrent;
private final StatusSignal<Double> topTemp;
private final StatusSignal<Double> bottomPositionRads;
private final StatusSignal<Double> bottomVelocityRPM;
private final StatusSignal<Double> bottomAppliedVolts;
private final StatusSignal<Double> bottomSupplyCurrent;
private final StatusSignal<Double> bottomTemp;
private final StatusSignal<Angle> topPosition;
private final StatusSignal<AngularVelocity> topVelocity;
private final StatusSignal<Voltage> topAppliedVolts;
private final StatusSignal<Current> topSupplyCurrent;
private final StatusSignal<Temperature> topTemp;
private final StatusSignal<Angle> bottomPosition;
private final StatusSignal<AngularVelocity> bottomVelocity;
private final StatusSignal<Voltage> bottomAppliedVolts;
private final StatusSignal<Current> bottomSupplyCurrent;
private final StatusSignal<Temperature> bottomTemp;

private final Slot0Configs gainsConfig = new Slot0Configs();
private final VelocityVoltage velocityOutput = new VelocityVoltage(0).withUpdateFreqHz(0);
Expand All @@ -39,6 +45,7 @@ public FlywheelsIOTalonFX() {
config.CurrentLimits.SupplyCurrentLimit = 60.0; // FIXME
config.CurrentLimits.SupplyCurrentLimitEnable = true;
config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
config.Feedback.SensorToMechanismRatio = FLYWHEEL_CONFIG.reduction();

gainsConfig.kP = GAINS.kP();
Expand All @@ -53,17 +60,14 @@ public FlywheelsIOTalonFX() {
topTalon.getConfigurator().apply(gainsConfig);
bottomTalon.getConfigurator().apply(gainsConfig);

topTalon.setInverted(true);
bottomTalon.setInverted(true); // FIXME

topPositionRads = topTalon.getPosition();
topVelocityRPM = topTalon.getVelocity();
topPosition = topTalon.getPosition();
topVelocity = topTalon.getVelocity();
topAppliedVolts = topTalon.getMotorVoltage();
topSupplyCurrent = topTalon.getSupplyCurrent();
topTemp = topTalon.getDeviceTemp();

bottomPositionRads = bottomTalon.getPosition();
bottomVelocityRPM = bottomTalon.getVelocity();
bottomPosition = bottomTalon.getPosition();
bottomVelocity = bottomTalon.getVelocity();
bottomAppliedVolts = bottomTalon.getMotorVoltage();
bottomSupplyCurrent = bottomTalon.getSupplyCurrent();
bottomTemp = bottomTalon.getDeviceTemp();
Expand All @@ -73,25 +77,21 @@ public FlywheelsIOTalonFX() {
public void updateInputs(FlywheelsIOInputs inputs) {
inputs.topMotorConnected =
BaseStatusSignal.refreshAll(
topPositionRads, topVelocityRPM, topAppliedVolts, topSupplyCurrent, topTemp)
topPosition, topVelocity, topAppliedVolts, topSupplyCurrent, topTemp)
.isOK();
inputs.bottomMotorConnected =
BaseStatusSignal.refreshAll(
bottomPositionRads,
bottomVelocityRPM,
bottomAppliedVolts,
bottomSupplyCurrent,
bottomTemp)
bottomPosition, bottomVelocity, bottomAppliedVolts, bottomSupplyCurrent, bottomTemp)
.isOK();

inputs.topPositionRads = Units.rotationsToRadians(topPositionRads.getValueAsDouble());
inputs.topVelocityRPM = topVelocityRPM.getValueAsDouble() * 60.0;
inputs.topPositionRads = Units.rotationsToRadians(topPosition.getValueAsDouble());
inputs.topVelocityRPM = topVelocity.getValueAsDouble() * 60.0;
inputs.topAppliedVolts = topAppliedVolts.getValueAsDouble();
inputs.topSupplyCurrent = topSupplyCurrent.getValueAsDouble();
inputs.topTempCelcius = topTemp.getValueAsDouble();

inputs.bottomPositionRads = Units.rotationsToRadians(bottomPositionRads.getValueAsDouble());
inputs.bottomVelocityRPM = bottomVelocityRPM.getValueAsDouble() * 60.0;
inputs.bottomPositionRads = Units.rotationsToRadians(bottomPosition.getValueAsDouble());
inputs.bottomVelocityRPM = bottomVelocity.getValueAsDouble() * 60.0;
inputs.bottomAppliedVolts = bottomAppliedVolts.getValueAsDouble();
inputs.bottomSupplyCurrent = bottomSupplyCurrent.getValueAsDouble();
inputs.bottomTempCelcius = bottomTemp.getValueAsDouble();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,18 @@
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Voltage;

public abstract class GenericRollersIOTalonFX implements GenericRollersIO {
private final TalonFX talon;

private final StatusSignal<Double> position;
private final StatusSignal<Double> velocity;
private final StatusSignal<Double> appliedVolts;
private final StatusSignal<Double> supplyCurrent;
private final StatusSignal<Angle> position;
private final StatusSignal<AngularVelocity> velocity;
private final StatusSignal<Voltage> appliedVolts;
private final StatusSignal<Current> supplyCurrent;

private final VoltageOut voltageOutput = new VoltageOut(0).withUpdateFreqHz(0);
private final NeutralOut neutralOutput = new NeutralOut();
Expand Down
7 changes: 5 additions & 2 deletions src/main/java/frc/robot/subsystems/swerve/Drive.java
Original file line number Diff line number Diff line change
Expand Up @@ -74,15 +74,18 @@ public void periodic() {
/* use kinematics to get desired module states */
ChassisSpeeds discretizedSpeeds =
ChassisSpeeds.discretize(targetSpeeds, Constants.PERIODIC_LOOP_SEC);
/* ChassisSpeeds discretizedSpeeds = targetSpeeds; // FIXME
discretizedSpeeds.discretize(Constants.PERIODIC_LOOP_SEC); */

SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(discretizedSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(
moduleTargetStates, DRIVE_CONFIG.maxLinearVelocity());

SwerveModuleState[] optimizedTargetStates = new SwerveModuleState[4];

for (int i = 0; i < modules.length; i++) {
optimizedTargetStates[i] =
SwerveModuleState.optimize(moduleTargetStates[i], modules[i].getSteerHeading());
optimizedTargetStates[i] = moduleTargetStates[i];
optimizedTargetStates[i].optimize(modules[i].getSteerHeading());
modules[i].runToSetpoint(optimizedTargetStates[i]);
}

Expand Down
6 changes: 4 additions & 2 deletions src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,11 +6,13 @@
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;

public class GyroIOPigeon2 implements GyroIO {
private final Pigeon2 pigeon;
private final StatusSignal<Double> yaw;
private final StatusSignal<Double> yawVelocity;
private final StatusSignal<Angle> yaw;
private final StatusSignal<AngularVelocity> yawVelocity;

public GyroIOPigeon2() {
pigeon = new Pigeon2(DriveConstants.GYRO_ID);
Expand Down
24 changes: 14 additions & 10 deletions src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,10 @@
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Voltage;
import frc.robot.subsystems.swerve.DriveConstants.Gains;
import frc.robot.subsystems.swerve.DriveConstants.ModuleConfig;
import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains;
Expand All @@ -24,18 +28,18 @@ public class ModuleIOTalonFX implements ModuleIO {
private final TalonFX steerTalon;
private final CANcoder encoder;

private final StatusSignal<Double> drivePosition;
private final StatusSignal<Double> driveVelocity;
private final StatusSignal<Double> driveAppliedVolts;
private final StatusSignal<Double> driveSupplyCurrent;
private final StatusSignal<Double> driveStatorCurrent;
private final StatusSignal<Angle> drivePosition;
private final StatusSignal<AngularVelocity> driveVelocity;
private final StatusSignal<Voltage> driveAppliedVolts;
private final StatusSignal<Current> driveSupplyCurrent;
private final StatusSignal<Current> driveStatorCurrent;

private final Supplier<Rotation2d> steerAbsolutePosition;
private final StatusSignal<Double> steerPosition;
private final StatusSignal<Double> steerVelocity;
private final StatusSignal<Double> steerAppliedVolts;
private final StatusSignal<Double> steerSupplyCurrent;
private final StatusSignal<Double> steerStatorCurrent;
private final StatusSignal<Angle> steerPosition;
private final StatusSignal<AngularVelocity> steerVelocity;
private final StatusSignal<Voltage> steerAppliedVolts;
private final StatusSignal<Current> steerSupplyCurrent;
private final StatusSignal<Current> steerStatorCurrent;

private final TalonFXConfiguration driveConfig = new TalonFXConfiguration();
private final TalonFXConfiguration steerConfig = new TalonFXConfiguration();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,16 @@ public ChassisSpeeds update(Rotation2d yaw) {
linearVelocity.getY() * DRIVE_CONFIG.maxLinearVelocity(),
omega * DRIVE_CONFIG.maxAngularVelocity(),
yaw);

/* ChassisSpeeds speeds =
new ChassisSpeeds(
linearVelocity.getX() * DRIVE_CONFIG.maxLinearVelocity(),
linearVelocity.getY() * DRIVE_CONFIG.maxLinearVelocity(),
omega * DRIVE_CONFIG.maxAngularVelocity());
// eventually run off of pose estimation?
speeds.toRobotRelativeSpeeds(yaw);
return speeds; */
}

public Translation2d calculateLinearVelocity(double x, double y) {
Expand Down
12 changes: 6 additions & 6 deletions vendordeps/AdvantageKit.json
Original file line number Diff line number Diff line change
@@ -1,33 +1,33 @@
{
"fileName": "AdvantageKit.json",
"name": "AdvantageKit",
"version": "3.2.1",
"version": "4.0.0-alpha-1",
"uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
"frcYear": "2024",
"frcYear": "2025",
"mavenUrls": [],
"jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
"javaDependencies": [
{
"groupId": "org.littletonrobotics.akit.junction",
"artifactId": "wpilib-shim",
"version": "3.2.1"
"version": "4.0.0-alpha-1"
},
{
"groupId": "org.littletonrobotics.akit.junction",
"artifactId": "junction-core",
"version": "3.2.1"
"version": "4.0.0-alpha-1"
},
{
"groupId": "org.littletonrobotics.akit.conduit",
"artifactId": "conduit-api",
"version": "3.2.1"
"version": "4.0.0-alpha-1"
}
],
"jniDependencies": [
{
"groupId": "org.littletonrobotics.akit.conduit",
"artifactId": "conduit-wpilibio",
"version": "3.2.1",
"version": "4.0.0-alpha-1",
"skipInvalidPlatforms": false,
"isJar": false,
"validPlatforms": [
Expand Down
Original file line number Diff line number Diff line change
@@ -1,38 +1,38 @@
{
"fileName": "PathplannerLib.json",
"fileName": "PathplannerLib-beta.json",
"name": "PathplannerLib",
"version": "2024.2.8",
"version": "2025.0.0-beta-5",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2024",
"frcYear": "2025",
"mavenUrls": [
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
],
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json",
"javaDependencies": [
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
"version": "2024.2.8"
"version": "2025.0.0-beta-5"
}
],
"jniDependencies": [],
"cppDependencies": [
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
"version": "2024.2.8",
"version": "2025.0.0-beta-5",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
"osxuniversal",
"linuxx86-64",
"linuxathena",
"linuxarm32",
"linuxarm64"
]
}
]
}
}
Loading

0 comments on commit 1b11b2a

Please sign in to comment.