Removing REV template hot hot garbage programming practices

This commit is contained in:
Bradley Bickford 2025-01-07 18:14:14 -05:00
parent 8ac67fcf21
commit a48cff9fb2
11 changed files with 341 additions and 404 deletions

0
gradlew vendored Normal file → Executable file
View File

View File

@ -1,56 +0,0 @@
package frc.robot;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import frc.robot.Constants.ModuleConstants;
public final class Configs {
public static final class MAXSwerveModule {
public static final SparkMaxConfig drivingConfig = new SparkMaxConfig();
public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
static {
// Use module constants to calculate conversion factors and feed forward gain.
double drivingFactor = ModuleConstants.kWheelDiameterMeters * Math.PI
/ ModuleConstants.kDrivingMotorReduction;
double turningFactor = 2 * Math.PI;
double drivingVelocityFeedForward = 1 / ModuleConstants.kDriveWheelFreeSpeedRps;
drivingConfig
.idleMode(IdleMode.kBrake)
.smartCurrentLimit(50);
drivingConfig.encoder
.positionConversionFactor(drivingFactor) // meters
.velocityConversionFactor(drivingFactor / 60.0); // meters per second
drivingConfig.closedLoop
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
// These are example gains you may need to them for your own robot!
.pid(0.04, 0, 0)
.velocityFF(drivingVelocityFeedForward)
.outputRange(-1, 1);
turningConfig
.idleMode(IdleMode.kBrake)
.smartCurrentLimit(20);
turningConfig.absoluteEncoder
// Invert the turning encoder, since the output shaft rotates in the opposite
// direction of the steering motor in the MAXSwerve Module.
.inverted(true)
.positionConversionFactor(turningFactor) // radians
.velocityConversionFactor(turningFactor / 60.0); // radians per second
turningConfig.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
// These are example gains you may need to them for your own robot!
.pid(1, 0, 0)
.outputRange(-1, 1)
// 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.
.positionWrappingEnabled(true)
.positionWrappingInputRange(0, turningFactor);
}
}
}

View File

@ -1,102 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
/**
* The Constants class provides a convenient place for teams to hold robot-wide
* numerical or boolean
* constants. This class should not be used for any other purpose. All constants
* should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>
* It is advised to statically import this class (or one of its inner classes)
* wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
// 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.8;
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
// Chassis configuration
public static final double kTrackWidth = Units.inchesToMeters(26.5);
// Distance between centers of right and left wheels on robot
public static final double kWheelBase = Units.inchesToMeters(26.5);
// 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 = 11;
public static final int kRearLeftDrivingCanId = 13;
public static final int kFrontRightDrivingCanId = 15;
public static final int kRearRightDrivingCanId = 17;
public static final int kFrontLeftTurningCanId = 10;
public static final int kRearLeftTurningCanId = 12;
public static final int kFrontRightTurningCanId = 14;
public static final int kRearRightTurningCanId = 16;
public static final boolean kGyroReversed = false;
}
public static final 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;
// 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 class OIConstants {
public static final int kDriverControllerPort = 0;
public static final double kDriveDeadband = 0.05;
}
public static final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 1;
public static final double kPYController = 1;
public static final double kPThetaController = 1;
// Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
}
public static final class NeoMotorConstants {
public static final double kFreeSpeedRpm = 5676;
}
}

View File

@ -4,119 +4,39 @@
package frc.robot;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.PS4Controller.Button;
import frc.robot.Constants.AutoConstants;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.OIConstants;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import java.util.List;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
/*
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot
* (including subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private DriveSubsystem drivetrain;
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
private CommandXboxController driver;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the button bindings
drivetrain = new DriveSubsystem();
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
configureButtonBindings();
// Configure default commands
m_robotDrive.setDefaultCommand(
// The left stick controls translation of the robot.
// Turning is controlled by the X axis of the right stick.
new RunCommand(
() -> m_robotDrive.drive(
-MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband),
true),
m_robotDrive));
}
/**
* Use this method to define your button->command mappings. Buttons can be
* created by
* instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its
* subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling
* passing it to a
* {@link JoystickButton}.
*/
private void configureButtonBindings() {
new JoystickButton(m_driverController, Button.kR1.value)
.whileTrue(new RunCommand(
() -> m_robotDrive.setX(),
m_robotDrive));
drivetrain.setDefaultCommand(
drivetrain.drive(
driver::getLeftY,
driver::getLeftX,
driver::getRightX,
() -> true
)
);
driver.start().whileTrue(drivetrain.setXCommand());
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config = new TrajectoryConfig(
AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(0, 0, new Rotation2d(0)),
// Pass through these two interior waypoints, making an 's' curve path
List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(3, 0, new Rotation2d(0)),
config);
var thetaController = new ProfiledPIDController(
AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
thetaController.enableContinuousInput(-Math.PI, Math.PI);
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
exampleTrajectory,
m_robotDrive::getPose, // Functional interface to feed supplier
DriveConstants.kDriveKinematics,
// Position controllers
new PIDController(AutoConstants.kPXController, 0, 0),
new PIDController(AutoConstants.kPYController, 0, 0),
thetaController,
m_robotDrive::setModuleStates,
m_robotDrive);
// Reset odometry to the starting pose of the trajectory.
m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
// Run path following command, then stop at the end.
return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
return new PrintCommand("NO AUTO DEFINED");
}
}

