mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[commands, wpimath] Remove Mecanum/SwerveControllerCommand and HolonomicDriveController (#8119)
This commit is contained in:
Gold856
@@ -475,42 +475,6 @@
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "MecanumControllerCommand",
|
||||
"description": "Follow a pre-generated trajectory with a mecanum drive using MecanumControllerCommand.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Mecanum Drive",
|
||||
"Gyro",
|
||||
"Encoder",
|
||||
"Odometry",
|
||||
"Trajectory",
|
||||
"Path Following",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "mecanumcontrollercommand",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "SwerveControllerCommand",
|
||||
"description": "Follow a pre-generated trajectory with a swerve drive using SwerveControllerCommand.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Swerve Drive",
|
||||
"Gyro",
|
||||
"Encoder",
|
||||
"Odometry",
|
||||
"Trajectory",
|
||||
"Path Following",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "swervecontrollercommand",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "StateSpaceFlywheel",
|
||||
"description": "Control a flywheel using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
|
||||
|
||||
@@ -1,87 +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 edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
|
||||
|
||||
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
|
||||
/**
|
||||
* 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 {
|
||||
public static final int kFrontLeftMotorPort = 0;
|
||||
public static final int kRearLeftMotorPort = 1;
|
||||
public static final int kFrontRightMotorPort = 2;
|
||||
public static final int kRearRightMotorPort = 3;
|
||||
|
||||
public static final int[] kFrontLeftEncoderPorts = new int[] {0, 1};
|
||||
public static final int[] kRearLeftEncoderPorts = new int[] {2, 3};
|
||||
public static final int[] kFrontRightEncoderPorts = new int[] {4, 5};
|
||||
public static final int[] kRearRightEncoderPorts = new int[] {6, 7};
|
||||
|
||||
public static final boolean kFrontLeftEncoderReversed = false;
|
||||
public static final boolean kRearLeftEncoderReversed = true;
|
||||
public static final boolean kFrontRightEncoderReversed = false;
|
||||
public static final boolean kRearRightEncoderReversed = true;
|
||||
|
||||
public static final double kTrackwidth = 0.5;
|
||||
// Distance between centers of right and left wheels on robot
|
||||
public static final double kWheelBase = 0.7;
|
||||
// Distance between centers of front and back wheels on robot
|
||||
|
||||
public static final MecanumDriveKinematics kDriveKinematics =
|
||||
new MecanumDriveKinematics(
|
||||
new Translation2d(kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(kWheelBase / 2, -kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, -kTrackwidth / 2));
|
||||
|
||||
public static final int kEncoderCPR = 1024;
|
||||
public static final double kWheelDiameter = 0.15; // m
|
||||
public static final double kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter * Math.PI) / kEncoderCPR;
|
||||
|
||||
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
// These characterization values MUST be determined either experimentally or theoretically
|
||||
// for *your* robot's drive.
|
||||
// The SysId tool provides a convenient method for obtaining these values for your robot.
|
||||
public static final SimpleMotorFeedforward kFeedforward =
|
||||
new SimpleMotorFeedforward(1, 0.8, 0.15);
|
||||
|
||||
// Example value only - as above, this must be tuned for your drive!
|
||||
public static final double kPFrontLeftVel = 0.5;
|
||||
public static final double kPRearLeftVel = 0.5;
|
||||
public static final double kPFrontRightVel = 0.5;
|
||||
public static final double kPRearRightVel = 0.5;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int kDriverControllerPort = 0;
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final double kMaxSpeed = 3; // m/s
|
||||
public static final double kMaxAcceleration = 3; // m/s²
|
||||
public static final double kMaxAngularSpeed = Math.PI; // rad/s
|
||||
public static final double kMaxAngularAcceleration = Math.PI; // rad/s²
|
||||
|
||||
public static final double kPXController = 0.5;
|
||||
public static final double kPYController = 0.5;
|
||||
public static final double kPThetaController = 0.5;
|
||||
|
||||
// Constraint for the motion profilied robot angle controller
|
||||
public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
|
||||
new TrapezoidProfile.Constraints(kMaxAngularSpeed, kMaxAngularAcceleration);
|
||||
}
|
||||
}
|
||||
@@ -1,25 +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 edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -1,100 +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 edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
/**
|
||||
* The methods in this class are called automatically corresponding to each mode, as described in
|
||||
* the TimedRobot documentation. If you change the name of this class or the package after creating
|
||||
* this project, you must also update the Main.java file in the project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
|
||||
private final RobotContainer m_robotContainer;
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be used for any
|
||||
* initialization code.
|
||||
*/
|
||||
public Robot() {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
|
||||
* that you want ran during disabled, autonomous, teleoperated and test.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
|
||||
* SmartDashboard integrated updating.
|
||||
*/
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
/** This function is called once each time the robot enters Disabled mode. */
|
||||
@Override
|
||||
public void disabledInit() {}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
/*
|
||||
* String autoSelected = SmartDashboard.getString("Auto Selector",
|
||||
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
|
||||
* = new MyAutoCommand(); break; case "Default Auto": default:
|
||||
* autonomousCommand = new ExampleCommand(); break; }
|
||||
*/
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
CommandScheduler.getInstance().schedule(m_autonomousCommand);
|
||||
}
|
||||
}
|
||||
|
||||
/** This function is called periodically during autonomous. */
|
||||
@Override
|
||||
public void autonomousPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
}
|
||||
|
||||
/** This function is called periodically during operator control. */
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
}
|
||||
@@ -1,130 +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 edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
|
||||
|
||||
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.XboxController.Button;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import java.util.List;
|
||||
|
||||
/*
|
||||
* 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();
|
||||
|
||||
// The driver's controller
|
||||
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
|
||||
|
||||
/** The container for the robot. Contains subsystems, OI devices, and commands. */
|
||||
public RobotContainer() {
|
||||
// Configure the button bindings
|
||||
configureButtonBindings();
|
||||
|
||||
// Configure default commands
|
||||
// Set the default drive command to split-stick arcade drive
|
||||
m_robotDrive.setDefaultCommand(
|
||||
// A split-stick arcade command, with forward/backward controlled by the left
|
||||
// hand, and turning controlled by the right.
|
||||
new RunCommand(
|
||||
() ->
|
||||
m_robotDrive.drive(
|
||||
-m_driverController.getLeftY(),
|
||||
-m_driverController.getRightX(),
|
||||
-m_driverController.getLeftX(),
|
||||
false),
|
||||
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() {
|
||||
// Drive at half speed when the right bumper is held
|
||||
new JoystickButton(m_driverController, Button.kRightBumper.value)
|
||||
.onTrue(new InstantCommand(() -> m_robotDrive.setMaxOutput(0.5)))
|
||||
.onFalse(new InstantCommand(() -> m_robotDrive.setMaxOutput(1)));
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.kMaxSpeed, AutoConstants.kMaxAcceleration)
|
||||
// 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
|
||||
Pose2d.kZero,
|
||||
// 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, Rotation2d.kZero),
|
||||
config);
|
||||
|
||||
MecanumControllerCommand mecanumControllerCommand =
|
||||
new MecanumControllerCommand(
|
||||
exampleTrajectory,
|
||||
m_robotDrive::getPose,
|
||||
DriveConstants.kFeedforward,
|
||||
DriveConstants.kDriveKinematics,
|
||||
|
||||
// Position controllers
|
||||
new PIDController(AutoConstants.kPXController, 0, 0),
|
||||
new PIDController(AutoConstants.kPYController, 0, 0),
|
||||
new ProfiledPIDController(
|
||||
AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints),
|
||||
|
||||
// Needed for normalizing wheel speeds
|
||||
AutoConstants.kMaxSpeed,
|
||||
|
||||
// Velocity PID's
|
||||
new PIDController(DriveConstants.kPFrontLeftVel, 0, 0),
|
||||
new PIDController(DriveConstants.kPRearLeftVel, 0, 0),
|
||||
new PIDController(DriveConstants.kPFrontRightVel, 0, 0),
|
||||
new PIDController(DriveConstants.kPRearRightVel, 0, 0),
|
||||
m_robotDrive::getCurrentWheelSpeeds,
|
||||
m_robotDrive::setDriveMotorControllersVolts, // Consumer for the output motor voltages
|
||||
m_robotDrive);
|
||||
|
||||
// Reset odometry to the initial pose of the trajectory, run path following
|
||||
// command, then stop at the end.
|
||||
return Commands.sequence(
|
||||
new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
|
||||
mecanumControllerCommand,
|
||||
new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
|
||||
}
|
||||
}
|
||||
@@ -1,239 +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 edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
|
||||
import edu.wpi.first.util.sendable.SendableRegistry;
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.drive.MecanumDrive;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class DriveSubsystem extends SubsystemBase {
|
||||
private final PWMSparkMax m_frontLeft = new PWMSparkMax(DriveConstants.kFrontLeftMotorPort);
|
||||
private final PWMSparkMax m_rearLeft = new PWMSparkMax(DriveConstants.kRearLeftMotorPort);
|
||||
private final PWMSparkMax m_frontRight = new PWMSparkMax(DriveConstants.kFrontRightMotorPort);
|
||||
private final PWMSparkMax m_rearRight = new PWMSparkMax(DriveConstants.kRearRightMotorPort);
|
||||
|
||||
private final MecanumDrive m_drive =
|
||||
new MecanumDrive(m_frontLeft::set, m_rearLeft::set, m_frontRight::set, m_rearRight::set);
|
||||
|
||||
// The front-left-side drive encoder
|
||||
private final Encoder m_frontLeftEncoder =
|
||||
new Encoder(
|
||||
DriveConstants.kFrontLeftEncoderPorts[0],
|
||||
DriveConstants.kFrontLeftEncoderPorts[1],
|
||||
DriveConstants.kFrontLeftEncoderReversed);
|
||||
|
||||
// The rear-left-side drive encoder
|
||||
private final Encoder m_rearLeftEncoder =
|
||||
new Encoder(
|
||||
DriveConstants.kRearLeftEncoderPorts[0],
|
||||
DriveConstants.kRearLeftEncoderPorts[1],
|
||||
DriveConstants.kRearLeftEncoderReversed);
|
||||
|
||||
// The front-right--side drive encoder
|
||||
private final Encoder m_frontRightEncoder =
|
||||
new Encoder(
|
||||
DriveConstants.kFrontRightEncoderPorts[0],
|
||||
DriveConstants.kFrontRightEncoderPorts[1],
|
||||
DriveConstants.kFrontRightEncoderReversed);
|
||||
|
||||
// The rear-right-side drive encoder
|
||||
private final Encoder m_rearRightEncoder =
|
||||
new Encoder(
|
||||
DriveConstants.kRearRightEncoderPorts[0],
|
||||
DriveConstants.kRearRightEncoderPorts[1],
|
||||
DriveConstants.kRearRightEncoderReversed);
|
||||
|
||||
// The gyro sensor
|
||||
private final AnalogGyro m_gyro = new AnalogGyro(0);
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
MecanumDriveOdometry m_odometry =
|
||||
new MecanumDriveOdometry(
|
||||
DriveConstants.kDriveKinematics,
|
||||
m_gyro.getRotation2d(),
|
||||
new MecanumDriveWheelPositions());
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public DriveSubsystem() {
|
||||
SendableRegistry.addChild(m_drive, m_frontLeft);
|
||||
SendableRegistry.addChild(m_drive, m_rearLeft);
|
||||
SendableRegistry.addChild(m_drive, m_frontRight);
|
||||
SendableRegistry.addChild(m_drive, m_rearRight);
|
||||
|
||||
// Sets the distance per pulse for the encoders
|
||||
m_frontLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
m_rearLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
m_frontRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
m_rearRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_frontRight.setInverted(true);
|
||||
m_rearRight.setInverted(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// Update the odometry in the periodic block
|
||||
m_odometry.update(m_gyro.getRotation2d(), getCurrentWheelDistances());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
*
|
||||
* @return The pose.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPose();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(m_gyro.getRotation2d(), getCurrentWheelDistances(), pose);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] and the linear
|
||||
* speeds have no effect on the angular speed.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction (forward/backwards).
|
||||
* @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) {
|
||||
if (fieldRelative) {
|
||||
m_drive.driveCartesian(xSpeed, ySpeed, rot, m_gyro.getRotation2d());
|
||||
} else {
|
||||
m_drive.driveCartesian(xSpeed, ySpeed, rot);
|
||||
}
|
||||
}
|
||||
|
||||
/** Sets the front left drive MotorController to a voltage. */
|
||||
public void setDriveMotorControllersVolts(
|
||||
double frontLeftVoltage,
|
||||
double frontRightVoltage,
|
||||
double rearLeftVoltage,
|
||||
double rearRightVoltage) {
|
||||
m_frontLeft.setVoltage(frontLeftVoltage);
|
||||
m_rearLeft.setVoltage(rearLeftVoltage);
|
||||
m_frontRight.setVoltage(frontRightVoltage);
|
||||
m_rearRight.setVoltage(rearRightVoltage);
|
||||
}
|
||||
|
||||
/** Resets the drive encoders to currently read a position of 0. */
|
||||
public void resetEncoders() {
|
||||
m_frontLeftEncoder.reset();
|
||||
m_rearLeftEncoder.reset();
|
||||
m_frontRightEncoder.reset();
|
||||
m_rearRightEncoder.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the front left drive encoder.
|
||||
*
|
||||
* @return the front left drive encoder
|
||||
*/
|
||||
public Encoder getFrontLeftEncoder() {
|
||||
return m_frontLeftEncoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the rear left drive encoder.
|
||||
*
|
||||
* @return the rear left drive encoder
|
||||
*/
|
||||
public Encoder getRearLeftEncoder() {
|
||||
return m_rearLeftEncoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the front right drive encoder.
|
||||
*
|
||||
* @return the front right drive encoder
|
||||
*/
|
||||
public Encoder getFrontRightEncoder() {
|
||||
return m_frontRightEncoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the rear right drive encoder.
|
||||
*
|
||||
* @return the rear right encoder
|
||||
*/
|
||||
public Encoder getRearRightEncoder() {
|
||||
return m_rearRightEncoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current wheel speeds.
|
||||
*
|
||||
* @return the current wheel speeds in a MecanumDriveWheelSpeeds object.
|
||||
*/
|
||||
public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
|
||||
return new MecanumDriveWheelSpeeds(
|
||||
m_frontLeftEncoder.getRate(),
|
||||
m_rearLeftEncoder.getRate(),
|
||||
m_frontRightEncoder.getRate(),
|
||||
m_rearRightEncoder.getRate());
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current wheel distance measurements.
|
||||
*
|
||||
* @return the current wheel distance measurements in a MecanumDriveWheelPositions object.
|
||||
*/
|
||||
public MecanumDriveWheelPositions getCurrentWheelDistances() {
|
||||
return new MecanumDriveWheelPositions(
|
||||
m_frontLeftEncoder.getDistance(),
|
||||
m_rearLeftEncoder.getDistance(),
|
||||
m_frontRightEncoder.getDistance(),
|
||||
m_rearRightEncoder.getDistance());
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
|
||||
*
|
||||
* @param maxOutput the maximum output to which the drive will be constrained
|
||||
*/
|
||||
public void setMaxOutput(double maxOutput) {
|
||||
m_drive.setMaxOutput(maxOutput);
|
||||
}
|
||||
|
||||
/** 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 m_gyro.getRotation2d().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();
|
||||
}
|
||||
}
|
||||
@@ -1,116 +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 edu.wpi.first.wpilibj.examples.swervecontrollercommand;
|
||||
|
||||
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.wpilibj.TimedRobot;
|
||||
|
||||
/**
|
||||
* 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 {
|
||||
public static final int kFrontLeftDriveMotorPort = 0;
|
||||
public static final int kRearLeftDriveMotorPort = 2;
|
||||
public static final int kFrontRightDriveMotorPort = 4;
|
||||
public static final int kRearRightDriveMotorPort = 6;
|
||||
|
||||
public static final int kFrontLeftTurningMotorPort = 1;
|
||||
public static final int kRearLeftTurningMotorPort = 3;
|
||||
public static final int kFrontRightTurningMotorPort = 5;
|
||||
public static final int kRearRightTurningMotorPort = 7;
|
||||
|
||||
public static final int[] kFrontLeftTurningEncoderPorts = new int[] {0, 1};
|
||||
public static final int[] kRearLeftTurningEncoderPorts = new int[] {2, 3};
|
||||
public static final int[] kFrontRightTurningEncoderPorts = new int[] {4, 5};
|
||||
public static final int[] kRearRightTurningEncoderPorts = new int[] {6, 7};
|
||||
|
||||
public static final boolean kFrontLeftTurningEncoderReversed = false;
|
||||
public static final boolean kRearLeftTurningEncoderReversed = true;
|
||||
public static final boolean kFrontRightTurningEncoderReversed = false;
|
||||
public static final boolean kRearRightTurningEncoderReversed = true;
|
||||
|
||||
public static final int[] kFrontLeftDriveEncoderPorts = new int[] {8, 9};
|
||||
public static final int[] kRearLeftDriveEncoderPorts = new int[] {10, 11};
|
||||
public static final int[] kFrontRightDriveEncoderPorts = new int[] {12, 13};
|
||||
public static final int[] kRearRightDriveEncoderPorts = new int[] {14, 15};
|
||||
|
||||
public static final boolean kFrontLeftDriveEncoderReversed = false;
|
||||
public static final boolean kRearLeftDriveEncoderReversed = true;
|
||||
public static final boolean kFrontRightDriveEncoderReversed = false;
|
||||
public static final boolean kRearRightDriveEncoderReversed = true;
|
||||
|
||||
// If you call DriveSubsystem.drive() with a different period make sure to update this.
|
||||
public static final double kDrivePeriod = TimedRobot.kDefaultPeriod;
|
||||
|
||||
public static final double kTrackwidth = 0.5;
|
||||
// Distance between centers of right and left wheels on robot
|
||||
public static final double kWheelBase = 0.7;
|
||||
// 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));
|
||||
|
||||
public static final boolean kGyroReversed = false;
|
||||
|
||||
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
// These characterization values MUST be determined either experimentally or theoretically
|
||||
// for *your* robot's drive.
|
||||
// The SysId tool provides a convenient method for obtaining these values for your robot.
|
||||
public static final double ks = 1; // V
|
||||
public static final double kv = 0.8; // V/(m/s)
|
||||
public static final double ka = 0.15; // V/(m/s²)
|
||||
|
||||
public static final double kMaxSpeed = 3; // m/s
|
||||
}
|
||||
|
||||
public static final class ModuleConstants {
|
||||
public static final double kMaxModuleAngularSpeed = 2 * Math.PI; // rad/s
|
||||
public static final double kMaxModuleAngularAcceleration = 2 * Math.PI; // rad/s²
|
||||
|
||||
public static final int kEncoderCPR = 1024;
|
||||
public static final double kWheelDiameter = 0.15; // m
|
||||
public static final double kDriveEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter * Math.PI) / kEncoderCPR;
|
||||
|
||||
public static final double kTurningEncoderDistancePerPulse =
|
||||
// Assumes the encoders are on a 1:1 reduction with the module shaft.
|
||||
(2 * Math.PI) / kEncoderCPR;
|
||||
|
||||
public static final double kPModuleTurningController = 1;
|
||||
|
||||
public static final double kPModuleDriveController = 1;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int kDriverControllerPort = 0;
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final double kMaxSpeed = 3; // m/s
|
||||
public static final double kMaxAcceleration = 3; // m/s²
|
||||
public static final double kMaxAngularSpeed = Math.PI; // rad/s
|
||||
public static final double kMaxAngularAcceleration = Math.PI; // rad/s²
|
||||
|
||||
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(kMaxAngularSpeed, kMaxAngularAcceleration);
|
||||
}
|
||||
}
|
||||
@@ -1,25 +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 edu.wpi.first.wpilibj.examples.swervecontrollercommand;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -1,100 +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 edu.wpi.first.wpilibj.examples.swervecontrollercommand;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
/**
|
||||
* The methods in this class are called automatically corresponding to each mode, as described in
|
||||
* the TimedRobot documentation. If you change the name of this class or the package after creating
|
||||
* this project, you must also update the Main.java file in the project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
|
||||
private final RobotContainer m_robotContainer;
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be used for any
|
||||
* initialization code.
|
||||
*/
|
||||
public Robot() {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
|
||||
* that you want ran during disabled, autonomous, teleoperated and test.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
|
||||
* SmartDashboard integrated updating.
|
||||
*/
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
/** This function is called once each time the robot enters Disabled mode. */
|
||||
@Override
|
||||
public void disabledInit() {}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
/*
|
||||
* String autoSelected = SmartDashboard.getString("Auto Selector",
|
||||
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
|
||||
* = new MyAutoCommand(); break; case "Default Auto": default:
|
||||
* autonomousCommand = new ExampleCommand(); break; }
|
||||
*/
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
CommandScheduler.getInstance().schedule(m_autonomousCommand);
|
||||
}
|
||||
}
|
||||
|
||||
/** This function is called periodically during autonomous. */
|
||||
@Override
|
||||
public void autonomousPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
}
|
||||
|
||||
/** This function is called periodically during operator control. */
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
}
|
||||
@@ -1,120 +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 edu.wpi.first.wpilibj.examples.swervecontrollercommand;
|
||||
|
||||
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.examples.swervecontrollercommand.Constants.AutoConstants;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
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;
|
||||
|
||||
/*
|
||||
* 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();
|
||||
|
||||
// The driver's controller
|
||||
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
|
||||
|
||||
/** The container for the robot. Contains subsystems, OI devices, and commands. */
|
||||
public RobotContainer() {
|
||||
// Configure the button bindings
|
||||
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(
|
||||
// Multiply by max speed to map the joystick unitless inputs to actual units.
|
||||
// This will map the [-1, 1] to [max speed backwards, max speed forwards],
|
||||
// converting them to actual units.
|
||||
m_driverController.getLeftY() * DriveConstants.kMaxSpeed,
|
||||
m_driverController.getLeftX() * DriveConstants.kMaxSpeed,
|
||||
m_driverController.getRightX() * ModuleConstants.kMaxModuleAngularSpeed,
|
||||
false),
|
||||
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() {}
|
||||
|
||||
/**
|
||||
* 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.kMaxSpeed, AutoConstants.kMaxAcceleration)
|
||||
// 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
|
||||
Pose2d.kZero,
|
||||
// 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, Rotation2d.kZero),
|
||||
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 initial pose of the trajectory, run path following
|
||||
// command, then stop at the end.
|
||||
return Commands.sequence(
|
||||
new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
|
||||
swerveControllerCommand,
|
||||
new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
|
||||
}
|
||||
}
|
||||
@@ -1,179 +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 edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
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.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class DriveSubsystem extends SubsystemBase {
|
||||
// Robot swerve modules
|
||||
private final SwerveModule m_frontLeft =
|
||||
new SwerveModule(
|
||||
DriveConstants.kFrontLeftDriveMotorPort,
|
||||
DriveConstants.kFrontLeftTurningMotorPort,
|
||||
DriveConstants.kFrontLeftDriveEncoderPorts,
|
||||
DriveConstants.kFrontLeftTurningEncoderPorts,
|
||||
DriveConstants.kFrontLeftDriveEncoderReversed,
|
||||
DriveConstants.kFrontLeftTurningEncoderReversed);
|
||||
|
||||
private final SwerveModule m_rearLeft =
|
||||
new SwerveModule(
|
||||
DriveConstants.kRearLeftDriveMotorPort,
|
||||
DriveConstants.kRearLeftTurningMotorPort,
|
||||
DriveConstants.kRearLeftDriveEncoderPorts,
|
||||
DriveConstants.kRearLeftTurningEncoderPorts,
|
||||
DriveConstants.kRearLeftDriveEncoderReversed,
|
||||
DriveConstants.kRearLeftTurningEncoderReversed);
|
||||
|
||||
private final SwerveModule m_frontRight =
|
||||
new SwerveModule(
|
||||
DriveConstants.kFrontRightDriveMotorPort,
|
||||
DriveConstants.kFrontRightTurningMotorPort,
|
||||
DriveConstants.kFrontRightDriveEncoderPorts,
|
||||
DriveConstants.kFrontRightTurningEncoderPorts,
|
||||
DriveConstants.kFrontRightDriveEncoderReversed,
|
||||
DriveConstants.kFrontRightTurningEncoderReversed);
|
||||
|
||||
private final SwerveModule m_rearRight =
|
||||
new SwerveModule(
|
||||
DriveConstants.kRearRightDriveMotorPort,
|
||||
DriveConstants.kRearRightTurningMotorPort,
|
||||
DriveConstants.kRearRightDriveEncoderPorts,
|
||||
DriveConstants.kRearRightTurningEncoderPorts,
|
||||
DriveConstants.kRearRightDriveEncoderReversed,
|
||||
DriveConstants.kRearRightTurningEncoderReversed);
|
||||
|
||||
// The gyro sensor
|
||||
private final AnalogGyro m_gyro = new AnalogGyro(0);
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
SwerveDriveOdometry m_odometry =
|
||||
new SwerveDriveOdometry(
|
||||
DriveConstants.kDriveKinematics,
|
||||
m_gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public DriveSubsystem() {}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// Update the odometry in the periodic block
|
||||
m_odometry.update(
|
||||
m_gyro.getRotation2d(),
|
||||
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.getPose();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(
|
||||
m_gyro.getRotation2d(),
|
||||
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.
|
||||
*/
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
|
||||
if (fieldRelative) {
|
||||
chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
|
||||
}
|
||||
chassisSpeeds = chassisSpeeds.discretize(DriveConstants.kDrivePeriod);
|
||||
|
||||
var states = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds);
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeed);
|
||||
|
||||
m_frontLeft.setDesiredState(states[0]);
|
||||
m_frontRight.setDesiredState(states[1]);
|
||||
m_rearLeft.setDesiredState(states[2]);
|
||||
m_rearRight.setDesiredState(states[3]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the swerve ModuleStates.
|
||||
*
|
||||
* @param desiredStates The desired SwerveModule states.
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kMaxSpeed);
|
||||
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();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return m_gyro.getRotation2d().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() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
}
|
||||
@@ -1,137 +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 edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
|
||||
public class SwerveModule {
|
||||
private final Spark m_driveMotor;
|
||||
private final Spark m_turningMotor;
|
||||
|
||||
private final Encoder m_driveEncoder;
|
||||
private final Encoder m_turningEncoder;
|
||||
|
||||
private final PIDController m_drivePIDController =
|
||||
new PIDController(ModuleConstants.kPModuleDriveController, 0, 0);
|
||||
|
||||
// Using a TrapezoidProfile PIDController to allow for smooth turning
|
||||
private final ProfiledPIDController m_turningPIDController =
|
||||
new ProfiledPIDController(
|
||||
ModuleConstants.kPModuleTurningController,
|
||||
0,
|
||||
0,
|
||||
new TrapezoidProfile.Constraints(
|
||||
ModuleConstants.kMaxModuleAngularSpeed,
|
||||
ModuleConstants.kMaxModuleAngularAcceleration));
|
||||
|
||||
/**
|
||||
* Constructs a SwerveModule.
|
||||
*
|
||||
* @param driveMotorChannel The channel of the drive motor.
|
||||
* @param turningMotorChannel The channel of the turning motor.
|
||||
* @param driveEncoderChannels The channels of the drive encoder.
|
||||
* @param turningEncoderChannels The channels of the turning encoder.
|
||||
* @param driveEncoderReversed Whether the drive encoder is reversed.
|
||||
* @param turningEncoderReversed Whether the turning encoder is reversed.
|
||||
*/
|
||||
public SwerveModule(
|
||||
int driveMotorChannel,
|
||||
int turningMotorChannel,
|
||||
int[] driveEncoderChannels,
|
||||
int[] turningEncoderChannels,
|
||||
boolean driveEncoderReversed,
|
||||
boolean turningEncoderReversed) {
|
||||
m_driveMotor = new Spark(driveMotorChannel);
|
||||
m_turningMotor = new Spark(turningMotorChannel);
|
||||
|
||||
m_driveEncoder = new Encoder(driveEncoderChannels[0], driveEncoderChannels[1]);
|
||||
|
||||
m_turningEncoder = new Encoder(turningEncoderChannels[0], turningEncoderChannels[1]);
|
||||
|
||||
// Set the distance per pulse for the drive encoder. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_driveEncoder.setDistancePerPulse(ModuleConstants.kDriveEncoderDistancePerPulse);
|
||||
|
||||
// Set whether drive encoder should be reversed or not
|
||||
m_driveEncoder.setReverseDirection(driveEncoderReversed);
|
||||
|
||||
// Set the distance (in this case, angle) in radians per pulse for the turning encoder.
|
||||
// This is the the angle through an entire rotation (2 * pi) divided by the
|
||||
// encoder resolution.
|
||||
m_turningEncoder.setDistancePerPulse(ModuleConstants.kTurningEncoderDistancePerPulse);
|
||||
|
||||
// Set whether turning encoder should be reversed or not
|
||||
m_turningEncoder.setReverseDirection(turningEncoderReversed);
|
||||
|
||||
// Limit the PID Controller's input range between -pi and pi and set the input
|
||||
// to be continuous.
|
||||
m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current state of the module.
|
||||
*
|
||||
* @return The current state of the module.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
return new SwerveModuleState(
|
||||
m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current position of the module.
|
||||
*
|
||||
* @return The current position of the module.
|
||||
*/
|
||||
public SwerveModulePosition getPosition() {
|
||||
return new SwerveModulePosition(
|
||||
m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.getDistance()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the desired state for the module.
|
||||
*
|
||||
* @param desiredState Desired state with speed and angle.
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
|
||||
|
||||
// Optimize the reference state to avoid spinning further than 90 degrees
|
||||
desiredState.optimize(encoderRotation);
|
||||
|
||||
// Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
|
||||
// direction of travel that can occur when modules change directions. This results in smoother
|
||||
// driving.
|
||||
desiredState.cosineScale(encoderRotation);
|
||||
|
||||
// Calculate the drive output from the drive PID controller.
|
||||
final double driveOutput =
|
||||
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
|
||||
|
||||
// Calculate the turning motor output from the turning PID controller.
|
||||
final double turnOutput =
|
||||
m_turningPIDController.calculate(
|
||||
m_turningEncoder.getDistance(), desiredState.angle.getRadians());
|
||||
|
||||
// Calculate the turning motor output from the turning PID controller.
|
||||
m_driveMotor.set(driveOutput);
|
||||
m_turningMotor.set(turnOutput);
|
||||
}
|
||||
|
||||
/** Zeroes all the SwerveModule encoders. */
|
||||
public void resetEncoders() {
|
||||
m_driveEncoder.reset();
|
||||
m_turningEncoder.reset();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user