PLS5618/Reefscape-2025
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Swerve + commencer pathplanner

Browse Source
This commit is contained in:
samuel desharnais 2025-01-27 20:15:31 -05:00
parent edbff8f4dd
commit 4b0262fa4d
13 changed files with 1201 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/deploy/pathplanner/navgrid.json Normal file
View File

File diff suppressed because one or more lines are too long

54
src/main/deploy/pathplanner/paths/21 x 27.5.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.066905737704918,
"y": 1.058043032786885
},
"prevControl": null,
"nextControl": {
"x": 2.4151156514267984,
"y": 3.2911993104946093
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.7641393442622952,
"y": 2.832223360655737
},
"prevControl": {
"x": 2.7003578170234857,
"y": 1.9970799919720519
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 59.99999999999999
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -125.00000000000001
},
"useDefaultConstraints": true
}

70
src/main/deploy/pathplanner/paths/BlueBasChercher.path Normal file
View File

@ -0,0 +1,70 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 5.154713114754099,
"y": 2.832223360655737
},
"prevControl": null,
"nextControl": {
"x": 4.627254098360655,
"y": 2.2568135245901635
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.296618852459016,
"y": 1.6334528688524586
},
"prevControl": {
"x": 4.5553278688524586,
"y": 1.9091700819672122
},
"nextControl": {
"x": 2.853313965378558,
"y": 1.5363479888253109
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.1508196721311474,
"y": 1.1299692622950817
},
"prevControl": {
"x": 2.1498657381584616,
"y": 1.3782484974959424
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -125.00000000000001
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 119.99999999999999
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/BlueBasStart.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.204610655737704,
"y": 1.058043032786885
},
"prevControl": null,
"nextControl": {
"x": 6.400623816881145,
"y": 1.8077934662846444
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.25061475409836,
"y": 2.820235655737705
},
"prevControl": {
"x": 6.057528863906536,
"y": 2.075180182319969
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 119.99999999999999
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 119.99999999999999
},
"useDefaultConstraints": true
}

70
src/main/deploy/pathplanner/paths/BlueHautChercher.path Normal file
View File

@ -0,0 +1,70 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 5.202663934426229,
"y": 5.205788934426229
},
"prevControl": null,
"nextControl": {
"x": 4.195696721311475,
"y": 6.104866803278688
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.104815573770492,
"y": 6.296670081967212
},
"prevControl": {
"x": 4.495389344262295,
"y": 5.949026639344262
},
"nextControl": {
"x": 2.5881297183495136,
"y": 6.425841545822457
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.2107581967213117,
"y": 6.955993852459016
},
"prevControl": {
"x": 2.302517803906852,
"y": 6.503295936938067
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 125.00000000000001
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -119.99999999999999
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/BlueHautPorter.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.234733606557377,
"y": 6.9679815573770485
},
"prevControl": null,
"nextControl": {
"x": 2.271389052764998,
"y": 4.855519714825475
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.7761270491803276,
"y": 5.205788934426229
},
"prevControl": {
"x": 2.911254448656024,
"y": 6.06899512479262
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -59.99999999999999
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 125.00000000000001
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/BlueHautStart.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.22858606557377,
"y": 7.075870901639344
},
"prevControl": null,
"nextControl": {
"x": 5.75022271481963,
"y": 5.75328924697144
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.166700819672132,
"y": 5.205788934426229
},
"prevControl": {
"x": 6.247675208676453,
"y": 6.156109471736201
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -119.99999999999999
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -119.99999999999999
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/BlueMilieuChercher.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 5.838012295081967,
"y": 3.983043032786885
},
"prevControl": null,
"nextControl": {
"x": 6.916905737704918,
"y": 2.304764344262295
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.1508196721311474,
"y": 1.1299692622950817
},
"prevControl": {
"x": 2.125184035848923,
"y": 1.1165271593444053
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -125.00000000000001
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/BlueMilieuStart.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.300512295081968,
"y": 3.971055327868852
},
"prevControl": null,
"nextControl": {
"x": 6.294286688638351,
"y": 3.9754565484205786
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.8619877049180324,
"y": 3.971055327868852
},
"prevControl": {
"x": 6.836352068635808,
"y": 3.9576132249181755
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
}

