Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add holonomic follower examples (#2052)

Browse Source
This commit is contained in:
CTT
2019-11-21 19:52:56 -08:00
committed by Peter Johnson
parent 9a8067465c
commit a58dbec8aa
51 changed files with 4793 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
View File

@@ -482,5 +482,35 @@
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "MecanumControllerCommand",
"description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",
"tags": [
"MecanumControllerCommand",
"Mecanum",
"PID",
"Trajectory",
"Path following"
],
"foldername": "mecanumcontrollercommand",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "SwerveControllerCommand",
"description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.",
"tags": [
"SwerveControllerCommand",
"Swerve",
"PID",
"Trajectory",
"Path following"
],
"foldername": "swervecontrollercommand",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
}
]

97
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java Normal file
View File

@@ -0,0 +1,97 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
import edu.wpi.first.wpilibj.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[]{5, 6};
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 kWheelDiameterMeters = 0.15;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
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 RobotPy Characterization Toolsuite provides a convenient tool 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 = 1;
}
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 = 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(kMaxAngularSpeedRadiansPerSecond,
kMaxAngularSpeedRadiansPerSecondSquared);
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the 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);
}
}

121
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java Normal file
View File

@@ -0,0 +1,121 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the 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 VM is configured to automatically run this class, and to call the functions 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 build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// 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 robot packet, 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) {
m_autonomousCommand.schedule();
}
}
/**
* 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() {
}
}

148
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java Normal file
View File

@@ -0,0 +1,148 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
import java.util.List;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPThetaController;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPXController;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPYController;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kThetaControllerConstraints;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFeedforward;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontLeftVel;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontRightVel;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearLeftVel;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearRightVel;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants.kDriverControllerPort;
/*
* 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.
*/
@SuppressWarnings("PMD.ExcessiveImports")
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// The driver's controller
XboxController m_driverController = new XboxController(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.getY(GenericHID.Hand.kLeft),
m_driverController.getX(GenericHID.Hand.kRight),
m_driverController.getX(GenericHID.Hand.kLeft), false)));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link 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.kBumperRight.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> 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(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(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
);
MecanumControllerCommand mecanumControllerCommand = new MecanumControllerCommand(
exampleTrajectory,
m_robotDrive::getPose,
kFeedforward,
kDriveKinematics,
//Position contollers
new PIDController(kPXController, 0, 0),
new PIDController(kPYController, 0, 0),
new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints),
//Needed for normalizing wheel speeds
kMaxSpeedMetersPerSecond,
//Velocity PID's
new PIDController(kPFrontLeftVel, 0, 0),
new PIDController(kPRearLeftVel, 0, 0),
new PIDController(kPFrontRightVel, 0, 0),
new PIDController(kPRearRightVel, 0, 0),
m_robotDrive::getCurrentWheelSpeeds,
m_robotDrive::setDriveSpeedControllersVolts, //Consumer for the output motor voltages
m_robotDrive
);
// Run path following command, then stop at the end.
return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
}
}

253
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java Normal file
View File

