Team_2648/2024_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Indentation, whitespace, and linting changes

Browse Source
This commit is contained in:
Bradley Bickford 2024-01-09 22:17:24 -05:00
parent 1e2af3716d
commit 1f76cb39ea
4 changed files with 137 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/main/java/frc/robot/constants/AutoConstants.java
View File

@ -3,19 +3,19 @@ package frc.robot.constants;
import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile;
public final class AutoConstants { public final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6 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 kMaxAccelerationMetersPerSecondSquared = 3; // max capable = 7.4
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 1.05; public static final double kPXController = 1.05;
public static final double kPYController = 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 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 kPHeadingController = 0.02; // for heading control NOT PATHING
public static final double kDHeadingController = 0.0025; public static final double kDHeadingController = 0.0025;
// Constraint for the motion profiled robot angle controller // Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
} }

72
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -5,47 +5,47 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.util.Units; import edu.wpi.first.math.util.Units;
public final class DrivetrainConstants { public final class DrivetrainConstants {
// Driving Parameters - Note that these are not the maximum capable speeds of // Driving Parameters - Note that these are not the maximum capable speeds of
// the robot, rather the allowed maximum speeds // the robot, rather the allowed maximum speeds
public static final double kMaxSpeedMetersPerSecond = 4.6; public static final double kMaxSpeedMetersPerSecond = 4.6;
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second 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 kDirectionSlewRate = 2.4; // radians per second
public static final double kMagnitudeSlewRate = 3.2; // percent per second (1 = 100%) 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 kRotationalSlewRate = 4.0; // percent per second (1 = 100%)
// Chassis configuration // Chassis configuration
public static final double kTrackWidth = Units.inchesToMeters(26.5-1.75*2); public static final double kTrackWidth = Units.inchesToMeters(26.5-1.75*2);
// Distance between centers of right and left wheels on robot // Distance between centers of right and left wheels on robot
public static final double kWheelBase = Units.inchesToMeters(32.3-1.75*2); public static final double kWheelBase = Units.inchesToMeters(32.3-1.75*2);
// Distance between front and back wheels on robot // Distance between front and back wheels on robot
public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( 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),
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 // Angular offsets of the modules relative to the chassis in radians
public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
public static final double kFrontRightChassisAngularOffset = 0; public static final double kFrontRightChassisAngularOffset = 0;
public static final double kBackLeftChassisAngularOffset = Math.PI; public static final double kBackLeftChassisAngularOffset = Math.PI;
public static final double kBackRightChassisAngularOffset = Math.PI / 2; public static final double kBackRightChassisAngularOffset = Math.PI / 2;
// SPARK MAX CAN IDs // SPARK MAX CAN IDs
public static final int kFrontLeftDrivingCanId = 1; public static final int kFrontLeftDrivingCanId = 1;
public static final int kRearLeftDrivingCanId = 3; public static final int kRearLeftDrivingCanId = 3;
public static final int kFrontRightDrivingCanId = 4; public static final int kFrontRightDrivingCanId = 4;
public static final int kRearRightDrivingCanId = 2; public static final int kRearRightDrivingCanId = 2;
public static final int kFrontLeftTurningCanId = 5; public static final int kFrontLeftTurningCanId = 5;
public static final int kRearLeftTurningCanId = 7; public static final int kRearLeftTurningCanId = 7;
public static final int kFrontRightTurningCanId = 8; public static final int kFrontRightTurningCanId = 8;
public static final int kRearRightTurningCanId = 6; public static final int kRearRightTurningCanId = 6;
public static final double kTurnToleranceDeg = 0; public static final double kTurnToleranceDeg = 0;
public static final double kTurnRateToleranceDegPerS = 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;
} }

76
src/main/java/frc/robot/constants/ModuleConstants.java
View File

