diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/300 Right to HP.path b/src/main/deploy/pathplanner/paths/300 Right to HP.path new file mode 100644 index 0000000..f84b1d1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/300 Right to HP.path @@ -0,0 +1,72 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6061643835666786, + "y": 5.031720890416444 + }, + "prevControl": null, + "nextControl": { + "x": 3.065239726031196, + "y": 5.647773972598556 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2510679859194649, + "y": 7.0812357195448 + }, + "prevControl": { + "x": 1.6678510274010594, + "y": 6.6394691780780075 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.498997995991984, + "rotationDegrees": -52.46519085612145 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Score L4", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "HP Pickup", + "waypointRelativePos": 0.16666666666666663, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.98486432191523 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP to 300 Left.path b/src/main/deploy/pathplanner/paths/HP to 300 Left.path new file mode 100644 index 0000000..c12f794 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP to 300 Left.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3072345890410957, + "y": 7.135316780821918 + }, + "prevControl": null, + "nextControl": { + "x": 2.5994434931506847, + "y": 6.909931506849315 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.026883561643835, + "y": 5.257106164383561 + }, + "prevControl": { + "x": 3.7113441780821916, + "y": 5.783005136986301 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Score L4", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.69923999693802 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -53.97262661489646 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..aae9a27 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.8763, + "robotLength": 0.8763, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 4.0, + "defaultMaxAccel": 4.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 48.35, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.038, + "driveGearing": 4.29, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 65.0, + "wheelCOF": 1.1, + "flModuleX": 0.31115, + "flModuleY": 0.31115, + "frModuleX": 0.31115, + "frModuleY": -0.31115, + "blModuleX": -0.31115, + "blModuleY": 0.31115, + "brModuleX": -0.31115, + "brModuleY": -0.31115, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 0bfbc31..963f4ed 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -11,8 +11,8 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.trajectory.TrapezoidProfile; public class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxSpeedMetersPerSecond = 4; + public static final double kMaxAccelerationMetersPerSecondSquared = 4; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index b3ecead..ebef85d 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -17,9 +17,9 @@ public class DrivetrainConstants { public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second // Chassis configuration - public static final double kTrackWidth = Units.inchesToMeters(26.5); + public static final double kTrackWidth = Units.inchesToMeters(24.5); // Distance between centers of right and left wheels on robot - public static final double kWheelBase = Units.inchesToMeters(26.5); + public static final double kWheelBase = Units.inchesToMeters(24.5); // Angular offsets of the modules relative to the chassis in radians public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; @@ -48,7 +48,7 @@ public class DrivetrainConstants { public static final int kFrontRightTurningCanId = 14; public static final int kRearRightTurningCanId = 16; - public static final boolean kGyroReversed = false; + public static final boolean kGyroReversed = true; // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 6982271..d2d5263 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -19,7 +19,7 @@ public class ElevatorConstants { public static final double kEncoderConversionFactor = 0; - public static final int kMotorAmpsMax = 0; + public static final int kMotorAmpsMax = 40; public static final double kPositionControllerP = 0; public static final double kPositionControllerI = 0; @@ -30,8 +30,8 @@ public class ElevatorConstants { public static final double kVelocityControllerD = 0; public static final double kFeedForwardS = 0; - public static final double kFeedForwardG = 0; - public static final double kFeedForwardV = 0; + public static final double kFeedForwardG = 0.41; // calculated value + public static final double kFeedForwardV = 1.75; // calculated value public static final double kMaxVelocity = 0; diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index 5d8196f..cc91d7b 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -17,7 +17,7 @@ public class ManipulatorPivotConstants { public static final int kArmMotorID = 0; public static final int kCANcoderID = 0; - public static final int kMotorAmpsMax = 0; + public static final int kMotorAmpsMax = 40; public static final double kPivotMaxVelocity = 0; @@ -29,6 +29,11 @@ public class ManipulatorPivotConstants { public static final double kVelocityP = 0; public static final double kVelocityI = 0; public static final double kVelocityD = 0; + + public static final double kFeedForwardS = 0; + public static final double kFeedForwardG = 0.41; // calculated value + public static final double kFeedForwardV = 0.68; //calculated value + // TODO Is this reasonable? public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60; diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index a5f1f87..2048d3d 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -41,7 +41,8 @@ public class ModuleConstants { public static final double kTurnI = 0; public static final double kTurnD = 0; - public static final int kDriveMotorStatorCurrentLimit = 120; + public static final int kDriveMotorStatorCurrentLimit = 100; + public static final int kDriveMotorSupplyCurrentLimit = 65; public static final int kTurnMotorCurrentLimit = 20; public static final IdleMode kTurnIdleMode = IdleMode.kBrake; @@ -62,7 +63,9 @@ public class ModuleConstants { kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction; kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true; + kDriveCurrentLimitConfig.SupplyCurrentLimitEnable = true; kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit; + kDriveCurrentLimitConfig.SupplyCurrentLimit = kDriveMotorSupplyCurrentLimit; kDriveMotorConfig.Inverted = kDriveInversionState; kDriveMotorConfig.NeutralMode = kDriveIdleMode; diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index f5d868e..cf83963 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -22,120 +22,120 @@ import com.revrobotics.AbsoluteEncoder; import frc.robot.constants.ModuleConstants; public class MAXSwerveModule { - private final TalonFX m_drive; - private final SparkMax m_turningSpark; + private final TalonFX m_drive; + private final SparkMax m_turningSpark; - private final AbsoluteEncoder m_turningEncoder; + private final AbsoluteEncoder m_turningEncoder; - private final SparkClosedLoopController m_turningClosedLoopController; + private final SparkClosedLoopController m_turningClosedLoopController; - private final VelocityVoltage driveVelocityRequest; + private final VelocityVoltage driveVelocityRequest; - private double m_chassisAngularOffset = 0; - private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d()); + private double m_chassisAngularOffset = 0; + private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d()); - /** - * Constructs a MAXSwerveModule and configures the driving and turning motor, - * encoder, and PID controller. This configuration is specific to the REV - * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore - * Encoder. - */ - public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { - m_drive = new TalonFX(drivingCANId); - m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless); + /** + * Constructs a MAXSwerveModule and configures the driving and turning motor, + * encoder, and PID controller. This configuration is specific to the REV + * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore + * Encoder. + */ + public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { + m_drive = new TalonFX(drivingCANId); + m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless); - m_turningEncoder = m_turningSpark.getAbsoluteEncoder(); + m_turningEncoder = m_turningSpark.getAbsoluteEncoder(); - m_turningClosedLoopController = m_turningSpark.getClosedLoopController(); + m_turningClosedLoopController = m_turningSpark.getClosedLoopController(); - driveVelocityRequest = new VelocityVoltage(0).withSlot(0); + driveVelocityRequest = new VelocityVoltage(0).withSlot(0); - // Apply the respective configurations to the SPARKS. Reset parameters before - // applying the configuration to bring the SPARK to a known good state. Persist - // the settings to the SPARK to avoid losing them on a power cycle. - m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig); - m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig); - m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig); - m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config); + // Apply the respective configurations to the SPARKS. Reset parameters before + // applying the configuration to bring the SPARK to a known good state. Persist + // the settings to the SPARK to avoid losing them on a power cycle. + m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig); + m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig); + m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig); + m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config); - m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); + m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); - m_chassisAngularOffset = chassisAngularOffset; - m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition()); - m_drive.setPosition(0); - } + m_chassisAngularOffset = chassisAngularOffset; + m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition()); + m_drive.setPosition(0); + } - /** - * Returns the current state of the module. - * - * @return The current state of the module. - */ - public SwerveModuleState getState() { - // Apply chassis angular offset to the encoder position to get the position - // relative to the chassis. - return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(), - new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); - } + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + public SwerveModuleState getState() { + // Apply chassis angular offset to the encoder position to get the position + // relative to the chassis. + return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(), + new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); + } - /** - * Returns the current position of the module. - * - * @return The current position of the module. - */ - public SwerveModulePosition getPosition() { - // Apply chassis angular offset to the encoder position to get the position - // relative to the chassis. - return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(), - new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); - } + /** + * Returns the current position of the module. + * + * @return The current position of the module. + */ + public SwerveModulePosition getPosition() { + // Apply chassis angular offset to the encoder position to get the position + // relative to the chassis. + return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(), + new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); + } - /** - * Sets the desired state for the module. - * - * @param desiredState Desired state with speed and angle. - */ - public void setDesiredState(SwerveModuleState desiredState) { - // Apply chassis angular offset to the desired state. - SwerveModuleState correctedDesiredState = new SwerveModuleState(); - correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond; - correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset)); + /** + * Sets the desired state for the module. + * + * @param desiredState Desired state with speed and angle. + */ + public void setDesiredState(SwerveModuleState desiredState) { + // Apply chassis angular offset to the desired state. + SwerveModuleState correctedDesiredState = new SwerveModuleState(); + correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond; + correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset)); - // Optimize the reference state to avoid spinning further than 90 degrees. - correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition())); + // Optimize the reference state to avoid spinning further than 90 degrees. + correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition())); - // Command driving and turning SPARKS towards their respective setpoints. - m_drive.setControl( - driveVelocityRequest.withVelocity( - correctedDesiredState.speedMetersPerSecond - ).withFeedForward( - correctedDesiredState.speedMetersPerSecond - ) - ); + // Command driving and turning SPARKS towards their respective setpoints. + m_drive.setControl( + driveVelocityRequest.withVelocity( + correctedDesiredState.speedMetersPerSecond + ).withFeedForward( + correctedDesiredState.speedMetersPerSecond + ) + ); - m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition); + m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition); - m_desiredState = desiredState; - } + m_desiredState = desiredState; + } - public void setVoltageDrive(double voltage){ - m_drive.setVoltage(voltage); - } + public void setVoltageDrive(double voltage){ + m_drive.setVoltage(voltage); + } - public void setVoltageTurn(double voltage) { - m_turningSpark.setVoltage(voltage); - } + public void setVoltageTurn(double voltage) { + m_turningSpark.setVoltage(voltage); + } - public double getVoltageDrive() { - return m_drive.get() * RobotController.getBatteryVoltage(); - } + public double getVoltageDrive() { + return m_drive.get() * RobotController.getBatteryVoltage(); + } - public double getVoltageTurn() { - return m_turningSpark.get() * RobotController.getBatteryVoltage(); - } + public double getVoltageTurn() { + return m_turningSpark.get() * RobotController.getBatteryVoltage(); + } - /** Zeroes all the SwerveModule encoders. */ - public void resetEncoders() { - m_drive.setPosition(0); - } + /** Zeroes all the SwerveModule encoders. */ + public void resetEncoders() { + m_drive.setPosition(0); + } } diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index aaa315c..b2160bb 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -58,6 +58,12 @@ public class ManipulatorPivot extends SubsystemBase { canCoder = new CANcoder(ManipulatorPivotConstants.kCANcoderID); canCoder.getConfigurator().apply(ManipulatorPivotConstants.canCoderConfig); + + feedForward = new ArmFeedforward( + ManipulatorPivotConstants.kFeedForwardS, + ManipulatorPivotConstants.kFeedForwardG, + ManipulatorPivotConstants.kFeedForwardV + ); } /** diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/TylerVision.java similarity index 91% rename from src/main/java/frc/robot/subsystems/Vision.java rename to src/main/java/frc/robot/subsystems/TylerVision.java index b541514..271fd11 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/TylerVision.java @@ -4,9 +4,8 @@ import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Vision extends SubsystemBase{ +public class TylerVision{ private DoubleSubscriber cam1GlobalPose; private DoubleSubscriber cam1ClosestTag; @@ -14,7 +13,7 @@ public class Vision extends SubsystemBase{ private DoubleSubscriber framerate; - public Vision(){ + public TylerVision(){ NetworkTableInstance inst = NetworkTableInstance.getDefault(); NetworkTable visionTable = inst.getTable("Fiducial");