32
src/main/deploy/pathplanner/settings.json Normal file
View File

@ -0,0 +1,32 @@
{
"robotWidth": 0.9,
"robotLength": 0.9,
"holonomicMode": true,
"pathFolders": [],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0,
"robotMass": 74.088,
"robotMOI": 6.883,
"robotTrackwidth": 0.546,
"driveWheelRadius": 0.048,
"driveGearing": 5.143,
"maxDriveSpeed": 5.45,
"driveMotorType": "krakenX60",
"driveCurrentLimit": 60.0,
"wheelCOF": 1.2,
"flModuleX": 0.35,
"flModuleY": 0.267,
"frModuleX": 0.273,
"frModuleY": -0.273,
"blModuleX": -0.273,
"blModuleY": 0.273,
"brModuleX": -0.273,
"brModuleY": -0.273,
"bumperOffsetX": 0.0,
"bumperOffsetY": 0.0,
"robotFeatures": []
}

124
src/main/java/frc/robot/Telemetry.java Normal file
View File

@ -0,0 +1,124 @@
package frc.robot;
import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StructArrayPublisher;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
public class Telemetry {
private final double MaxSpeed;
/**
* Construct a telemetry object, with the specified max speed of the robot
*
* @param maxSpeed Maximum speed in meters per second
*/
public Telemetry(double maxSpeed) {
MaxSpeed = maxSpeed;
SignalLogger.start();
}
/* What to publish over networktables for telemetry */
private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
/* Robot swerve drive state */
private final NetworkTable driveStateTable = inst.getTable("DriveState");
private final StructPublisher<Pose2d> drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish();
private final StructPublisher<ChassisSpeeds> driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish();
private final StructArrayPublisher<SwerveModuleState> driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish();
private final StructArrayPublisher<SwerveModuleState> driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish();
private final StructArrayPublisher<SwerveModulePosition> driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish();
private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish();
private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish();
/* Robot pose for field positioning */
private final NetworkTable table = inst.getTable("Pose");
private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
/* Mechanisms to represent the swerve module states */
private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
new Mechanism2d(1, 1),
new Mechanism2d(1, 1),
new Mechanism2d(1, 1),
new Mechanism2d(1, 1),
};
/* A direction and length changing ligament for speed representation */
private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] {
m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
};
/* A direction changing and length constant ligament for module direction */
private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] {
m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
};
private final double[] m_poseArray = new double[3];
private final double[] m_moduleStatesArray = new double[8];
private final double[] m_moduleTargetsArray = new double[8];
/** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
public void telemeterize(SwerveDriveState state) {
/* Telemeterize the swerve drive state */
drivePose.set(state.Pose);
driveSpeeds.set(state.Speeds);
driveModuleStates.set(state.ModuleStates);
driveModuleTargets.set(state.ModuleTargets);
driveModulePositions.set(state.ModulePositions);
driveTimestamp.set(state.Timestamp);
driveOdometryFrequency.set(1.0 / state.OdometryPeriod);
/* Also write to log file */
m_poseArray[0] = state.Pose.getX();
m_poseArray[1] = state.Pose.getY();
m_poseArray[2] = state.Pose.getRotation().getDegrees();
for (int i = 0; i < 4; ++i) {
m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians();
m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond;
m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians();
m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond;
}
SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray);
SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray);
SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray);
SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");
/* Telemeterize the pose to a Field2d */
fieldTypePub.set("Field2d");
fieldPub.set(m_poseArray);
/* Telemeterize the module states to a Mechanism2d */
for (int i = 0; i < 4; ++i) {
m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));
SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
}
}
}

286
src/main/java/frc/robot/TunerConstants/TunerConstants.java Normal file
View File