@@ -0,0 +1,253 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderPorts;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderReversed;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftMotorPort;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderPorts;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderReversed;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightMotorPort;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kGyroReversed;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderPorts;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderReversed;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftMotorPort;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderPorts;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderReversed;
import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightMotorPort;
public class DriveSubsystem extends SubsystemBase {
private final PWMVictorSPX m_frontLeft = new PWMVictorSPX(kFrontLeftMotorPort);
private final PWMVictorSPX m_rearLeft = new PWMVictorSPX(kRearLeftMotorPort);
private final PWMVictorSPX m_frontRight = new PWMVictorSPX(kFrontRightMotorPort);
private final PWMVictorSPX m_rearRight = new PWMVictorSPX(kRearRightMotorPort);
private final MecanumDrive m_drive = new MecanumDrive(
m_frontLeft,
m_rearLeft,
m_frontRight,
m_rearRight);
// The front-left-side drive encoder
private final Encoder m_frontLeftEncoder =
new Encoder(kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1],
kFrontLeftEncoderReversed);
// The rear-left-side drive encoder
private final Encoder m_rearLeftEncoder =
new Encoder(kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1],
kRearLeftEncoderReversed);
// The front-right--side drive encoder
private final Encoder m_frontRightEncoder =
new Encoder(kFrontRightEncoderPorts[0], kFrontRightEncoderPorts[1],
kFrontRightEncoderReversed);
// The rear-right-side drive encoder
private final Encoder m_rearRightEncoder =
new Encoder(kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
kRearRightEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
// Odometry class for tracking robot pose
MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(kDriveKinematics, getAngle());
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_frontLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rearLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_frontRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rearRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
}
/**
* Returns the angle of the robot as a Rotation2d.
*
* @return The angle of the robot.
*/
public Rotation2d getAngle() {
// Negating the angle because WPILib gyros are CW positive.
return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0));
}
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(getAngle(),
new MecanumDriveWheelSpeeds(
m_frontLeftEncoder.getRate(),
m_rearLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_rearRightEncoder.getRate()));
}
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
/**
* 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(pose, getAngle());
}
/**
* 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.
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
if ( fieldRelative ) {
m_drive.driveCartesian(ySpeed, xSpeed, rot, -m_gyro.getAngle());
} else {
m_drive.driveCartesian(ySpeed, xSpeed, rot);
}
}
/**
* Sets the front left drive SpeedController to a voltage.
*/
public void setDriveSpeedControllersVolts(MecanumDriveMotorVoltages volts) {
m_frontLeft.setVoltage(volts.frontLeftVoltage);
m_rearLeft.setVoltage(volts.rearLeftVoltage);
m_frontRight.setVoltage(volts.frontRightVoltage);
m_rearRight.setVoltage(volts.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());
}
/**
* 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 Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
}
/**
* 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() * (kGyroReversed ? -1.0 : 1.0);
}
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
View File

@@ -67,7 +67,7 @@ public final class Constants {
new DifferentialDriveKinematicsConstraint(DriveConstants.kDriveKinematics,
kMaxSpeedMetersPerSecond);
// Reasonable baseline values for a RAMSETE follower in units of meters and seconds
// Reasonable baseline values for a RAMSETE Controller in units of meters and seconds
public static final double kRamseteB = 2;
public static final double kRamseteZeta = 0.7;
}

120
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java Normal file
View File

@@ -0,0 +1,120 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
import edu.wpi.first.wpilibj.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 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[]{5, 6};
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[]{7, 8};
public static final int[] kRearLeftDriveEncoderPorts = new int[]{9, 10};
public static final int[] kFrontRightDriveEncoderPorts = new int[]{11, 12};
public static final int[] kRearRightDriveEncoderPorts = new int[]{13, 14};
public static final boolean kFrontLeftDriveEncoderReversed = false;
public static final boolean kRearLeftDriveEncoderReversed = true;
public static final boolean kFrontRightDriveEncoderReversed = false;
public static final boolean kRearRightDriveEncoderReversed = 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 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 RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
// values for your robot.
public static final double ksVolts = 1;
public static final double kvVoltSecondsPerMeter = 0.8;
public static final double kaVoltSecondsSquaredPerMeter = 0.15;
}
public static final class ModuleConstants {
public static final double kMaxModuleAngularSpeedRadiansPerSecond = 2 * Math.PI;
public static final double kMaxModuleAngularAccelerationRadiansPerSecondSquared = 2 * Math.PI;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterMeters = 0.15;
public static final double kDriveEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
public static final double kTurningEncoderDistancePerPulse =
// Assumes the encoders are on a 1:1 reduction with the module shaft.
(2 * Math.PI) / (double) kEncoderCPR;
public static final double kPModuleTurningController = 1;
public static final double kPModuleDriveController = 1;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 1;
}
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 profilied robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
kMaxAngularSpeedRadiansPerSecondSquared);
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the 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);
}
}

121
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java Normal file
View File

@@ -0,0 +1,121 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the 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 VM is configured to automatically run this class, and to call the functions 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 build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// 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 robot packet, 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) {
m_autonomousCommand.schedule();
}
}
/**
* 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() {
}
}

125
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java Normal file
View File

@@ -0,0 +1,125 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
import java.util.List;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
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 edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPThetaController;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPXController;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPYController;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kThetaControllerConstraints;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants.kDriverControllerPort;
/*
* 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(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.getY(GenericHID.Hand.kLeft),
m_driverController.getX(GenericHID.Hand.kRight),
m_driverController.getX(GenericHID.Hand.kLeft), false)));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link 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(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(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
);
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
exampleTrajectory,
m_robotDrive::getPose, //Functional interface to feed supplier
kDriveKinematics,
//Position controllers
new PIDController(kPXController, 0, 0),
new PIDController(kPYController, 0, 0),
new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints),
m_robotDrive::setModuleStates,
m_robotDrive
);
// Run path following command, then stop at the end.
return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
}
}

200
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java Normal file
View File

@@ -0,0 +1,200 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderPorts;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderReversed;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveMotorPort;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderPorts;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderReversed;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningMotorPort;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderPorts;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderReversed;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveMotorPort;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderPorts;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderReversed;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningMotorPort;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kGyroReversed;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderPorts;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderReversed;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveMotorPort;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderPorts;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderReversed;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningMotorPort;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderPorts;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderReversed;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveMotorPort;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderPorts;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderReversed;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningMotorPort;
@SuppressWarnings("PMD.ExcessiveImports")
public class DriveSubsystem extends SubsystemBase {
//Robot swerve modules
private final SwerveModule m_frontLeft = new SwerveModule(kFrontLeftDriveMotorPort,
kFrontLeftTurningMotorPort,
kFrontLeftDriveEncoderPorts,
kFrontLeftTurningEncoderPorts,
kFrontLeftDriveEncoderReversed,
kFrontLeftTurningEncoderReversed);
private final SwerveModule m_rearLeft = new SwerveModule(kRearLeftDriveMotorPort,
kRearLeftTurningMotorPort,
kRearLeftDriveEncoderPorts,
kRearLeftTurningEncoderPorts,
kRearLeftDriveEncoderReversed,
kRearLeftTurningEncoderReversed);
private final SwerveModule m_frontRight = new SwerveModule(kFrontRightDriveMotorPort,
kFrontRightTurningMotorPort,
kFrontRightDriveEncoderPorts,
kFrontRightTurningEncoderPorts,
kFrontRightDriveEncoderReversed,
kFrontRightTurningEncoderReversed);
private final SwerveModule m_rearRight = new SwerveModule(kRearRightDriveMotorPort,
kRearRightTurningMotorPort,
kRearRightDriveEncoderPorts,
kRearRightTurningEncoderPorts,
kRearRightDriveEncoderReversed,
kRearRightTurningEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
// Odometry class for tracking robot pose
SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(kDriveKinematics, getAngle());
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {}
/**
* Returns the angle of the robot as a Rotation2d.
*
* @return The angle of the robot.
*/
public Rotation2d getAngle() {
// Negating the angle because WPILib gyros are CW positive.
return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0));
}
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
new Rotation2d(getHeading()),
m_frontLeft.getState(),
m_rearLeft.getState(),
m_frontRight.getState(),
m_rearRight.getState());
}
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
/**
* 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(pose, getAngle());
}
/**
* 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.
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
var swerveModuleStates = kDriveKinematics.toSwerveModuleStates(
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, getAngle())
: new ChassisSpeeds(xSpeed, ySpeed, rot)
);
SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
m_rearLeft.setDesiredState(swerveModuleStates[2]);
m_rearRight.setDesiredState(swerveModuleStates[3]);
}
/**
* Sets the swerve ModuleStates.
*
* @param desiredStates The desired SwerveModule states.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates, kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(desiredStates[0]);
m_frontRight.setDesiredState(desiredStates[1]);
m_rearLeft.setDesiredState(desiredStates[2]);
m_rearRight.setDesiredState(desiredStates[3]);
}
/**
* Resets the drive encoders to currently read a position of 0.
*/
public void resetEncoders() {
m_frontLeft.resetEncoders();
m_rearLeft.resetEncoders();
m_frontRight.resetEncoders();
m_rearRight.resetEncoders();
}
/**
* Zeroes the heading of the robot.
*/
public void zeroHeading() {
m_gyro.reset();
}
/**
* Returns the heading of the robot.
*
* @return the robot's heading in degrees, from 180 to 180
*/
public double getHeading() {
return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
}
/**
* 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() * (kGyroReversed ? -1.0 : 1.0);
}
}

119
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java Normal file
View File

@@ -0,0 +1,119 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kDriveEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleDriveController;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleTurningController;
import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kTurningEncoderDistancePerPulse;
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(kPModuleDriveController, 0, 0);
//Using a TrapezoidProfile PIDController to allow for smooth turning
private final ProfiledPIDController m_turningPIDController
= new ProfiledPIDController(kPModuleTurningController, 0, 0,
new TrapezoidProfile.Constraints(kMaxModuleAngularSpeedRadiansPerSecond,
kMaxModuleAngularAccelerationRadiansPerSecondSquared));
/**
* Constructs a SwerveModule.
*
* @param driveMotorChannel ID for the drive motor.
* @param turningMotorChannel ID for the turning motor.
*/
public SwerveModule(int driveMotorChannel,
int turningMotorChannel,
int[] driveEncoderPorts,
int[] turningEncoderPorts,
boolean driveEncoderReversed,
boolean turningEncoderReversed) {
m_driveMotor = new Spark(driveMotorChannel);
m_turningMotor = new Spark(turningMotorChannel);
this.m_driveEncoder = new Encoder(driveEncoderPorts[0], driveEncoderPorts[1]);
this.m_turningEncoder = new Encoder(turningEncoderPorts[0], turningEncoderPorts[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(kDriveEncoderDistancePerPulse);
//Set whether drive encoder should be reversed or not
m_driveEncoder.setReverseDirection(driveEncoderReversed);
// Set the distance (in this case, angle) per pulse for the turning encoder.
// This is the the angle through an entire rotation (2 * wpi::math::pi)
// divided by the encoder resolution.
m_turningEncoder.setDistancePerPulse(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.get()));
}
/**
* Sets the desired state for the module.
*
* @param state Desired state with speed and angle.
*/
public void setDesiredState(SwerveModuleState state) {
// Calculate the drive output from the drive PID controller.
final var driveOutput = m_drivePIDController.calculate(
m_driveEncoder.getRate(), state.speedMetersPerSecond);
// Calculate the turning motor output from the turning PID controller.
final var turnOutput = m_turningPIDController.calculate(
m_turningEncoder.get(), state.angle.getRadians()
);
// Calculate the turning motor output from the turning PID controller.
m_driveMotor.set(driveOutput);
m_turningMotor.set(turnOutput);
}
/**
* Zeros all the SwerveModule encoders.
*/
public void resetEncoders() {
m_driveEncoder.reset();
m_turningEncoder.reset();
}
}