From 1f76cb39ea790f918a5367c8a4c7874b9b7836fc Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Tue, 9 Jan 2024 22:17:24 -0500 Subject: [PATCH] Indentation, whitespace, and linting changes --- .../frc/robot/constants/AutoConstants.java | 26 ++--- .../robot/constants/DrivetrainConstants.java | 72 +++++++------- .../frc/robot/constants/ModuleConstants.java | 76 +++++++------- .../java/frc/robot/subsystems/Drivetrain.java | 98 ++++++++++--------- 4 files changed, 137 insertions(+), 135 deletions(-) diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 646c199..3bfe98c 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -3,19 +3,19 @@ package frc.robot.constants; import edu.wpi.first.math.trajectory.TrapezoidProfile; public final class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6 - public static final double kMaxAccelerationMetersPerSecondSquared = 3; // max capable = 7.4 - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6 + public static final double kMaxAccelerationMetersPerSecondSquared = 3; // max capable = 7.4 + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final double kPXController = 1.05; - public static final double kPYController = 1.05; - public static final double kPThetaController = 0.95; // needs to be separate from heading control + public static final double kPXController = 1.05; + public static final double kPYController = 1.05; + public static final double kPThetaController = 0.95; // needs to be separate from heading control - public static final double kPHeadingController = 0.02; // for heading control NOT PATHING - public static final double kDHeadingController = 0.0025; + public static final double kPHeadingController = 0.02; // for heading control NOT PATHING + public static final double kDHeadingController = 0.0025; - // Constraint for the motion profiled robot angle controller - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); - } \ No newline at end of file + // Constraint for the motion profiled robot angle controller + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 22bf733..d5abcbf 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -5,47 +5,47 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; public final class DrivetrainConstants { - // Driving Parameters - Note that these are not the maximum capable speeds of - // the robot, rather the allowed maximum speeds - public static final double kMaxSpeedMetersPerSecond = 4.6; - public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second + // Driving Parameters - Note that these are not the maximum capable speeds of + // the robot, rather the allowed maximum speeds + public static final double kMaxSpeedMetersPerSecond = 4.6; + public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second - public static final double kDirectionSlewRate = 2.4; // radians per second - public static final double kMagnitudeSlewRate = 3.2; // percent per second (1 = 100%) - public static final double kRotationalSlewRate = 4.0; // percent per second (1 = 100%) + public static final double kDirectionSlewRate = 2.4; // radians per second + public static final double kMagnitudeSlewRate = 3.2; // percent per second (1 = 100%) + public static final double kRotationalSlewRate = 4.0; // percent per second (1 = 100%) - // Chassis configuration - public static final double kTrackWidth = Units.inchesToMeters(26.5-1.75*2); - // Distance between centers of right and left wheels on robot - public static final double kWheelBase = Units.inchesToMeters(32.3-1.75*2); - // Distance between front and back wheels on robot - public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + // Chassis configuration + public static final double kTrackWidth = Units.inchesToMeters(26.5-1.75*2); + // Distance between centers of right and left wheels on robot + public static final double kWheelBase = Units.inchesToMeters(32.3-1.75*2); + // Distance between front and back wheels on robot + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); - // Angular offsets of the modules relative to the chassis in radians - public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; - public static final double kFrontRightChassisAngularOffset = 0; - public static final double kBackLeftChassisAngularOffset = Math.PI; - public static final double kBackRightChassisAngularOffset = Math.PI / 2; + // Angular offsets of the modules relative to the chassis in radians + public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; + public static final double kFrontRightChassisAngularOffset = 0; + public static final double kBackLeftChassisAngularOffset = Math.PI; + public static final double kBackRightChassisAngularOffset = Math.PI / 2; - // SPARK MAX CAN IDs - public static final int kFrontLeftDrivingCanId = 1; - public static final int kRearLeftDrivingCanId = 3; - public static final int kFrontRightDrivingCanId = 4; - public static final int kRearRightDrivingCanId = 2; + // SPARK MAX CAN IDs + public static final int kFrontLeftDrivingCanId = 1; + public static final int kRearLeftDrivingCanId = 3; + public static final int kFrontRightDrivingCanId = 4; + public static final int kRearRightDrivingCanId = 2; - public static final int kFrontLeftTurningCanId = 5; - public static final int kRearLeftTurningCanId = 7; - public static final int kFrontRightTurningCanId = 8; - public static final int kRearRightTurningCanId = 6; + public static final int kFrontLeftTurningCanId = 5; + public static final int kRearLeftTurningCanId = 7; + public static final int kFrontRightTurningCanId = 8; + public static final int kRearRightTurningCanId = 6; - public static final double kTurnToleranceDeg = 0; - public static final double kTurnRateToleranceDegPerS = 0; + public static final double kTurnToleranceDeg = 0; + public static final double kTurnRateToleranceDegPerS = 0; - public static final boolean kGyroReversed = true; + public static final boolean kGyroReversed = true; - public static final double kRobotStartOffset = 180; - } + public static final double kRobotStartOffset = 180; +} diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index f62044a..4d073bb 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -4,51 +4,51 @@ import com.revrobotics.CANSparkBase.IdleMode; public class ModuleConstants { // The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. - // This changes the drive speed of the module (a pinion gear with more teeth will result in a - // robot that drives faster). - public static final int kDrivingMotorPinionTeeth = 14; + // This changes the drive speed of the module (a pinion gear with more teeth will result in a + // robot that drives faster). + public static final int kDrivingMotorPinionTeeth = 14; - // Invert the turning encoder, since the output shaft rotates in the opposite direction of - // the steering motor in the MAXSwerve Module. - public static final boolean kTurningEncoderInverted = true; + // Invert the turning encoder, since the output shaft rotates in the opposite direction of + // the steering motor in the MAXSwerve Module. + public static final boolean kTurningEncoderInverted = true; - // Calculations required for driving motor conversion factors and feed forward - public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; - public static final double kWheelDiameterMeters = 0.0762; - public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; - // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion - public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); - public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) - / kDrivingMotorReduction; + // Calculations required for driving motor conversion factors and feed forward + public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; + public static final double kWheelDiameterMeters = 0.0762; + public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; + // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion + public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) + / kDrivingMotorReduction; - public static final double kDrivingEncoderPositionFactor = (kWheelDiameterMeters * Math.PI) - / kDrivingMotorReduction; // meters - public static final double kDrivingEncoderVelocityFactor = ((kWheelDiameterMeters * Math.PI) - / kDrivingMotorReduction) / 60.0; // meters per second + public static final double kDrivingEncoderPositionFactor = (kWheelDiameterMeters * Math.PI) + / kDrivingMotorReduction; // meters + public static final double kDrivingEncoderVelocityFactor = ((kWheelDiameterMeters * Math.PI) + / kDrivingMotorReduction) / 60.0; // meters per second - public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians - public static final double kTurningEncoderVelocityFactor = (2 * Math.PI) / 60.0; // radians per second + public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians + public static final double kTurningEncoderVelocityFactor = (2 * Math.PI) / 60.0; // radians per second - public static final double kTurningEncoderPositionPIDMinInput = 0; // radians - public static final double kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor; // radians + public static final double kTurningEncoderPositionPIDMinInput = 0; // radians + public static final double kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor; // radians - public static final double kDrivingP = 0.04; - public static final double kDrivingI = 0; - public static final double kDrivingD = 0; - public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps; - public static final double kDrivingMinOutput = -1; - public static final double kDrivingMaxOutput = 1; + public static final double kDrivingP = 0.04; + public static final double kDrivingI = 0; + public static final double kDrivingD = 0; + public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps; + public static final double kDrivingMinOutput = -1; + public static final double kDrivingMaxOutput = 1; - public static final double kTurningP = 1; - public static final double kTurningI = 0; - public static final double kTurningD = 0; - public static final double kTurningFF = 0; - public static final double kTurningMinOutput = -1; - public static final double kTurningMaxOutput = 1; + public static final double kTurningP = 1; + public static final double kTurningI = 0; + public static final double kTurningD = 0; + public static final double kTurningFF = 0; + public static final double kTurningMinOutput = -1; + public static final double kTurningMaxOutput = 1; - public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; - public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; + public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; + public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; - public static final int kDrivingMotorCurrentLimit = 60; // amps - public static final int kTurningMotorCurrentLimit = 30; // amps + public static final int kDrivingMotorCurrentLimit = 60; // amps + public static final int kTurningMotorCurrentLimit = 30; // amps } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 2e29542..9889daa 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -29,22 +29,26 @@ public class Drivetrain extends SubsystemBase { private final MAXSwerveModule m_frontLeft = new MAXSwerveModule( DrivetrainConstants.kFrontLeftDrivingCanId, DrivetrainConstants.kFrontLeftTurningCanId, - DrivetrainConstants.kFrontLeftChassisAngularOffset); + DrivetrainConstants.kFrontLeftChassisAngularOffset + ); private final MAXSwerveModule m_frontRight = new MAXSwerveModule( DrivetrainConstants.kFrontRightDrivingCanId, DrivetrainConstants.kFrontRightTurningCanId, - DrivetrainConstants.kFrontRightChassisAngularOffset); + DrivetrainConstants.kFrontRightChassisAngularOffset + ); private final MAXSwerveModule m_rearLeft = new MAXSwerveModule( DrivetrainConstants.kRearLeftDrivingCanId, DrivetrainConstants.kRearLeftTurningCanId, - DrivetrainConstants.kBackLeftChassisAngularOffset); + DrivetrainConstants.kBackLeftChassisAngularOffset + ); private final MAXSwerveModule m_rearRight = new MAXSwerveModule( DrivetrainConstants.kRearRightDrivingCanId, DrivetrainConstants.kRearRightTurningCanId, - DrivetrainConstants.kBackRightChassisAngularOffset); + DrivetrainConstants.kBackRightChassisAngularOffset + ); // The gyro sensor private final AHRS m_gyro = new AHRS(SPI.Port.kMXP); @@ -67,7 +71,8 @@ public class Drivetrain extends SubsystemBase { m_frontRight.getPosition(), m_rearLeft.getPosition(), m_rearRight.getPosition() - }); + } + ); /** Creates a new DriveSubsystem. */ public Drivetrain() { @@ -83,7 +88,8 @@ public class Drivetrain extends SubsystemBase { m_frontRight.getPosition(), m_rearLeft.getPosition(), m_rearRight.getPosition() - }); + } + ); } /** @@ -117,7 +123,8 @@ public class Drivetrain extends SubsystemBase { m_rearLeft.getPosition(), m_rearRight.getPosition() }, - pose); + pose + ); } /** @@ -131,56 +138,51 @@ public class Drivetrain extends SubsystemBase { * @param rateLimit Whether to enable rate limiting for smoother control. */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) { - double xSpeedCommanded; double ySpeedCommanded; if (rateLimit) { - // Convert XY to polar for rate limiting - double inputTranslationDir = Math.atan2(ySpeed, xSpeed); - double inputTranslationMag = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); + // Convert XY to polar for rate limiting + double inputTranslationDir = Math.atan2(ySpeed, xSpeed); + double inputTranslationMag = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); - // Calculate the direction slew rate based on an estimate of the lateral acceleration - double directionSlewRate; - if (m_currentTranslationMag != 0.0) { - directionSlewRate = Math.abs(DrivetrainConstants.kDirectionSlewRate / m_currentTranslationMag); - } else { - directionSlewRate = 500.0; //some high number that means the slew rate is effectively instantaneous - } - - - double currentTime = WPIUtilJNI.now() * 1e-6; - double elapsedTime = currentTime - m_prevTime; - double angleDif = SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDir); - if (angleDif < 0.45*Math.PI) { - m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); - m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); - } - else if (angleDif > 0.85*Math.PI) { - if (m_currentTranslationMag > 1e-4) { //some small number to avoid floating-point errors with equality checking - // keep currentTranslationDir unchanged - m_currentTranslationMag = m_magLimiter.calculate(0.0); + // Calculate the direction slew rate based on an estimate of the lateral acceleration + double directionSlewRate; + if (m_currentTranslationMag != 0.0) { + directionSlewRate = Math.abs(DrivetrainConstants.kDirectionSlewRate / m_currentTranslationMag); + } else { + directionSlewRate = 500.0; //some high number that means the slew rate is effectively instantaneous } - else { - m_currentTranslationDir = SwerveUtils.WrapAngle(m_currentTranslationDir + Math.PI); - m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); + + double currentTime = WPIUtilJNI.now() * 1e-6; + double elapsedTime = currentTime - m_prevTime; + double angleDif = SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDir); + + if (angleDif < 0.45*Math.PI) { + m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); + m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); + } else if (angleDif > 0.85*Math.PI) { + if (m_currentTranslationMag > 1e-4) { //some small number to avoid floating-point errors with equality checking + // keep currentTranslationDir unchanged + m_currentTranslationMag = m_magLimiter.calculate(0.0); + } else { + m_currentTranslationDir = SwerveUtils.WrapAngle(m_currentTranslationDir + Math.PI); + m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); + } + } else { + m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); + m_currentTranslationMag = m_magLimiter.calculate(0.0); } - } - else { - m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); - m_currentTranslationMag = m_magLimiter.calculate(0.0); - } - m_prevTime = currentTime; - - xSpeedCommanded = m_currentTranslationMag * Math.cos(m_currentTranslationDir); - ySpeedCommanded = m_currentTranslationMag * Math.sin(m_currentTranslationDir); - m_currentRotation = m_rotLimiter.calculate(rot); - + m_prevTime = currentTime; + + xSpeedCommanded = m_currentTranslationMag * Math.cos(m_currentTranslationDir); + ySpeedCommanded = m_currentTranslationMag * Math.sin(m_currentTranslationDir); + m_currentRotation = m_rotLimiter.calculate(rot); } else { - xSpeedCommanded = xSpeed; - ySpeedCommanded = ySpeed; - m_currentRotation = rot; + xSpeedCommanded = xSpeed; + ySpeedCommanded = ySpeed; + m_currentRotation = rot; } // Convert the commanded speeds into the correct units for the drivetrain