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

[wpimath] Deprecate RamseteController (#6494)

Browse Source 
LTVUnicycleController is a drop-in replacement with better tuning knobs.

The RamseteCommand examples were removed instead of retrofitted with
LTVUnicycleController because we're planning on removing the command
controller classes anyway, so it would be wasted effort. The
SimpleDifferentialDriveSimulation example shows direct
LTVUnicycleController usage.
This commit is contained in:
Tyler Veness
2024-04-29 22:01:42 -07:00
committed by GitHub
parent 7601b7250a
commit 5359112b15
45 changed files with 45 additions and 2929 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -504,25 +504,6 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "RamseteCommand",
"description": "Follow a pre-generated trajectory with a differential drive using RamseteCommand.",
"tags": [
"Differential Drive",
"Command-based",
"Ramsete",
"Trajectory",
"Path Following",
"Odometry",
"Encoder",
"Gyro",
"XboxController"
],
"foldername": "ramsetecommand",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Arcade Drive Xbox Controller",
"description": "Control a differential drive with split-stick arcade drive in teleop.",
@@ -685,24 +666,6 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "RamseteController",
"description": "Follow a pre-generated trajectory with a differential drive using RamseteController.",
"tags": [
"Basic Robot",
"Differential Drive",
"Ramsete",
"PID",
"Odometry",
"Path Following",
"Trajectory",
"XboxController"
],
"foldername": "ramsetecontroller",
"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.",
@@ -786,21 +749,6 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "StateSpaceDriveSimulation",
"description": "Simulate a differential drivetrain and follow trajectories with RamseteCommand (command-based).",
"tags": [
"Command-based",
"Differential Drive",
"State-Space",
"XboxController",
"Simulation"
],
"foldername": "statespacedifferentialdrivesimulation",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "ElevatorSimulation",
"description": "Simulate an elevator.",

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

@@ -1,64 +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.ramsetecommand;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
/**
* 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 kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[] {0, 1};
public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final double kTrackwidthMeters = 0.69;
public static final DifferentialDriveKinematics kDriveKinematics =
new DifferentialDriveKinematics(kTrackwidthMeters);
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;
// 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 Robot Characterization Toolsuite provides a convenient tool for obtaining these
// values for your robot.
public static final double ksVolts = 0.22;
public static final double kvVoltSecondsPerMeter = 1.98;
public static final double kaVoltSecondsSquaredPerMeter = 0.2;
// Example value only - as above, this must be tuned for your drive!
public static final double kPDriveVel = 8.5;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 0;
}
public static final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 1;
// Reasonable baseline values for a RAMSETE follower in units of meters and seconds
public static final double kRamseteB = 2;
public static final double kRamseteZeta = 0.7;
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
View File

@@ -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.ramsetecommand;
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);
}
}

102
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java
View File

@@ -1,102 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.ramsetecommand;
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 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) {
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() {}
}

135
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
View File

@@ -1,135 +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.ramsetecommand;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
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.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.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.RamseteCommand;
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.arcadeDrive(
-m_driverController.getLeftY(), -m_driverController.getRightX()),
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 a voltage constraint to ensure we don't accelerate too fast
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
new SimpleMotorFeedforward(
DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
10);
// Create config for trajectory
TrajectoryConfig config =
new TrajectoryConfig(
AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics)
// Apply the voltage constraint
.addConstraint(autoVoltageConstraint);
// 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)),
// Pass config
config);
RamseteCommand ramseteCommand =
new RamseteCommand(
exampleTrajectory,
m_robotDrive::getPose,
new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
new SimpleMotorFeedforward(
DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
m_robotDrive::getWheelSpeeds,
new PIDController(DriveConstants.kPDriveVel, 0, 0),
new PIDController(DriveConstants.kPDriveVel, 0, 0),
// RamseteCommand passes volts to the callback
m_robotDrive::tankDriveVolts,
m_robotDrive);
// Reset odometry to the initial pose of the trajectory, run path following
// command, then stop at the end.
return Commands.runOnce(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()))
.andThen(ramseteCommand)
.andThen(Commands.runOnce(() -> m_robotDrive.tankDriveVolts(0, 0)));
}
}

195
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
View File

@@ -1,195 +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.ramsetecommand.subsystems;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
// The gyro sensor
private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
// Odometry class for tracking robot pose
private final DifferentialDriveOdometry m_odometry;
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
resetEncoders();
m_odometry =
new DifferentialDriveOdometry(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
/**
* Returns the current wheel speeds of the robot.
*
* @return The current wheel speeds.
*/
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
}
/**
* 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(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/**
* Controls the left and right sides of the drive directly with voltages.
*
* @param leftVolts the commanded left output
* @param rightVolts the commanded right output
*/
public void tankDriveVolts(double leftVolts, double rightVolts) {
m_leftLeader.setVoltage(leftVolts);
m_rightLeader.setVoltage(rightVolts);
m_drive.feed();
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* 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();
}
}