@ -4,51 +4,51 @@ import com.revrobotics.CANSparkBase.IdleMode;
public class ModuleConstants { public class ModuleConstants {
// The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. // 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 // This changes the drive speed of the module (a pinion gear with more teeth will result in a
// robot that drives faster). // robot that drives faster).
public static final int kDrivingMotorPinionTeeth = 14; public static final int kDrivingMotorPinionTeeth = 14;
// Invert the turning encoder, since the output shaft rotates in the opposite direction of // Invert the turning encoder, since the output shaft rotates in the opposite direction of
// the steering motor in the MAXSwerve Module. // the steering motor in the MAXSwerve Module.
public static final boolean kTurningEncoderInverted = true; public static final boolean kTurningEncoderInverted = true;
// Calculations required for driving motor conversion factors and feed forward // Calculations required for driving motor conversion factors and feed forward
public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60;
public static final double kWheelDiameterMeters = 0.0762; public static final double kWheelDiameterMeters = 0.0762;
public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; 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 // 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 kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
/ kDrivingMotorReduction; / kDrivingMotorReduction;
public static final double kDrivingEncoderPositionFactor = (kWheelDiameterMeters * Math.PI) public static final double kDrivingEncoderPositionFactor = (kWheelDiameterMeters * Math.PI)
/ kDrivingMotorReduction; // meters / kDrivingMotorReduction; // meters
public static final double kDrivingEncoderVelocityFactor = ((kWheelDiameterMeters * Math.PI) public static final double kDrivingEncoderVelocityFactor = ((kWheelDiameterMeters * Math.PI)
/ kDrivingMotorReduction) / 60.0; // meters per second / kDrivingMotorReduction) / 60.0; // meters per second
public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians 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 kTurningEncoderVelocityFactor = (2 * Math.PI) / 60.0; // radians per second
public static final double kTurningEncoderPositionPIDMinInput = 0; // radians public static final double kTurningEncoderPositionPIDMinInput = 0; // radians
public static final double kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor; // radians public static final double kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor; // radians
public static final double kDrivingP = 0.04; public static final double kDrivingP = 0.04;
public static final double kDrivingI = 0; public static final double kDrivingI = 0;
public static final double kDrivingD = 0; public static final double kDrivingD = 0;
public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps; public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps;
public static final double kDrivingMinOutput = -1; public static final double kDrivingMinOutput = -1;
public static final double kDrivingMaxOutput = 1; public static final double kDrivingMaxOutput = 1;
public static final double kTurningP = 1; public static final double kTurningP = 1;
public static final double kTurningI = 0; public static final double kTurningI = 0;
public static final double kTurningD = 0; public static final double kTurningD = 0;
public static final double kTurningFF = 0; public static final double kTurningFF = 0;
public static final double kTurningMinOutput = -1; public static final double kTurningMinOutput = -1;
public static final double kTurningMaxOutput = 1; public static final double kTurningMaxOutput = 1;
public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake;
public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake;
public static final int kDrivingMotorCurrentLimit = 60; // amps public static final int kDrivingMotorCurrentLimit = 60; // amps
public static final int kTurningMotorCurrentLimit = 30; // amps public static final int kTurningMotorCurrentLimit = 30; // amps
} }

98
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -29,22 +29,26 @@ public class Drivetrain extends SubsystemBase {
private final MAXSwerveModule m_frontLeft = new MAXSwerveModule( private final MAXSwerveModule m_frontLeft = new MAXSwerveModule(
DrivetrainConstants.kFrontLeftDrivingCanId, DrivetrainConstants.kFrontLeftDrivingCanId,
DrivetrainConstants.kFrontLeftTurningCanId, DrivetrainConstants.kFrontLeftTurningCanId,
DrivetrainConstants.kFrontLeftChassisAngularOffset); DrivetrainConstants.kFrontLeftChassisAngularOffset
);
private final MAXSwerveModule m_frontRight = new MAXSwerveModule( private final MAXSwerveModule m_frontRight = new MAXSwerveModule(
DrivetrainConstants.kFrontRightDrivingCanId, DrivetrainConstants.kFrontRightDrivingCanId,
DrivetrainConstants.kFrontRightTurningCanId, DrivetrainConstants.kFrontRightTurningCanId,
DrivetrainConstants.kFrontRightChassisAngularOffset); DrivetrainConstants.kFrontRightChassisAngularOffset
);
private final MAXSwerveModule m_rearLeft = new MAXSwerveModule( private final MAXSwerveModule m_rearLeft = new MAXSwerveModule(
DrivetrainConstants.kRearLeftDrivingCanId, DrivetrainConstants.kRearLeftDrivingCanId,
DrivetrainConstants.kRearLeftTurningCanId, DrivetrainConstants.kRearLeftTurningCanId,
DrivetrainConstants.kBackLeftChassisAngularOffset); DrivetrainConstants.kBackLeftChassisAngularOffset
);
private final MAXSwerveModule m_rearRight = new MAXSwerveModule( private final MAXSwerveModule m_rearRight = new MAXSwerveModule(
DrivetrainConstants.kRearRightDrivingCanId, DrivetrainConstants.kRearRightDrivingCanId,
DrivetrainConstants.kRearRightTurningCanId, DrivetrainConstants.kRearRightTurningCanId,
DrivetrainConstants.kBackRightChassisAngularOffset); DrivetrainConstants.kBackRightChassisAngularOffset
);
// The gyro sensor // The gyro sensor
private final AHRS m_gyro = new AHRS(SPI.Port.kMXP); private final AHRS m_gyro = new AHRS(SPI.Port.kMXP);
@ -67,7 +71,8 @@ public class Drivetrain extends SubsystemBase {
m_frontRight.getPosition(), m_frontRight.getPosition(),
m_rearLeft.getPosition(), m_rearLeft.getPosition(),
m_rearRight.getPosition() m_rearRight.getPosition()
}); }
);
/** Creates a new DriveSubsystem. */ /** Creates a new DriveSubsystem. */
public Drivetrain() { public Drivetrain() {
@ -83,7 +88,8 @@ public class Drivetrain extends SubsystemBase {
m_frontRight.getPosition(), m_frontRight.getPosition(),
m_rearLeft.getPosition(), m_rearLeft.getPosition(),
m_rearRight.getPosition() m_rearRight.getPosition()
}); }
);
} }
/** /**
@ -117,7 +123,8 @@ public class Drivetrain extends SubsystemBase {
m_rearLeft.getPosition(), m_rearLeft.getPosition(),
m_rearRight.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. * @param rateLimit Whether to enable rate limiting for smoother control.
*/ */
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) { public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) {
double xSpeedCommanded; double xSpeedCommanded;
double ySpeedCommanded; double ySpeedCommanded;
if (rateLimit) { if (rateLimit) {
// Convert XY to polar for rate limiting // Convert XY to polar for rate limiting
double inputTranslationDir = Math.atan2(ySpeed, xSpeed); double inputTranslationDir = Math.atan2(ySpeed, xSpeed);
double inputTranslationMag = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); 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 // Calculate the direction slew rate based on an estimate of the lateral acceleration
double directionSlewRate; double directionSlewRate;
if (m_currentTranslationMag != 0.0) { if (m_currentTranslationMag != 0.0) {
directionSlewRate = Math.abs(DrivetrainConstants.kDirectionSlewRate / m_currentTranslationMag); directionSlewRate = Math.abs(DrivetrainConstants.kDirectionSlewRate / m_currentTranslationMag);
} else { } else {
directionSlewRate = 500.0; //some high number that means the slew rate is effectively instantaneous 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);
} }
else {
m_currentTranslationDir = SwerveUtils.WrapAngle(m_currentTranslationDir + Math.PI); double currentTime = WPIUtilJNI.now() * 1e-6;
m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); 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 { } else {
xSpeedCommanded = xSpeed; xSpeedCommanded = xSpeed;
ySpeedCommanded = ySpeed; ySpeedCommanded = ySpeed;
m_currentRotation = rot; m_currentRotation = rot;
} }
// Convert the commanded speeds into the correct units for the drivetrain // Convert the commanded speeds into the correct units for the drivetrain