Removing REV template hot hot garbage programming practices
This commit is contained in:
parent
8ac67fcf21
commit
a48cff9fb2
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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");
|
||||
}
|
||||
}
|
||||
|
||||
18
src/main/java/frc/robot/constants/AutoConstants.java
Normal file
18
src/main/java/frc/robot/constants/AutoConstants.java
Normal 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);
|
||||
}
|
||||
42
src/main/java/frc/robot/constants/DriveConstants.java
Normal file
42
src/main/java/frc/robot/constants/DriveConstants.java
Normal 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;
|
||||
}
|
||||
66
src/main/java/frc/robot/constants/ModuleConstants.java
Normal file
66
src/main/java/frc/robot/constants/ModuleConstants.java
Normal 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);
|
||||
}
|
||||
}
|
||||
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 kDriverControllerPort = 0;
|
||||
|
||||
public static final double kDriveDeadband = 0.05;
|
||||
}
|
||||
@ -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,168 +17,201 @@ 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);
|
||||
// Create MAXSwerveModules
|
||||
private MAXSwerveModule m_frontLeft;
|
||||
private MAXSwerveModule m_frontRight;
|
||||
private MAXSwerveModule m_rearLeft;
|
||||
private MAXSwerveModule m_rearRight;
|
||||
|
||||
private final MAXSwerveModule m_frontRight = new MAXSwerveModule(
|
||||
DriveConstants.kFrontRightDrivingCanId,
|
||||
DriveConstants.kFrontRightTurningCanId,
|
||||
DriveConstants.kFrontRightChassisAngularOffset);
|
||||
// The gyro sensor
|
||||
private ADIS16470_IMU m_gyro;
|
||||
|
||||
private final MAXSwerveModule m_rearLeft = new MAXSwerveModule(
|
||||
DriveConstants.kRearLeftDrivingCanId,
|
||||
DriveConstants.kRearLeftTurningCanId,
|
||||
DriveConstants.kBackLeftChassisAngularOffset);
|
||||
// Odometry class for tracking robot pose
|
||||
private SwerveDriveOdometry m_odometry;
|
||||
|
||||
private final MAXSwerveModule m_rearRight = new MAXSwerveModule(
|
||||
DriveConstants.kRearRightDrivingCanId,
|
||||
DriveConstants.kRearRightTurningCanId,
|
||||
DriveConstants.kBackRightChassisAngularOffset);
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public DriveSubsystem() {
|
||||
m_frontLeft = new MAXSwerveModule(
|
||||
DriveConstants.kFrontLeftDrivingCanId,
|
||||
DriveConstants.kFrontLeftTurningCanId,
|
||||
DriveConstants.kFrontLeftChassisAngularOffset
|
||||
);
|
||||
|
||||
// The gyro sensor
|
||||
private final ADIS16470_IMU m_gyro = new ADIS16470_IMU();
|
||||
m_frontRight = new MAXSwerveModule(
|
||||
DriveConstants.kFrontRightDrivingCanId,
|
||||
DriveConstants.kFrontRightTurningCanId,
|
||||
DriveConstants.kFrontRightChassisAngularOffset
|
||||
);
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
|
||||
DriveConstants.kDriveKinematics,
|
||||
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
m_rearLeft = new MAXSwerveModule(
|
||||
DriveConstants.kRearLeftDrivingCanId,
|
||||
DriveConstants.kRearLeftTurningCanId,
|
||||
DriveConstants.kBackLeftChassisAngularOffset
|
||||
);
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public DriveSubsystem() {
|
||||
}
|
||||
m_rearRight = new MAXSwerveModule(
|
||||
DriveConstants.kRearRightDrivingCanId,
|
||||
DriveConstants.kRearRightTurningCanId,
|
||||
DriveConstants.kBackRightChassisAngularOffset
|
||||
);
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// Update the odometry in the periodic block
|
||||
m_odometry.update(
|
||||
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
m_gyro = new ADIS16470_IMU();
|
||||
|
||||
m_odometry = new SwerveDriveOdometry(
|
||||
DriveConstants.kDriveKinematics,
|
||||
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
|
||||
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();
|
||||
}
|
||||
@Override
|
||||
public void periodic() {
|
||||
// Update the odometry in the periodic block
|
||||
m_odometry.update(
|
||||
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(m_gyro.getAngle(IMUAxis.kZ)),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
},
|
||||
pose);
|
||||
}
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
*
|
||||
* @return The pose.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPoseMeters();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
// Convert the commanded speeds into the correct units for the drivetrain
|
||||
double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeedMetersPerSecond;
|
||||
double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeedMetersPerSecond;
|
||||
double rotDelivered = rot * DriveConstants.kMaxAngularSpeed;
|
||||
/**
|
||||
* 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(m_gyro.getAngle(IMUAxis.kZ)),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
},
|
||||
pose
|
||||
);
|
||||
}
|
||||
|
||||
var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
|
||||
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
|
||||
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
|
||||
m_frontLeft.setDesiredState(swerveModuleStates[0]);
|
||||
m_frontRight.setDesiredState(swerveModuleStates[1]);
|
||||
m_rearLeft.setDesiredState(swerveModuleStates[2]);
|
||||
m_rearRight.setDesiredState(swerveModuleStates[3]);
|
||||
}
|
||||
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()
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* 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)));
|
||||
}
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
// Convert the commanded speeds into the correct units for the drivetrain
|
||||
double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeedMetersPerSecond;
|
||||
double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeedMetersPerSecond;
|
||||
double rotDelivered = rot * DriveConstants.kMaxAngularSpeed;
|
||||
|
||||
/**
|
||||
* Sets the swerve ModuleStates.
|
||||
*
|
||||
* @param desiredStates The desired SwerveModule states.
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
|
||||
m_frontLeft.setDesiredState(desiredStates[0]);
|
||||
m_frontRight.setDesiredState(desiredStates[1]);
|
||||
m_rearLeft.setDesiredState(desiredStates[2]);
|
||||
m_rearRight.setDesiredState(desiredStates[3]);
|
||||
}
|
||||
var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
|
||||
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
|
||||
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
|
||||
m_frontLeft.setDesiredState(swerveModuleStates[0]);
|
||||
m_frontRight.setDesiredState(swerveModuleStates[1]);
|
||||
m_rearLeft.setDesiredState(swerveModuleStates[2]);
|
||||
m_rearRight.setDesiredState(swerveModuleStates[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();
|
||||
}
|
||||
public Command setXCommand() {
|
||||
return run(() -> {
|
||||
setX();
|
||||
});
|
||||
}
|
||||
|
||||
/** Zeroes the heading of the robot. */
|
||||
public void zeroHeading() {
|
||||
m_gyro.reset();
|
||||
}
|
||||
/**
|
||||
* 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)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
|
||||
}
|
||||
/**
|
||||
* Sets the swerve ModuleStates.
|
||||
*
|
||||
* @param desiredStates The desired SwerveModule states.
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
|
||||
m_frontLeft.setDesiredState(desiredStates[0]);
|
||||
m_frontRight.setDesiredState(desiredStates[1]);
|
||||
m_rearLeft.setDesiredState(desiredStates[2]);
|
||||
m_rearRight.setDesiredState(desiredStates[3]);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(IMUAxis.kZ) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
/** 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();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(IMUAxis.kZ) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user