129
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
View File

@@ -1,129 +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.ramsetecontroller;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/** Represents a differential drive style drivetrain. */
public class Drivetrain {
public static final double kMaxSpeed = 3.0; // meters per second
public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
private static final double kTrackWidth = 0.381 * 2; // meters
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
private final DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(kTrackWidth);
private final DifferentialDriveOdometry m_odometry;
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
/**
* Constructs a differential drive object. Sets the encoder distance per pulse and resets the
* gyro.
*/
public Drivetrain() {
m_gyro.reset();
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_leftEncoder.reset();
m_rightEncoder.reset();
m_odometry =
new DifferentialDriveOdometry(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/**
* Sets the desired wheel speeds.
*
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
}
/**
* Drives the robot with the given linear velocity and angular velocity.
*
* @param xSpeed Linear velocity in m/s.
* @param rot Angular velocity in rad/s.
*/
public void drive(double xSpeed, double rot) {
var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
setSpeeds(wheelSpeeds);
}
/** Updates the field-relative position. */
public void updateOdometry() {
m_odometry.update(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/**
* Resets the field-relative position to a specific location.
*
* @param pose The position to reset to.
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
}
/**
* Returns the pose of the robot.
*
* @return The pose of the robot.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java
View File

@@ -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.ramsetecontroller;
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);
}
}

108
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
View File

@@ -1,108 +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.ramsetecontroller;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.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.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.List;
public class Robot extends TimedRobot {
private final XboxController m_controller = new XboxController(0);
private final Drivetrain m_drive = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
// An example trajectory to follow during the autonomous period.
private Trajectory m_trajectory;
// The Ramsete Controller to follow the trajectory.
private final RamseteController m_ramseteController = new RamseteController();
// The timer to use during the autonomous period.
private Timer m_timer;
// Create Field2d for robot and trajectory visualizations.
private Field2d m_field;
@Override
public void robotInit() {
// Create the trajectory to follow in autonomous. It is best to initialize
// trajectories here to avoid wasting time in autonomous.
m_trajectory =
TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)));
// Create and push Field2d to SmartDashboard.
m_field = new Field2d();
SmartDashboard.putData(m_field);
// Push the trajectory to Field2d.
m_field.getObject("traj").setTrajectory(m_trajectory);
}
@Override
public void autonomousInit() {
// Initialize the timer.
m_timer = new Timer();
m_timer.start();
// Reset the drivetrain's odometry to the starting pose of the trajectory.
m_drive.resetOdometry(m_trajectory.getInitialPose());
}
@Override
public void autonomousPeriodic() {
// Update odometry.
m_drive.updateOdometry();
// Update robot position on Field2d.
m_field.setRobotPose(m_drive.getPose());
if (m_timer.get() < m_trajectory.getTotalTimeSeconds()) {
// Get the desired pose from the trajectory.
var desiredPose = m_trajectory.sample(m_timer.get());
// Get the reference chassis speeds from the Ramsete controller.
var refChassisSpeeds = m_ramseteController.calculate(m_drive.getPose(), desiredPose);
// Set the linear and angular speeds.
m_drive.drive(refChassisSpeeds.vxMetersPerSecond, refChassisSpeeds.omegaRadiansPerSecond);
} else {
m_drive.drive(0, 0);
}
}
@Override
public void teleopPeriodic() {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
final var xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
m_drive.drive(xSpeed, rot);
}
}

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.LTVUnicycleController;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -26,7 +26,7 @@ public class Robot extends TimedRobot {
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final Drivetrain m_drive = new Drivetrain();
private final RamseteController m_ramsete = new RamseteController();
private final LTVUnicycleController m_feedback = new LTVUnicycleController(0.020);
private final Timer m_timer = new Timer();
private Trajectory m_trajectory;
@@ -55,7 +55,7 @@ public class Robot extends TimedRobot {
public void autonomousPeriodic() {
double elapsed = m_timer.get();
Trajectory.State reference = m_trajectory.sample(elapsed);
ChassisSpeeds speeds = m_ramsete.calculate(m_drive.getPose(), reference);
ChassisSpeeds speeds = m_feedback.calculate(m_drive.getPose(), reference);
m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
}

88
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
View File

@@ -1,88 +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.statespacedifferentialdrivesimulation;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
/**
* 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 kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[] {0, 1};
public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final double kTrackwidthMeters = 0.69;
public static final DifferentialDriveKinematics kDriveKinematics =
new DifferentialDriveKinematics(kTrackwidthMeters);
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 = true;
// 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 Robot Characterization Toolsuite provides a convenient tool for obtaining these
// values for your robot.
public static final double ksVolts = 0.22;
public static final double kvVoltSecondsPerMeter = 1.98;
public static final double kaVoltSecondsSquaredPerMeter = 0.2;
// 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.
// These two values are "angular" kV and kA
public static final double kvVoltSecondsPerRadian = 1.5;
public static final double kaVoltSecondsSquaredPerRadian = 0.3;
public static final LinearSystem<N2, N2, N2> kDrivetrainPlant =
LinearSystemId.identifyDrivetrainSystem(
kvVoltSecondsPerMeter,
kaVoltSecondsSquaredPerMeter,
kvVoltSecondsPerRadian,
kaVoltSecondsSquaredPerRadian);
// Example values only -- use what's on your physical robot!
public static final DCMotor kDriveGearbox = DCMotor.getCIM(2);
public static final double kDriveGearing = 8;
// Example value only - as above, this must be tuned for your drive!
public static final double kPDriveVel = 8.5;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 0;
}
public static final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
// Reasonable baseline values for a RAMSETE follower in units of meters and seconds
public static final double kRamseteB = 2;
public static final double kRamseteZeta = 0.7;
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java
View File

@@ -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.statespacedifferentialdrivesimulation;
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);
}
}

57
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
View File

@@ -1,57 +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.statespacedifferentialdrivesimulation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* This is a sample program to demonstrate the use of state-space classes in robot simulation. This
* robot has a flywheel, elevator, arm and differential drivetrain, and interfaces with the sim
* GUI's {@link edu.wpi.first.wpilibj.simulation.Field2d} class.
*/
public class Robot extends TimedRobot {
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();
}
@Override
public void simulationPeriodic() {
// Here we calculate the battery voltage based on drawn current.
// As our robot draws more power from the battery its voltage drops.
// The estimated voltage is highly dependent on the battery's internal
// resistance.
double drawCurrent = m_robotContainer.getRobotDrive().getDrawnCurrentAmps();
double loadedVoltage = BatterySim.calculateDefaultBatteryLoadedVoltage(drawCurrent);
RoboRioSim.setVInVoltage(loadedVoltage);
}
@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();
}
@Override
public void autonomousInit() {
m_robotContainer.getAutonomousCommand().schedule();
}
@Override
public void disabledInit() {
CommandScheduler.getInstance().cancelAll();
m_robotContainer.zeroAllOutputs();
}
}

145
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
View File

@@ -1,145 +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.statespacedifferentialdrivesimulation;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
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.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
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 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(Constants.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.
// If you are using the keyboard as a joystick, it is recommended that you go
// to the following link to read about editing the keyboard settings.
// https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/simulation-gui.html#using-the-keyboard-as-a-joystick
new RunCommand(
() ->
m_robotDrive.arcadeDrive(
-m_driverController.getLeftY(), -m_driverController.getRightX()),
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)));
}
public DriveSubsystem getRobotDrive() {
return m_robotDrive;
}
/** Zeros the outputs of all subsystems. */
public void zeroAllOutputs() {
m_robotDrive.tankDriveVolts(0, 0);
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Create a voltage constraint to ensure we don't accelerate too fast
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
new SimpleMotorFeedforward(
Constants.DriveConstants.ksVolts,
Constants.DriveConstants.kvVoltSecondsPerMeter,
Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
Constants.DriveConstants.kDriveKinematics,
7);
// Create config for trajectory
TrajectoryConfig config =
new TrajectoryConfig(
Constants.AutoConstants.kMaxSpeedMetersPerSecond,
Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(Constants.DriveConstants.kDriveKinematics)
// Apply the voltage constraint
.addConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory =
TrajectoryGenerator.generateTrajectory(
// Start at (1, 2) facing the +X direction
new Pose2d(1, 2, new Rotation2d(0)),
// Pass through these two interior waypoints, making an 's' curve path
List.of(new Translation2d(2, 3), new Translation2d(3, 1)),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(4, 2, new Rotation2d(0)),
// Pass config
config);
RamseteCommand ramseteCommand =
new RamseteCommand(
exampleTrajectory,
m_robotDrive::getPose,
new RamseteController(
Constants.AutoConstants.kRamseteB, Constants.AutoConstants.kRamseteZeta),
new SimpleMotorFeedforward(
Constants.DriveConstants.ksVolts,
Constants.DriveConstants.kvVoltSecondsPerMeter,
Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
Constants.DriveConstants.kDriveKinematics,
m_robotDrive::getWheelSpeeds,
new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
// RamseteCommand passes volts to the callback
m_robotDrive::tankDriveVolts,
m_robotDrive);
// Reset odometry to starting pose of trajectory.
m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
// Run path following command, then stop at the end.
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
}
}

267
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
View File

@@ -1,267 +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.statespacedifferentialdrivesimulation.subsystems;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
// The gyro sensor
private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
// Odometry class for tracking robot pose
private final DifferentialDriveOdometry m_odometry;
// These classes help us simulate our drivetrain
public DifferentialDrivetrainSim m_drivetrainSimulator;
private final EncoderSim m_leftEncoderSim;
private final EncoderSim m_rightEncoderSim;
// The Field2d class shows the field in the sim GUI
private final Field2d m_fieldSim;
private final ADXRS450_GyroSim m_gyroSim;
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
resetEncoders();
m_odometry =
new DifferentialDriveOdometry(
Rotation2d.fromDegrees(getHeading()),
m_leftEncoder.getDistance(),
m_rightEncoder.getDistance());
if (RobotBase.isSimulation()) { // If our robot is simulated
// This class simulates our drivetrain's motion around the field.
m_drivetrainSimulator =
new DifferentialDrivetrainSim(
DriveConstants.kDrivetrainPlant,
DriveConstants.kDriveGearbox,
DriveConstants.kDriveGearing,
DriveConstants.kTrackwidthMeters,
DriveConstants.kWheelDiameterMeters / 2.0,
VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
// The encoder and gyro angle sims let us set simulated sensor readings
m_leftEncoderSim = new EncoderSim(m_leftEncoder);
m_rightEncoderSim = new EncoderSim(m_rightEncoder);
m_gyroSim = new ADXRS450_GyroSim(m_gyro);
// the Field2d class lets us visualize our robot in the simulation GUI.
m_fieldSim = new Field2d();
SmartDashboard.putData("Field", m_fieldSim);
} else {
m_leftEncoderSim = null;
m_rightEncoderSim = null;
m_gyroSim = null;
m_fieldSim = null;
}
}
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
Rotation2d.fromDegrees(getHeading()),
m_leftEncoder.getDistance(),
m_rightEncoder.getDistance());
m_fieldSim.setRobotPose(getPose());
}
@Override
public void simulationPeriodic() {
// To update our simulation, we set motor voltage inputs, update the simulation,
// and write the simulated positions and velocities to our simulated encoder and gyro.
// We negate the right side so that positive voltages make the right side
// move forward.
m_drivetrainSimulator.setInputs(
m_leftLeader.get() * RobotController.getBatteryVoltage(),
m_rightLeader.get() * RobotController.getBatteryVoltage());
m_drivetrainSimulator.update(0.020);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
}
/**
* Returns the current being drawn by the drivetrain. This works in SIMULATION ONLY! If you want
* it to work elsewhere, use the code in {@link DifferentialDrivetrainSim#getCurrentDrawAmps()}
*
* @return The drawn current in Amps.
*/
public double getDrawnCurrentAmps() {
return m_drivetrainSimulator.getCurrentDrawAmps();
}
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
/**
* Returns the current wheel speeds of the robot.
*
* @return The current wheel speeds.
*/
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
}
/**
* Resets the odometry to the specified pose.
*
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_drivetrainSimulator.setPose(pose);
m_odometry.resetPosition(
Rotation2d.fromDegrees(getHeading()),
m_leftEncoder.getDistance(),
m_rightEncoder.getDistance(),
pose);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/**
* Controls the left and right sides of the drive directly with voltages.
*
* @param leftVolts the commanded left output
* @param rightVolts the commanded right output
*/
public void tankDriveVolts(double leftVolts, double rightVolts) {
m_leftLeader.setVoltage(leftVolts);
m_rightLeader.setVoltage(rightVolts);
m_drive.feed();
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* 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) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
}