Indentation, whitespace, and linting changes
This commit is contained in:
parent
1e2af3716d
commit
1f76cb39ea
@ -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);
|
||||
}
|
||||
// Constraint for the motion profiled robot angle controller
|
||||
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
|
||||
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user