View File

@ -0,0 +1,18 @@
package frc.robot.constants;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
public class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 1;
public static final double kPYController = 1;
public static final double kPThetaController = 1;
// Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
}

View File

@ -0,0 +1,42 @@
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 class DriveConstants {
// 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.8;
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
// Chassis configuration
public static final double kTrackWidth = Units.inchesToMeters(26.5);
// Distance between centers of right and left wheels on robot
public static final double kWheelBase = Units.inchesToMeters(26.5);
// 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 = 11;
public static final int kRearLeftDrivingCanId = 13;
public static final int kFrontRightDrivingCanId = 15;
public static final int kRearRightDrivingCanId = 17;
public static final int kFrontLeftTurningCanId = 10;
public static final int kRearLeftTurningCanId = 12;
public static final int kFrontRightTurningCanId = 14;
public static final int kRearRightTurningCanId = 16;
public static final boolean kGyroReversed = false;
}

View File

@ -0,0 +1,66 @@
package frc.robot.constants;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
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;
// 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 SparkMaxConfig drivingConfig = new SparkMaxConfig();
public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
static {
// Use module constants to calculate conversion factors and feed forward gain.
double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
double turningFactor = 2 * Math.PI;
double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
drivingConfig
.idleMode(IdleMode.kBrake)
.smartCurrentLimit(50);
drivingConfig.encoder
.positionConversionFactor(drivingFactor) // meters
.velocityConversionFactor(drivingFactor / 60.0); // meters per second
drivingConfig.closedLoop
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
// These are example gains you may need to them for your own robot!
.pid(0.04, 0, 0)
.velocityFF(drivingVelocityFeedForward)
.outputRange(-1, 1);
turningConfig
.idleMode(IdleMode.kBrake)
.smartCurrentLimit(20);
turningConfig.absoluteEncoder
// Invert the turning encoder, since the output shaft rotates in the opposite
// direction of the steering motor in the MAXSwerve Module.
.inverted(true)
.positionConversionFactor(turningFactor) // radians
.velocityConversionFactor(turningFactor / 60.0); // radians per second
turningConfig.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
// These are example gains you may need to them for your own robot!
.pid(1, 0, 0)
.outputRange(-1, 1)
// 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.
.positionWrappingEnabled(true)
.positionWrappingInputRange(0, turningFactor);
}
}

View File

@ -0,0 +1,5 @@
package frc.robot.constants;
public class NeoMotorConstants {
public static final double kFreeSpeedRpm = 5676;
}

View File

@ -0,0 +1,7 @@
package frc.robot.constants;
public class OIConstants {
public static final int kDriverControllerPort = 0;
public static final double kDriveDeadband = 0.05;
}

View File

