Adding preliminary basic drivetrain stuff
This commit is contained in:
parent
ed41153869
commit
cd7ee67787
@ -4,15 +4,52 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.subsystems.Drivetrain;
|
||||
|
||||
public class RobotContainer {
|
||||
private Drivetrain drivetrain;
|
||||
|
||||
private CommandXboxController primary;
|
||||
private CommandXboxController secondary;
|
||||
|
||||
public RobotContainer() {
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
|
||||
secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
|
||||
|
||||
configureBindings();
|
||||
}
|
||||
|
||||
private void configureBindings() {}
|
||||
private void configureBindings() {
|
||||
drivetrain.setDefaultCommand(drivetrain.teleopCommand(
|
||||
primary::getLeftX,
|
||||
primary::getLeftY,
|
||||
primary::getRightX,
|
||||
OIConstants.kTeleopDriveDeadband
|
||||
));
|
||||
|
||||
primary.povCenter().onFalse(
|
||||
drivetrain.driveCardinal(
|
||||
primary::getLeftX,
|
||||
primary::getLeftY,
|
||||
primary.getHID()::getPOV,
|
||||
OIConstants.kTeleopDriveDeadband).until(
|
||||
() -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
||||
)
|
||||
);
|
||||
|
||||
// This was originally a run while held, not sure that's really necessary
|
||||
primary.y().onTrue(drivetrain.zeroHeadingCommand());
|
||||
|
||||
// This was originally a run while held, not sure that's really necessary
|
||||
primary.rightBumper().onTrue(drivetrain.setXCommand());
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
return Commands.print("No autonomous command configured");
|
||||
|
||||
21
src/main/java/frc/robot/constants/AutoConstants.java
Normal file
21
src/main/java/frc/robot/constants/AutoConstants.java
Normal file
@ -0,0 +1,21 @@
|
||||
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 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;
|
||||
|
||||
// Constraint for the motion profiled robot angle controller
|
||||
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
|
||||
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
|
||||
}
|
||||
51
src/main/java/frc/robot/constants/DrivetrainConstants.java
Normal file
51
src/main/java/frc/robot/constants/DrivetrainConstants.java
Normal file
@ -0,0 +1,51 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
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
|
||||
|
||||
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));
|
||||
|
||||
// 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;
|
||||
|
||||
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 boolean kGyroReversed = true;
|
||||
|
||||
public static final double kRobotStartOffset = 180;
|
||||
}
|
||||
54
src/main/java/frc/robot/constants/ModuleConstants.java
Normal file
54
src/main/java/frc/robot/constants/ModuleConstants.java
Normal file
@ -0,0 +1,54 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
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;
|
||||
|
||||
// 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;
|
||||
|
||||
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 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 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 int kDrivingMotorCurrentLimit = 60; // amps
|
||||
public static final int kTurningMotorCurrentLimit = 30; // amps
|
||||
}
|
||||
5
src/main/java/frc/robot/constants/NeoMotorConstants.java
Normal file
5
src/main/java/frc/robot/constants/NeoMotorConstants.java
Normal file
@ -0,0 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class NeoMotorConstants {
|
||||
public static final double kFreeSpeedRpm = 5676;
|
||||
}
|
||||
7
src/main/java/frc/robot/constants/OIConstants.java
Normal file
7
src/main/java/frc/robot/constants/OIConstants.java
Normal file
@ -0,0 +1,7 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class OIConstants {
|
||||
public static final int kPrimaryXboxUSB = 0;
|
||||
public static final int kSecondaryXboxUSB = 1;
|
||||
public static final double kTeleopDriveDeadband = 0.05;
|
||||
}
|
||||
310
src/main/java/frc/robot/subsystems/Drivetrain.java
Normal file
310
src/main/java/frc/robot/subsystems/Drivetrain.java
Normal file
@ -0,0 +1,310 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.filter.SlewRateLimiter;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.util.WPIUtilJNI;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
import edu.wpi.first.wpilibj.SPI;
|
||||
import frc.robot.constants.AutoConstants;
|
||||
import frc.robot.constants.DrivetrainConstants;
|
||||
import frc.robot.utilities.MAXSwerveModule;
|
||||
import frc.robot.utilities.SwerveUtils;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.PIDCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class Drivetrain extends SubsystemBase {
|
||||
// Create MAXSwerveModules
|
||||
private final MAXSwerveModule m_frontLeft = new MAXSwerveModule(
|
||||
DrivetrainConstants.kFrontLeftDrivingCanId,
|
||||
DrivetrainConstants.kFrontLeftTurningCanId,
|
||||
DrivetrainConstants.kFrontLeftChassisAngularOffset);
|
||||
|
||||
private final MAXSwerveModule m_frontRight = new MAXSwerveModule(
|
||||
DrivetrainConstants.kFrontRightDrivingCanId,
|
||||
DrivetrainConstants.kFrontRightTurningCanId,
|
||||
DrivetrainConstants.kFrontRightChassisAngularOffset);
|
||||
|
||||
private final MAXSwerveModule m_rearLeft = new MAXSwerveModule(
|
||||
DrivetrainConstants.kRearLeftDrivingCanId,
|
||||
DrivetrainConstants.kRearLeftTurningCanId,
|
||||
DrivetrainConstants.kBackLeftChassisAngularOffset);
|
||||
|
||||
private final MAXSwerveModule m_rearRight = new MAXSwerveModule(
|
||||
DrivetrainConstants.kRearRightDrivingCanId,
|
||||
DrivetrainConstants.kRearRightTurningCanId,
|
||||
DrivetrainConstants.kBackRightChassisAngularOffset);
|
||||
|
||||
// The gyro sensor
|
||||
private final AHRS m_gyro = new AHRS(SPI.Port.kMXP);
|
||||
|
||||
// Slew rate filter variables for controlling lateral acceleration
|
||||
private double m_currentRotation = 0.0;
|
||||
private double m_currentTranslationDir = 0.0;
|
||||
private double m_currentTranslationMag = 0.0;
|
||||
|
||||
private SlewRateLimiter m_magLimiter = new SlewRateLimiter(DrivetrainConstants.kMagnitudeSlewRate);
|
||||
private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(DrivetrainConstants.kRotationalSlewRate);
|
||||
private double m_prevTime = WPIUtilJNI.now() * 1e-6;
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
|
||||
DrivetrainConstants.kDriveKinematics,
|
||||
Rotation2d.fromDegrees(getGyroAngle()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public Drivetrain() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// Update the odometry in the periodic block
|
||||
m_odometry.update(
|
||||
Rotation2d.fromDegrees(getGyroAngle()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
*
|
||||
* @return The pose.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPoseMeters();
|
||||
}
|
||||
|
||||
public double getPoseX(){
|
||||
return m_odometry.getPoseMeters().getX();
|
||||
}
|
||||
|
||||
public double getPoseY(){
|
||||
return m_odometry.getPoseMeters().getY();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_odometry.resetPosition(
|
||||
Rotation2d.fromDegrees(getGyroAngle()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
},
|
||||
pose);
|
||||
}
|
||||
|
||||
/**
|
||||
* Method to drive the robot using joystick info.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction (forward).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the
|
||||
* field.
|
||||
* @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));
|
||||
|
||||
// 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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
// Convert the commanded speeds into the correct units for the drivetrain
|
||||
double xSpeedDelivered = xSpeedCommanded * DrivetrainConstants.kMaxSpeedMetersPerSecond;
|
||||
double ySpeedDelivered = ySpeedCommanded * DrivetrainConstants.kMaxSpeedMetersPerSecond;
|
||||
double rotDelivered = m_currentRotation * DrivetrainConstants.kMaxAngularSpeed;
|
||||
|
||||
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle()))
|
||||
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
|
||||
m_frontLeft.setDesiredState(swerveModuleStates[0]);
|
||||
m_frontRight.setDesiredState(swerveModuleStates[1]);
|
||||
m_rearLeft.setDesiredState(swerveModuleStates[2]);
|
||||
m_rearRight.setDesiredState(swerveModuleStates[3]);
|
||||
}
|
||||
|
||||
public Command teleopCommand(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, double deadband) {
|
||||
return run(() -> {
|
||||
drive(
|
||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
|
||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
|
||||
-MathUtil.applyDeadband(rotation.getAsDouble(), deadband),
|
||||
true,
|
||||
true
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
public Command driveCardinal(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier pov, double deadband) {
|
||||
PIDController controller = new PIDController(
|
||||
AutoConstants.kPHeadingController,
|
||||
0,
|
||||
AutoConstants.kDHeadingController
|
||||
);
|
||||
controller.enableContinuousInput(-180, 180);
|
||||
|
||||
return new PIDCommand(
|
||||
controller,
|
||||
this::getHeading,
|
||||
pov::getAsDouble,
|
||||
(output) -> {
|
||||
this.drive(
|
||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
|
||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
|
||||
output,
|
||||
true,
|
||||
true
|
||||
);
|
||||
},
|
||||
this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the wheels into an X formation to prevent movement.
|
||||
*/
|
||||
public void setX() {
|
||||
m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
|
||||
m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
|
||||
m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
|
||||
m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
|
||||
}
|
||||
|
||||
public Command setXCommand() {
|
||||
return runOnce(this::setX);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the swerve ModuleStates.
|
||||
*
|
||||
* @param desiredStates The desired SwerveModule states.
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
desiredStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
|
||||
m_frontLeft.setDesiredState(desiredStates[0]);
|
||||
m_frontRight.setDesiredState(desiredStates[1]);
|
||||
m_rearLeft.setDesiredState(desiredStates[2]);
|
||||
m_rearRight.setDesiredState(desiredStates[3]);
|
||||
}
|
||||
|
||||
/** Resets the drive encoders to currently read a position of 0. */
|
||||
public void resetEncoders() {
|
||||
m_frontLeft.resetEncoders();
|
||||
m_rearLeft.resetEncoders();
|
||||
m_frontRight.resetEncoders();
|
||||
m_rearRight.resetEncoders();
|
||||
}
|
||||
|
||||
/** Zeroes the heading of the robot. */
|
||||
public void zeroHeading() {
|
||||
m_gyro.reset();
|
||||
}
|
||||
|
||||
public Command zeroHeadingCommand() {
|
||||
return runOnce(this::zeroHeading);
|
||||
}
|
||||
|
||||
public void offsetZero(double angle){
|
||||
m_gyro.setAngleAdjustment(angle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return Rotation2d.fromDegrees(getGyroAngle()).getDegrees();
|
||||
}
|
||||
|
||||
public double getGyroAngle(){
|
||||
return m_gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0) + DrivetrainConstants.kRobotStartOffset;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
public double getTurnRate() {
|
||||
return m_gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
}
|
||||
160
src/main/java/frc/robot/utilities/MAXSwerveModule.java
Normal file
160
src/main/java/frc/robot/utilities/MAXSwerveModule.java
Normal file
@ -0,0 +1,160 @@
|
||||
package frc.robot.utilities;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkLowLevel.MotorType;
|
||||
import com.revrobotics.SparkAbsoluteEncoder.Type;
|
||||
import com.revrobotics.SparkPIDController;
|
||||
import com.revrobotics.AbsoluteEncoder;
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
|
||||
import frc.robot.constants.ModuleConstants;
|
||||
|
||||
public class MAXSwerveModule {
|
||||
private final CANSparkMax m_drivingSparkMax;
|
||||
private final CANSparkMax m_turningSparkMax;
|
||||
|
||||
private final RelativeEncoder m_drivingEncoder;
|
||||
private final AbsoluteEncoder m_turningEncoder;
|
||||
|
||||
private final SparkPIDController m_drivingPIDController;
|
||||
private final SparkPIDController m_turningPIDController;
|
||||
|
||||
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_drivingSparkMax = new CANSparkMax(drivingCANId, MotorType.kBrushless);
|
||||
m_turningSparkMax = new CANSparkMax(turningCANId, MotorType.kBrushless);
|
||||
|
||||
// Factory reset, so we get the SPARKS MAX to a known state before configuring
|
||||
// them. This is useful in case a SPARK MAX is swapped out.
|
||||
m_drivingSparkMax.restoreFactoryDefaults();
|
||||
m_turningSparkMax.restoreFactoryDefaults();
|
||||
|
||||
// Setup encoders and PID controllers for the driving and turning SPARKS MAX.
|
||||
m_drivingEncoder = m_drivingSparkMax.getEncoder();
|
||||
m_turningEncoder = m_turningSparkMax.getAbsoluteEncoder(Type.kDutyCycle);
|
||||
m_drivingPIDController = m_drivingSparkMax.getPIDController();
|
||||
m_turningPIDController = m_turningSparkMax.getPIDController();
|
||||
m_drivingPIDController.setFeedbackDevice(m_drivingEncoder);
|
||||
m_turningPIDController.setFeedbackDevice(m_turningEncoder);
|
||||
|
||||
// Apply position and velocity conversion factors for the driving encoder. The
|
||||
// native units for position and velocity are rotations and RPM, respectively,
|
||||
// but we want meters and meters per second to use with WPILib's swerve APIs.
|
||||
m_drivingEncoder.setPositionConversionFactor(ModuleConstants.kDrivingEncoderPositionFactor);
|
||||
m_drivingEncoder.setVelocityConversionFactor(ModuleConstants.kDrivingEncoderVelocityFactor);
|
||||
|
||||
// Apply position and velocity conversion factors for the turning encoder. We
|
||||
// want these in radians and radians per second to use with WPILib's swerve
|
||||
// APIs.
|
||||
m_turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderPositionFactor);
|
||||
m_turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderVelocityFactor);
|
||||
|
||||
// Invert the turning encoder, since the output shaft rotates in the opposite direction of
|
||||
// the steering motor in the MAXSwerve Module.
|
||||
m_turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted);
|
||||
|
||||
// Enable PID wrap around for the turning motor. This will allow the PID
|
||||
// controller to go through 0 to get to the setpoint i.e. going from 350 degrees
|
||||
// to 10 degrees will go through 0 rather than the other direction which is a
|
||||
// longer route.
|
||||
m_turningPIDController.setPositionPIDWrappingEnabled(true);
|
||||
m_turningPIDController.setPositionPIDWrappingMinInput(ModuleConstants.kTurningEncoderPositionPIDMinInput);
|
||||
m_turningPIDController.setPositionPIDWrappingMaxInput(ModuleConstants.kTurningEncoderPositionPIDMaxInput);
|
||||
|
||||
// Set the PID gains for the driving motor. Note these are example gains, and you
|
||||
// may need to tune them for your own robot!
|
||||
m_drivingPIDController.setP(ModuleConstants.kDrivingP);
|
||||
m_drivingPIDController.setI(ModuleConstants.kDrivingI);
|
||||
m_drivingPIDController.setD(ModuleConstants.kDrivingD);
|
||||
m_drivingPIDController.setFF(ModuleConstants.kDrivingFF);
|
||||
m_drivingPIDController.setOutputRange(ModuleConstants.kDrivingMinOutput,
|
||||
ModuleConstants.kDrivingMaxOutput);
|
||||
|
||||
// Set the PID gains for the turning motor. Note these are example gains, and you
|
||||
// may need to tune them for your own robot!
|
||||
m_turningPIDController.setP(ModuleConstants.kTurningP);
|
||||
m_turningPIDController.setI(ModuleConstants.kTurningI);
|
||||
m_turningPIDController.setD(ModuleConstants.kTurningD);
|
||||
m_turningPIDController.setFF(ModuleConstants.kTurningFF);
|
||||
m_turningPIDController.setOutputRange(ModuleConstants.kTurningMinOutput,
|
||||
ModuleConstants.kTurningMaxOutput);
|
||||
|
||||
m_drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode);
|
||||
m_turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode);
|
||||
m_drivingSparkMax.setSmartCurrentLimit(ModuleConstants.kDrivingMotorCurrentLimit);
|
||||
m_turningSparkMax.setSmartCurrentLimit(ModuleConstants.kTurningMotorCurrentLimit);
|
||||
|
||||
// Save the SPARK MAX configurations. If a SPARK MAX browns out during
|
||||
// operation, it will maintain the above configurations.
|
||||
m_drivingSparkMax.burnFlash();
|
||||
m_turningSparkMax.burnFlash();
|
||||
|
||||
m_chassisAngularOffset = chassisAngularOffset;
|
||||
m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
|
||||
m_drivingEncoder.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_drivingEncoder.getVelocity(),
|
||||
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_drivingEncoder.getPosition(),
|
||||
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));
|
||||
|
||||
// Optimize the reference state to avoid spinning further than 90 degrees.
|
||||
SwerveModuleState optimizedDesiredState = SwerveModuleState.optimize(correctedDesiredState,
|
||||
new Rotation2d(m_turningEncoder.getPosition()));
|
||||
|
||||
// Command driving and turning SPARKS MAX towards their respective setpoints.
|
||||
m_drivingPIDController.setReference(optimizedDesiredState.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity);
|
||||
m_turningPIDController.setReference(optimizedDesiredState.angle.getRadians(), CANSparkMax.ControlType.kPosition);
|
||||
|
||||
m_desiredState = desiredState;
|
||||
}
|
||||
|
||||
/** Zeroes all the SwerveModule encoders. */
|
||||
public void resetEncoders() {
|
||||
m_drivingEncoder.setPosition(0);
|
||||
}
|
||||
}
|
||||
89
src/main/java/frc/robot/utilities/SwerveUtils.java
Normal file
89
src/main/java/frc/robot/utilities/SwerveUtils.java
Normal file
@ -0,0 +1,89 @@
|
||||
package frc.robot.utilities;
|
||||
|
||||
public class SwerveUtils {
|
||||
|
||||
/**
|
||||
* Steps a value towards a target with a specified step size.
|
||||
* @param _current The current or starting value. Can be positive or negative.
|
||||
* @param _target The target value the algorithm will step towards. Can be positive or negative.
|
||||
* @param _stepsize The maximum step size that can be taken.
|
||||
* @return The new value for {@code _current} after performing the specified step towards the specified target.
|
||||
*/
|
||||
public static double StepTowards(double _current, double _target, double _stepsize) {
|
||||
if (Math.abs(_current - _target) <= _stepsize) {
|
||||
return _target;
|
||||
}
|
||||
else if (_target < _current) {
|
||||
return _current - _stepsize;
|
||||
}
|
||||
else {
|
||||
return _current + _stepsize;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Steps a value (angle) towards a target (angle) taking the shortest path with a specified step size.
|
||||
* @param _current The current or starting angle (in radians). Can lie outside the 0 to 2*PI range.
|
||||
* @param _target The target angle (in radians) the algorithm will step towards. Can lie outside the 0 to 2*PI range.
|
||||
* @param _stepsize The maximum step size that can be taken (in radians).
|
||||
* @return The new angle (in radians) for {@code _current} after performing the specified step towards the specified target.
|
||||
* This value will always lie in the range 0 to 2*PI (exclusive).
|
||||
*/
|
||||
public static double StepTowardsCircular(double _current, double _target, double _stepsize) {
|
||||
_current = WrapAngle(_current);
|
||||
_target = WrapAngle(_target);
|
||||
|
||||
double stepDirection = Math.signum(_target - _current);
|
||||
double difference = Math.abs(_current - _target);
|
||||
|
||||
if (difference <= _stepsize) {
|
||||
return _target;
|
||||
}
|
||||
else if (difference > Math.PI) { //does the system need to wrap over eventually?
|
||||
//handle the special case where you can reach the target in one step while also wrapping
|
||||
if (_current + 2*Math.PI - _target < _stepsize || _target + 2*Math.PI - _current < _stepsize) {
|
||||
return _target;
|
||||
}
|
||||
else {
|
||||
return WrapAngle(_current - stepDirection * _stepsize); //this will handle wrapping gracefully
|
||||
}
|
||||
|
||||
}
|
||||
else {
|
||||
return _current + stepDirection * _stepsize;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Finds the (unsigned) minimum difference between two angles including calculating across 0.
|
||||
* @param _angleA An angle (in radians).
|
||||
* @param _angleB An angle (in radians).
|
||||
* @return The (unsigned) minimum difference between the two angles (in radians).
|
||||
*/
|
||||
public static double AngleDifference(double _angleA, double _angleB) {
|
||||
double difference = Math.abs(_angleA - _angleB);
|
||||
return difference > Math.PI? (2 * Math.PI) - difference : difference;
|
||||
}
|
||||
|
||||
/**
|
||||
* Wraps an angle until it lies within the range from 0 to 2*PI (exclusive).
|
||||
* @param _angle The angle (in radians) to wrap. Can be positive or negative and can lie multiple wraps outside the output range.
|
||||
* @return An angle (in radians) from 0 and 2*PI (exclusive).
|
||||
*/
|
||||
public static double WrapAngle(double _angle) {
|
||||
double twoPi = 2*Math.PI;
|
||||
|
||||
if (_angle == twoPi) { // Handle this case separately to avoid floating point errors with the floor after the division in the case below
|
||||
return 0.0;
|
||||
}
|
||||
else if (_angle > twoPi) {
|
||||
return _angle - twoPi*Math.floor(_angle / twoPi);
|
||||
}
|
||||
else if (_angle < 0.0) {
|
||||
return _angle + twoPi*(Math.floor((-_angle) / twoPi)+1);
|
||||
}
|
||||
else {
|
||||
return _angle;
|
||||
}
|
||||
}
|
||||
}
|
||||
40
vendordeps/NavX.json
Normal file
40
vendordeps/NavX.json
Normal file
@ -0,0 +1,40 @@
|
||||
{
|
||||
"fileName": "NavX.json",
|
||||
"name": "NavX",
|
||||
"version": "2024.1.0",
|
||||
"uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
|
||||
"frcYear": "2024",
|
||||
"mavenUrls": [
|
||||
"https://dev.studica.com/maven/release/2024/"
|
||||
],
|
||||
"jsonUrl": "https://dev.studica.com/releases/2024/NavX.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.kauailabs.navx.frc",
|
||||
"artifactId": "navx-frc-java",
|
||||
"version": "2024.1.0"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.kauailabs.navx.frc",
|
||||
"artifactId": "navx-frc-cpp",
|
||||
"version": "2024.1.0",
|
||||
"headerClassifier": "headers",
|
||||
"sourcesClassifier": "sources",
|
||||
"sharedLibrary": false,
|
||||
"libName": "navx_frc",
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"linuxathena",
|
||||
"linuxraspbian",
|
||||
"linuxarm32",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal",
|
||||
"windowsx86-64"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
74
vendordeps/REVLib.json
Normal file
74
vendordeps/REVLib.json
Normal file
@ -0,0 +1,74 @@
|
||||
{
|
||||
"fileName": "REVLib.json",
|
||||
"name": "REVLib",
|
||||
"version": "2024.2.0",
|
||||
"frcYear": "2024",
|
||||
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
|
||||
"mavenUrls": [
|
||||
"https://maven.revrobotics.com/"
|
||||
],
|
||||
"jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-java",
|
||||
"version": "2024.2.0"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-driver",
|
||||
"version": "2024.2.0",
|
||||
"skipInvalidPlatforms": true,
|
||||
"isJar": false,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-cpp",
|
||||
"version": "2024.2.0",
|
||||
"libName": "REVLib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"osxuniversal"
|
||||
]
|
||||
},
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-driver",
|
||||
"version": "2024.2.0",
|
||||
"libName": "REVLibDriver",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user