@ -0,0 +1,286 @@
package frc.robot.TunerConstants;
import static edu.wpi.first.units.Units.*;
import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.configs.*;
import com.ctre.phoenix6.hardware.*;
import com.ctre.phoenix6.signals.*;
import com.ctre.phoenix6.swerve.*;
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.measure.*;
import frc.robot.subsystems.CommandSwerveDrivetrain;
// Generated by the Tuner X Swerve Project Generator
// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
public class TunerConstants {
// Both sets of gains need to be tuned to your individual robot.
// The steer motor uses any SwerveModule.SteerRequestType control request with the
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
private static final Slot0Configs steerGains = new Slot0Configs()
.withKP(100).withKI(0).withKD(0.5)
.withKS(0.1).withKV(1.91).withKA(0)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
// When using closed-loop control, the drive motor uses the control
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
private static final Slot0Configs driveGains = new Slot0Configs()
.withKP(0.1).withKI(0).withKD(0)
.withKS(0).withKV(0.124);
// The closed-loop output type to use for the steer motors;
// This affects the PID/FF gains for the steer motors
private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
// The closed-loop output type to use for the drive motors;
// This affects the PID/FF gains for the drive motors
private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
// The type of motor used for the drive motor
private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
// The type of motor used for the drive motor
private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
// The remote sensor feedback type to use for the steer motors;
// When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
// The stator current at which the wheels start to slip;
// This needs to be tuned to your individual robot
private static final Current kSlipCurrent = Amps.of(120.0);
// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
// Swerve azimuth does not require much torque output, so we can set a relatively low
// stator current limit to help avoid brownouts without impacting performance.
.withStatorCurrentLimit(Amps.of(60))
.withStatorCurrentLimitEnable(true)
);
private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
private static final Pigeon2Configuration pigeonConfigs = null;
// CAN bus that the devices are located on;
// All swerve devices must share the same CAN bus
public static final CANBus kCANBus = new CANBus("swerve", "./logs/example.hoot");
// Theoretical free speed (m/s) at 12 V applied output;
// This needs to be tuned to your individual robot
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21);
// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
// This may need to be tuned to your individual robot
private static final double kCoupleRatio = 0;
private static final double kDriveGearRatio = 6.122448979591837;
private static final double kSteerGearRatio = 15.42857142857143;
private static final Distance kWheelRadius = Inches.of(2);
private static final boolean kInvertLeftSide = false;
private static final boolean kInvertRightSide = true;
private static final int kPigeonId = 13;
// These are only used for simulation
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
// Simulated voltage necessary to overcome friction
private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
.withCANBusName(kCANBus.getName())
.withPigeon2Id(kPigeonId)
.withPigeon2Configs(pigeonConfigs);
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
.withDriveMotorGearRatio(kDriveGearRatio)
.withSteerMotorGearRatio(kSteerGearRatio)
.withCouplingGearRatio(kCoupleRatio)
.withWheelRadius(kWheelRadius)
.withSteerMotorGains(steerGains)
.withDriveMotorGains(driveGains)
.withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
.withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
.withSlipCurrent(kSlipCurrent)
.withSpeedAt12Volts(kSpeedAt12Volts)
.withDriveMotorType(kDriveMotorType)
.withSteerMotorType(kSteerMotorType)
.withFeedbackSource(kSteerFeedbackType)
.withDriveMotorInitialConfigs(driveInitialConfigs)
.withSteerMotorInitialConfigs(steerInitialConfigs)
.withEncoderInitialConfigs(encoderInitialConfigs)
.withSteerInertia(kSteerInertia)
.withDriveInertia(kDriveInertia)
.withSteerFrictionVoltage(kSteerFrictionVoltage)
.withDriveFrictionVoltage(kDriveFrictionVoltage);
// Front Left
private static final int kFrontLeftDriveMotorId = 2;
private static final int kFrontLeftSteerMotorId = 6;
private static final int kFrontLeftEncoderId = 9;
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.046142578125);
private static final boolean kFrontLeftSteerMotorInverted = true;
private static final boolean kFrontLeftEncoderInverted = false;
private static final Distance kFrontLeftXPos = Inches.of(13.75);
private static final Distance kFrontLeftYPos = Inches.of(13.75);
// Front Right
private static final int kFrontRightDriveMotorId = 4;
private static final int kFrontRightSteerMotorId = 5;
private static final int kFrontRightEncoderId = 12;
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.116455078125);
private static final boolean kFrontRightSteerMotorInverted = true;
private static final boolean kFrontRightEncoderInverted = false;
private static final Distance kFrontRightXPos = Inches.of(13.75);
private static final Distance kFrontRightYPos = Inches.of(-13.75);
// Back Left
private static final int kBackLeftDriveMotorId = 3;
private static final int kBackLeftSteerMotorId = 7;
private static final int kBackLeftEncoderId = 10;
private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.046630859375);
private static final boolean kBackLeftSteerMotorInverted = true;
private static final boolean kBackLeftEncoderInverted = false;
private static final Distance kBackLeftXPos = Inches.of(-13.75);
private static final Distance kBackLeftYPos = Inches.of(13.75);
// Back Right
private static final int kBackRightDriveMotorId = 18;
private static final int kBackRightSteerMotorId = 8;
private static final int kBackRightEncoderId = 11;
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.016357421875);
private static final boolean kBackRightSteerMotorInverted = true;
private static final boolean kBackRightEncoderInverted = false;
private static final Distance kBackRightXPos = Inches.of(-13.75);
private static final Distance kBackRightYPos = Inches.of(-13.75);
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
ConstantCreator.createModuleConstants(
kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset,
kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted
);
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
ConstantCreator.createModuleConstants(
kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted
);
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
ConstantCreator.createModuleConstants(
kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted
);
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
ConstantCreator.createModuleConstants(
kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset,
kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted
);
/**
* Creates a CommandSwerveDrivetrain instance.
* This should only be called once in your robot program,.
*/
public static CommandSwerveDrivetrain createDrivetrain() {
return new CommandSwerveDrivetrain(
DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
);
}
/**
* Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
*/
public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param modules Constants for each specific module
*/
public TunerSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules
) {
super(
TalonFX::new, TalonFX::new, CANcoder::new,
drivetrainConstants, modules
);
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
* unspecified or set to 0 Hz, this is 250 Hz on
* CAN FD, and 100 Hz on CAN 2.0.
* @param modules Constants for each specific module
*/
public TunerSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
SwerveModuleConstants<?, ?, ?>... modules
) {
super(
TalonFX::new, TalonFX::new, CANcoder::new,
drivetrainConstants, odometryUpdateFrequency, modules
);
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
* unspecified or set to 0 Hz, this is 250 Hz on
* CAN FD, and 100 Hz on CAN 2.0.
* @param odometryStandardDeviation The standard deviation for odometry calculation
* in the form [x, y, theta]ᵀ, with units in meters
* and radians
* @param visionStandardDeviation The standard deviation for vision calculation
* in the form [x, y, theta]ᵀ, with units in meters
* and radians
* @param modules Constants for each specific module
*/
public TunerSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
Matrix<N3, N1> odometryStandardDeviation,
Matrix<N3, N1> visionStandardDeviation,
SwerveModuleConstants<?, ?, ?>... modules
) {
super(
TalonFX::new, TalonFX::new, CANcoder::new,
drivetrainConstants, odometryUpdateFrequency,
odometryStandardDeviation, visionStandardDeviation, modules
);
}
}
}