@ -4,6 +4,10 @@
package frc.robot.subsystems;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
@ -13,36 +17,53 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import frc.robot.Constants.DriveConstants;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.DriveConstants;
import frc.robot.constants.OIConstants;
public class DriveSubsystem extends SubsystemBase {
// Create MAXSwerveModules
private final MAXSwerveModule m_frontLeft = new MAXSwerveModule(
DriveConstants.kFrontLeftDrivingCanId,
DriveConstants.kFrontLeftTurningCanId,
DriveConstants.kFrontLeftChassisAngularOffset);
private final MAXSwerveModule m_frontRight = new MAXSwerveModule(
DriveConstants.kFrontRightDrivingCanId,
DriveConstants.kFrontRightTurningCanId,
DriveConstants.kFrontRightChassisAngularOffset);
private final MAXSwerveModule m_rearLeft = new MAXSwerveModule(
DriveConstants.kRearLeftDrivingCanId,
DriveConstants.kRearLeftTurningCanId,
DriveConstants.kBackLeftChassisAngularOffset);
private final MAXSwerveModule m_rearRight = new MAXSwerveModule(
DriveConstants.kRearRightDrivingCanId,
DriveConstants.kRearRightTurningCanId,
DriveConstants.kBackRightChassisAngularOffset);
private MAXSwerveModule m_frontLeft;
private MAXSwerveModule m_frontRight;
private MAXSwerveModule m_rearLeft;
private MAXSwerveModule m_rearRight;
// The gyro sensor
private final ADIS16470_IMU m_gyro = new ADIS16470_IMU();
private ADIS16470_IMU m_gyro;
// Odometry class for tracking robot pose
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
private SwerveDriveOdometry m_odometry;
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
m_frontLeft = new MAXSwerveModule(
DriveConstants.kFrontLeftDrivingCanId,
DriveConstants.kFrontLeftTurningCanId,
DriveConstants.kFrontLeftChassisAngularOffset
);
m_frontRight = new MAXSwerveModule(
DriveConstants.kFrontRightDrivingCanId,
DriveConstants.kFrontRightTurningCanId,
DriveConstants.kFrontRightChassisAngularOffset
);
m_rearLeft = new MAXSwerveModule(
DriveConstants.kRearLeftDrivingCanId,
DriveConstants.kRearLeftTurningCanId,
DriveConstants.kBackLeftChassisAngularOffset
);
m_rearRight = new MAXSwerveModule(
DriveConstants.kRearRightDrivingCanId,
DriveConstants.kRearRightTurningCanId,
DriveConstants.kBackRightChassisAngularOffset
);
m_gyro = new ADIS16470_IMU();
m_odometry = new SwerveDriveOdometry(
DriveConstants.kDriveKinematics,
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
new SwerveModulePosition[] {
@ -51,9 +72,6 @@ public class DriveSubsystem extends SubsystemBase {
m_rearLeft.getPosition(),
m_rearRight.getPosition()
});
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
}
@Override
@ -92,7 +110,20 @@ public class DriveSubsystem extends SubsystemBase {
m_rearLeft.getPosition(),
m_rearRight.getPosition()
},
pose);
pose
);
}
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
BooleanSupplier fieldRelative) {
return run(() -> {
drive(
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
fieldRelative.getAsBoolean()
);
});
}
/**
@ -123,6 +154,12 @@ public class DriveSubsystem extends SubsystemBase {
m_rearRight.setDesiredState(swerveModuleStates[3]);
}
public Command setXCommand() {
return run(() -> {
setX();
});
}
/**
* Sets the wheels into an X formation to prevent movement.
*/

View File

@ -17,7 +17,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.RelativeEncoder;
import frc.robot.Configs;
import frc.robot.constants.ModuleConstants;
public class MAXSwerveModule {
private final SparkMax m_drivingSpark;
@ -51,9 +51,9 @@ public class MAXSwerveModule {
// Apply the respective configurations to the SPARKS. Reset parameters before
// applying the configuration to bring the SPARK to a known good state. Persist
// the settings to the SPARK to avoid losing them on a power cycle.
m_drivingSpark.configure(Configs.MAXSwerveModule.drivingConfig, ResetMode.kResetSafeParameters,
m_drivingSpark.configure(ModuleConstants.drivingConfig, ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters);
m_turningSpark.configure(Configs.MAXSwerveModule.turningConfig, ResetMode.kResetSafeParameters,
m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters);
m_chassisAngularOffset = chassisAngularOffset;