Indentation, whitespace, and linting changes
This commit is contained in:
parent
1e2af3716d
commit
1f76cb39ea
@ -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,7 +138,6 @@ 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;
|
||||||
|
|
||||||
@ -148,35 +154,31 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
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 currentTime = WPIUtilJNI.now() * 1e-6;
|
||||||
double elapsedTime = currentTime - m_prevTime;
|
double elapsedTime = currentTime - m_prevTime;
|
||||||
double angleDif = SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDir);
|
double angleDif = SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDir);
|
||||||
|
|
||||||
if (angleDif < 0.45*Math.PI) {
|
if (angleDif < 0.45*Math.PI) {
|
||||||
m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime);
|
m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime);
|
||||||
m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag);
|
m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag);
|
||||||
}
|
} else if (angleDif > 0.85*Math.PI) {
|
||||||
else if (angleDif > 0.85*Math.PI) {
|
|
||||||
if (m_currentTranslationMag > 1e-4) { //some small number to avoid floating-point errors with equality checking
|
if (m_currentTranslationMag > 1e-4) { //some small number to avoid floating-point errors with equality checking
|
||||||
// keep currentTranslationDir unchanged
|
// keep currentTranslationDir unchanged
|
||||||
m_currentTranslationMag = m_magLimiter.calculate(0.0);
|
m_currentTranslationMag = m_magLimiter.calculate(0.0);
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
m_currentTranslationDir = SwerveUtils.WrapAngle(m_currentTranslationDir + Math.PI);
|
m_currentTranslationDir = SwerveUtils.WrapAngle(m_currentTranslationDir + Math.PI);
|
||||||
m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag);
|
m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag);
|
||||||
}
|
}
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime);
|
m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime);
|
||||||
m_currentTranslationMag = m_magLimiter.calculate(0.0);
|
m_currentTranslationMag = m_magLimiter.calculate(0.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
m_prevTime = currentTime;
|
m_prevTime = currentTime;
|
||||||
|
|
||||||
xSpeedCommanded = m_currentTranslationMag * Math.cos(m_currentTranslationDir);
|
xSpeedCommanded = m_currentTranslationMag * Math.cos(m_currentTranslationDir);
|
||||||
ySpeedCommanded = m_currentTranslationMag * Math.sin(m_currentTranslationDir);
|
ySpeedCommanded = m_currentTranslationMag * Math.sin(m_currentTranslationDir);
|
||||||
m_currentRotation = m_rotLimiter.calculate(rot);
|
m_currentRotation = m_rotLimiter.calculate(rot);
|
||||||
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
xSpeedCommanded = xSpeed;
|
xSpeedCommanded = xSpeed;
|
||||||
ySpeedCommanded = ySpeed;
|
ySpeedCommanded = ySpeed;
|
||||||
|
Loading…
Reference in New Issue
Block a user