294
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java Normal file
View File

@ -0,0 +1,294 @@
package frc.robot.subsystems;
import static edu.wpi.first.units.Units.*;
import java.util.function.Supplier;
import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain;
/**
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
* Subsystem so it can easily be used in command-based projects.
*/
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
private static final double kSimLoopPeriod = 0.005; // 5 ms
private Notifier m_simNotifier = null;
private double m_lastSimTime;
/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
/* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
/* Keep track if we've ever applied the operator perspective before or not */
private boolean m_hasAppliedOperatorPerspective = false;
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
/* Swerve requests to apply during SysId characterization */
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
),
new SysIdRoutine.Mechanism(
output -> setControl(m_translationCharacterization.withVolts(output)),
null,
this
)
);
/* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(7), // Use dynamic voltage of 7 V
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
),
new SysIdRoutine.Mechanism(
volts -> setControl(m_steerCharacterization.withVolts(volts)),
null,
this
)
);
/*
* SysId routine for characterizing rotation.
* This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
*/
private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
new SysIdRoutine.Config(
/* This is in radians per second², but SysId only supports "volts per second" */
Volts.of(Math.PI / 6).per(Second),
/* This is in radians per second, but SysId only supports "volts" */
Volts.of(Math.PI),
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
),
new SysIdRoutine.Mechanism(
output -> {
/* output is actually radians per second, but SysId only supports "volts" */
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
/* also log the requested output for SysId */
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
},
null,
this
)
);
private void configureAutoBuilder() {
try {
var config = RobotConfig.fromGUISettings();
AutoBuilder.configure(
() -> getState().Pose, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto
() -> getState().Speeds, // Supplier of current robot speeds
// Consumer of ChassisSpeeds and feedforwards to drive the robot
(speeds, feedforwards) -> setControl(
m_pathApplyRobotSpeeds.withSpeeds(speeds)
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
),
new PPHolonomicDriveController(
// PID constants for translation
new PIDConstants(10, 0, 0),
// PID constants for rotation
new PIDConstants(7, 0, 0)
),
config,
// Assume the path needs to be flipped for Red vs Blue, this is normally the case
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
this // Subsystem for requirements
);
} catch (Exception ex) {
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
}
}
/* The SysId routine to test */
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param modules Constants for each specific module
*/
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules
) {
super(drivetrainConstants, modules);
if (Utils.isSimulation()) {
startSimThread();
}
configureAutoBuilder();
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
* unspecified or set to 0 Hz, this is 250 Hz on
* CAN FD, and 100 Hz on CAN 2.0.
* @param modules Constants for each specific module
*/
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
SwerveModuleConstants<?, ?, ?>... modules
) {
super(drivetrainConstants, odometryUpdateFrequency, modules);
if (Utils.isSimulation()) {
startSimThread();
}
configureAutoBuilder();
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
* unspecified or set to 0 Hz, this is 250 Hz on
* CAN FD, and 100 Hz on CAN 2.0.
* @param odometryStandardDeviation The standard deviation for odometry calculation
* in the form [x, y, theta]ᵀ, with units in meters
* and radians
* @param visionStandardDeviation The standard deviation for vision calculation
* in the form [x, y, theta]ᵀ, with units in meters
* and radians
* @param modules Constants for each specific module
*/
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
Matrix<N3, N1> odometryStandardDeviation,
Matrix<N3, N1> visionStandardDeviation,
SwerveModuleConstants<?, ?, ?>... modules
) {
super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules);
if (Utils.isSimulation()) {
startSimThread();
}
configureAutoBuilder();
}
/**
* Returns a command that applies the specified control request to this swerve drivetrain.
*
* @param request Function returning the request to apply
* @return Command to run
*/
public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
return run(() -> this.setControl(requestSupplier.get()));
}
/**
* Runs the SysId Quasistatic test in the given direction for the routine
* specified by {@link #m_sysIdRoutineToApply}.
*
* @param direction Direction of the SysId Quasistatic test
* @return Command to run
*/
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return m_sysIdRoutineToApply.quasistatic(direction);
}
/**
* Runs the SysId Dynamic test in the given direction for the routine
* specified by {@link #m_sysIdRoutineToApply}.
*
* @param direction Direction of the SysId Dynamic test
* @return Command to run
*/
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return m_sysIdRoutineToApply.dynamic(direction);
}
@Override
public void periodic() {
/*
* Periodically try to apply the operator perspective.
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
* This allows us to correct the perspective in case the robot code restarts mid-match.
* Otherwise, only check and apply the operator perspective if the DS is disabled.
* This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
*/
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
DriverStation.getAlliance().ifPresent(allianceColor -> {
setOperatorPerspectiveForward(
allianceColor == Alliance.Red
? kRedAlliancePerspectiveRotation
: kBlueAlliancePerspectiveRotation
);
m_hasAppliedOperatorPerspective = true;
});
}
}
private void startSimThread() {
m_lastSimTime = Utils.getCurrentTimeSeconds();
/* Run simulation at a faster rate so PID gains behave more reasonably */
m_simNotifier = new Notifier(() -> {
final double currentTime = Utils.getCurrentTimeSeconds();
double deltaTime = currentTime - m_lastSimTime;
m_lastSimTime = currentTime;
/* use the measured time delta, get battery voltage from WPILib */
updateSimState(deltaTime, RobotController.getBatteryVoltage());
});
m_simNotifier.startPeriodic(kSimLoopPeriod);
}
}