Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

4
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
View File

@@ -69,7 +69,9 @@ public class RamseteCommand extends Command {
* @param outputVolts A function that consumes the computed left and right outputs (in volts) for
* the robot drive.
* @param requirements The subsystems to require.
* @deprecated Use LTVUnicycleController instead.
*/
@Deprecated(since = "2024", forRemoval = true)
@SuppressWarnings("this-escape")
public RamseteCommand(
Trajectory trajectory,
@@ -109,7 +111,9 @@ public class RamseteCommand extends Command {
* @param kinematics The kinematics for the robot drivetrain.
* @param outputMetersPerSecond A function that consumes the computed left and right wheel speeds.
* @param requirements The subsystems to require.
* @deprecated Use LTVUnicycleController instead.
*/
@Deprecated(since = "2024", forRemoval = true)
@SuppressWarnings("this-escape")
public RamseteCommand(
Trajectory trajectory,

7
wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
View File

@@ -69,7 +69,9 @@ class RamseteCommand : public CommandHelper<Command, RamseteCommand> {
* @param output A function that consumes the computed left and right
* outputs (in volts) for the robot drive.
* @param requirements The subsystems to require.
* @deprecated Use LTVUnicycleController instead.
*/
[[deprecated("Use LTVUnicycleController instead.")]]
RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::RamseteController controller,
frc::SimpleMotorFeedforward<units::meters> feedforward,
@@ -95,13 +97,14 @@ class RamseteCommand : public CommandHelper<Command, RamseteCommand> {
* @param output A function that consumes the computed left and right
* wheel speeds.
* @param requirements The subsystems to require.
* @deprecated Use LTVUnicycleController instead.
*/
[[deprecated("Use LTVUnicycleController instead.")]]
RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::RamseteController controller,
frc::DifferentialDriveKinematics kinematics,
std::function<void(units::meters_per_second_t,
units::meters_per_second_t)>
output,
units::meters_per_second_t)> output,
Requirements requirements = {});
void Initialize() override;

8
wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
View File

@@ -7,8 +7,8 @@
#include <units/math.h>
#include <units/moment_of_inertia.h>
#include "frc/controller/LTVUnicycleController.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/RamseteController.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/simulation/DifferentialDrivetrainSim.h"
#include "frc/system/NumericalIntegration.h"
@@ -28,7 +28,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
1.0, 2_in, {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms};
frc::RamseteController ramsete;
frc::LTVUnicycleController feedback{20_ms};
feedforward.Reset(frc::Vectord<2>{0.0, 0.0});
@@ -44,9 +44,9 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
for (auto t = 0_s; t < trajectory.TotalTime(); t += 20_ms) {
auto state = trajectory.Sample(t);
auto ramseteOut = ramsete.Calculate(sim.GetPose(), state);
auto feedbackOut = feedback.Calculate(sim.GetPose(), state);
auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
auto [l, r] = kinematics.ToWheelSpeeds(feedbackOut);
auto voltages =
feedforward.Calculate(frc::Vectord<2>{l.value(), r.value()});

10
wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp
View File

@@ -1,10 +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.
#include "Constants.h"
using namespace DriveConstants;
const frc::DifferentialDriveKinematics DriveConstants::kDriveKinematics(
kTrackwidth);

72
wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp
View File

@@ -1,72 +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.
#include "Robot.h"
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc2/command/CommandScheduler.h>
void Robot::RobotInit() {}
/**
* This function is called every 20 ms, no matter the mode. Use
* this for items like diagnostics that you want to run during disabled,
* autonomous, teleoperated and test.
*
* <p> This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
void Robot::RobotPeriodic() {
frc2::CommandScheduler::GetInstance().Run();
}
/**
* This function is called once each time the robot enters Disabled mode. You
* can use it to reset any subsystem information you want to clear when the
* robot is disabled.
*/
void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}
/**
* This autonomous runs the autonomous command selected by your {@link
* RobotContainer} class.
*/
void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
if (m_autonomousCommand) {
m_autonomousCommand->Schedule();
}
}
void Robot::AutonomousPeriodic() {}
void Robot::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) {
m_autonomousCommand->Cancel();
m_autonomousCommand.reset();
}
}
/**
* This function is called periodically during operator control.
*/
void Robot::TeleopPeriodic() {}
/**
* This function is called periodically during test mode.
*/
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
}
#endif

94
wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
View File

@@ -1,94 +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.
#include "RobotContainer.h"
#include <utility>
#include <frc/controller/PIDController.h>
#include <frc/controller/RamseteController.h>
#include <frc/shuffleboard/Shuffleboard.h>
#include <frc/trajectory/Trajectory.h>
#include <frc/trajectory/TrajectoryGenerator.h>
#include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
#include <frc2/command/Commands.h>
#include <frc2/command/RamseteCommand.h>
#include <frc2/command/button/JoystickButton.h>
#include "Constants.h"
RobotContainer::RobotContainer() {
// Initialize all of your commands and subsystems here
// Configure the button bindings
ConfigureButtonBindings();
// Set up default drive command
m_drive.SetDefaultCommand(frc2::cmd::Run(
[this] {
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
-m_driverController.GetRightX());
},
{&m_drive}));
}
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
// While holding the shoulder button, drive at half speed
frc2::JoystickButton{&m_driverController, 6}
.OnTrue(&m_driveHalfSpeed)
.OnFalse(&m_driveFullSpeed);
}
frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
// Create a voltage constraint to ensure we don't accelerate too fast
frc::DifferentialDriveVoltageConstraint autoVoltageConstraint{
frc::SimpleMotorFeedforward<units::meters>{
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka},
DriveConstants::kDriveKinematics, 10_V};
// Set up config for trajectory
frc::TrajectoryConfig config{AutoConstants::kMaxSpeed,
AutoConstants::kMaxAcceleration};
// Add kinematics to ensure max speed is actually obeyed
config.SetKinematics(DriveConstants::kDriveKinematics);
// Apply the voltage constraint
config.AddConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
// Start at the origin facing the +X direction
frc::Pose2d{0_m, 0_m, 0_deg},
// Pass through these two interior waypoints, making an 's' curve path
{frc::Translation2d{1_m, 1_m}, frc::Translation2d{2_m, -1_m}},
// End 3 meters straight ahead of where we started, facing forward
frc::Pose2d{3_m, 0_m, 0_deg},
// Pass the config
config);
frc2::CommandPtr ramseteCommand{frc2::RamseteCommand(
exampleTrajectory, [this] { return m_drive.GetPose(); },
frc::RamseteController{AutoConstants::kRamseteB,
AutoConstants::kRamseteZeta},
frc::SimpleMotorFeedforward<units::meters>{
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka},
DriveConstants::kDriveKinematics,
[this] { return m_drive.GetWheelSpeeds(); },
frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
[this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
{&m_drive})};
// Reset odometry to the initial pose of the trajectory, run path following
// command, then stop at the end.
return frc2::cmd::RunOnce(
[this, initialPose = exampleTrajectory.InitialPose()] {
m_drive.ResetOdometry(initialPose);
},
{})
.AndThen(std::move(ramseteCommand))
.AndThen(
frc2::cmd::RunOnce([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
}

97
wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -1,97 +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.
#include "subsystems/DriveSubsystem.h"
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
using namespace DriveConstants;
DriveSubsystem::DriveSubsystem()
: m_left1{kLeftMotor1Port},
m_left2{kLeftMotor2Port},
m_right1{kRightMotor1Port},
m_right2{kRightMotor2Port},
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
m_left1.AddFollower(m_left2);
m_right1.AddFollower(m_right2);
// 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_right1.SetInverted(true);
// Set the distance per pulse for the encoders
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
ResetEncoders();
}
void DriveSubsystem::Periodic() {
// Implementation of subsystem periodic method goes here.
m_odometry.Update(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()});
}
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
m_drive.ArcadeDrive(fwd, rot);
}
void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
m_left1.SetVoltage(left);
m_right1.SetVoltage(right);
m_drive.Feed();
}
void DriveSubsystem::ResetEncoders() {
m_leftEncoder.Reset();
m_rightEncoder.Reset();
}
double DriveSubsystem::GetAverageEncoderDistance() {
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() {
return m_leftEncoder;
}
frc::Encoder& DriveSubsystem::GetRightEncoder() {
return m_rightEncoder;
}
void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
units::degree_t DriveSubsystem::GetHeading() const {
return m_gyro.GetRotation2d().Degrees();
}
double DriveSubsystem::GetTurnRate() {
return -m_gyro.GetRate();
}
frc::Pose2d DriveSubsystem::GetPose() {
return m_odometry.GetPose();
}
frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
return {units::meters_per_second_t{m_leftEncoder.GetRate()},
units::meters_per_second_t{m_rightEncoder.GetRate()}};
}
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}, pose);
}

72
wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
View File

@@ -1,72 +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.
#include <numbers>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
#include <units/voltage.h>
#pragma once
/**
* The Constants header provides a convenient place for teams to hold robot-wide
* numerical or bool constants. This should not be used for any other purpose.
*
* It is generally a good idea to place constants into subsystem- or
* command-specific namespaces within this header, which can then be used where
* they are needed.
*/
namespace DriveConstants {
inline constexpr int kLeftMotor1Port = 0;
inline constexpr int kLeftMotor2Port = 1;
inline constexpr int kRightMotor1Port = 2;
inline constexpr int kRightMotor2Port = 3;
inline constexpr int kLeftEncoderPorts[]{0, 1};
inline constexpr int kRightEncoderPorts[]{2, 3};
inline constexpr bool kLeftEncoderReversed = false;
inline constexpr bool kRightEncoderReversed = true;
inline constexpr auto kTrackwidth = 0.69_m;
extern const frc::DifferentialDriveKinematics kDriveKinematics;
inline constexpr int kEncoderCPR = 1024;
inline constexpr units::meter_t kWheelDiameter = 6_in;
inline constexpr auto kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameter * std::numbers::pi) / static_cast<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.
inline constexpr auto ks = 0.22_V;
inline constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
inline constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
// Example value only - as above, this must be tuned for your drive!
inline constexpr double kPDriveVel = 8.5;
} // namespace DriveConstants
namespace AutoConstants {
inline constexpr auto kMaxSpeed = 3_mps;
inline constexpr auto kMaxAcceleration = 1_mps_sq;
// Reasonable baseline values for a RAMSETE follower in units of meters and
// seconds
inline constexpr auto kRamseteB = 2.0 * 1_rad * 1_rad / (1_m * 1_m);
inline constexpr auto kRamseteZeta = 0.7 / 1_rad;
} // namespace AutoConstants
namespace OIConstants {
inline constexpr int kDriverControllerPort = 0;
} // namespace OIConstants

31
wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h
View File

@@ -1,31 +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.
#pragma once
#include <optional>
#include <frc/TimedRobot.h>
#include <frc2/command/CommandPtr.h>
#include "RobotContainer.h"
class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
void DisabledInit() override;
void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
void TeleopPeriodic() override;
void TestPeriodic() override;
private:
// Have it empty by default
std::optional<frc2::CommandPtr> m_autonomousCommand;
RobotContainer m_container;
};

45
wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
View File

@@ -1,45 +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.
#pragma once
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/smartdashboard/SendableChooser.h>
#include <frc2/command/CommandPtr.h>
#include <frc2/command/InstantCommand.h>
#include "Constants.h"
#include "subsystems/DriveSubsystem.h"
/**
* 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.
*/
class RobotContainer {
public:
RobotContainer();
frc2::CommandPtr GetAutonomousCommand();
private:
// The driver's controller
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
// The robot's subsystems and commands are defined here...
// The robot's subsystems
DriveSubsystem m_drive;
// RobotContainer-owned commands
frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
void ConfigureButtonBindings();
};

139
wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
View File

@@ -1,139 +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.
#pragma once
#include <frc/ADXRS450_Gyro.h>
#include <frc/Encoder.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <frc2/command/SubsystemBase.h>
#include <units/voltage.h>
#include "Constants.h"
class DriveSubsystem : public frc2::SubsystemBase {
public:
DriveSubsystem();
/**
* Will be called periodically whenever the CommandScheduler runs.
*/
void Periodic() override;
// Subsystem methods go here.
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
void ArcadeDrive(double fwd, double rot);
/**
* Controls each side of the drive directly with a voltage.
*
* @param left the commanded left output
* @param right the commanded right output
*/
void TankDriveVolts(units::volt_t left, units::volt_t right);
/**
* Resets the drive encoders to currently read a position of 0.
*/
void ResetEncoders();
/**
* Gets the average distance of the TWO encoders.
*
* @return the average of the TWO encoder readings
*/
double GetAverageEncoderDistance();
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
frc::Encoder& GetLeftEncoder();
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
frc::Encoder& GetRightEncoder();
/**
* 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
*/
void SetMaxOutput(double maxOutput);
/**
* Returns the heading of the robot.
*
* @return the robot's heading in degrees, from -180 to 180
*/
units::degree_t GetHeading() const;
/**
* Returns the turn rate of the robot.
*
* @return The turn rate of the robot, in degrees per second
*/
double GetTurnRate();
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
frc::Pose2d GetPose();
/**
* Returns the current wheel speeds of the robot.
*
* @return The current wheel speeds.
*/
frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
/**
* Resets the odometry to the specified pose.
*
* @param pose The pose to which to set the odometry.
*/
void ResetOdometry(frc::Pose2d pose);
private:
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
// The motor controllers
frc::PWMSparkMax m_left1;
frc::PWMSparkMax m_left2;
frc::PWMSparkMax m_right1;
frc::PWMSparkMax m_right2;
// The robot's drive
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
[&](double output) { m_right1.Set(output); }};
// The left-side drive encoder
frc::Encoder m_leftEncoder;
// The right-side drive encoder
frc::Encoder m_rightEncoder;
// The gyro sensor
frc::ADXRS450_Gyro m_gyro;
// Odometry class for tracking robot pose
frc::DifferentialDriveOdometry m_odometry;
};

38
wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
View File

@@ -1,38 +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.
#include "Drivetrain.h"
void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
const double leftOutput = m_leftPIDController.Calculate(
m_leftEncoder.GetRate(), speeds.left.value());
const double rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), speeds.right.value());
m_leftLeader.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
}
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot) {
SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()});
}
void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}, pose);
}
frc::Pose2d Drivetrain::GetPose() const {
return m_odometry.GetPose();
}

100
wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp
View File

@@ -1,100 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <frc/TimedRobot.h>
#include <frc/Timer.h>
#include <frc/XboxController.h>
#include <frc/controller/RamseteController.h>
#include <frc/filter/SlewRateLimiter.h>
#include <frc/smartdashboard/Field2d.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc/trajectory/TrajectoryGenerator.h>
#include "Drivetrain.h"
class Robot : public frc::TimedRobot {
public:
void AutonomousInit() override {
// Start the timer.
m_timer.Start();
// Send Field2d to SmartDashboard.
frc::SmartDashboard::PutData(&m_field);
// Reset the drivetrain's odometry to the starting pose of the trajectory.
m_drive.ResetOdometry(m_trajectory.InitialPose());
// Send our generated trajectory to Field2d.
m_field.GetObject("traj")->SetTrajectory(m_trajectory);
}
void AutonomousPeriodic() override {
// Update odometry.
m_drive.UpdateOdometry();
// Update robot position on Field2d.
m_field.SetRobotPose(m_drive.GetPose());
if (m_timer.Get() < m_trajectory.TotalTime()) {
// Get the desired pose from the trajectory.
auto desiredPose = m_trajectory.Sample(m_timer.Get());
// Get the reference chassis speeds from the Ramsete Controller.
auto refChassisSpeeds =
m_ramseteController.Calculate(m_drive.GetPose(), desiredPose);
// Set the linear and angular speeds.
m_drive.Drive(refChassisSpeeds.vx, refChassisSpeeds.omega);
} else {
m_drive.Drive(0_mps, 0_rad_per_s);
}
}
void TeleopPeriodic() override {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
const auto 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.
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
m_drive.Drive(xSpeed, rot);
}
private:
frc::XboxController m_controller{0};
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
Drivetrain m_drive;
// An example trajectory to follow.
frc::Trajectory m_trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d{0_m, 0_m, 0_rad},
{frc::Translation2d{1_m, 1_m}, frc::Translation2d{2_m, -1_m}},
frc::Pose2d{3_m, 0_m, 0_rad}, frc::TrajectoryConfig(3_fps, 3_fps_sq));
// The Ramsete Controller to follow the trajectory.
frc::RamseteController m_ramseteController;
// The timer to use during the autonomous period.
frc::Timer m_timer;
// Create Field2d for robot and trajectory visualizations.
frc::Field2d m_field;
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
}
#endif

85
wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
View File

@@ -1,85 +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.
#pragma once
#include <numbers>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <units/angular_velocity.h>
#include <units/length.h>
/**
* Represents a differential drive style drivetrain.
*/
class Drivetrain {
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 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
m_rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
m_leftEncoder.Reset();
m_rightEncoder.Reset();
}
static constexpr units::meters_per_second_t kMaxSpeed =
3.0_mps; // 3 meters per second
static constexpr units::radians_per_second_t kMaxAngularSpeed{
std::numbers::pi}; // 1/2 rotation per second
void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
void Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot);
void UpdateOdometry();
void ResetOdometry(const frc::Pose2d& pose);
frc::Pose2d GetPose() const;
private:
static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
static constexpr double kWheelRadius = 0.0508; // meters
static constexpr int kEncoderResolution = 4096;
frc::PWMSparkMax m_leftLeader{1};
frc::PWMSparkMax m_leftFollower{2};
frc::PWMSparkMax m_rightLeader{3};
frc::PWMSparkMax m_rightFollower{4};
frc::Encoder m_leftEncoder{0, 1};
frc::Encoder m_rightEncoder{2, 3};
frc::PIDController m_leftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_rightPIDController{1.0, 0.0, 0.0};
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!
frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
};

6
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp
View File

@@ -5,7 +5,7 @@
#include <frc/TimedRobot.h>
#include <frc/Timer.h>
#include <frc/XboxController.h>
#include <frc/controller/RamseteController.h>
#include <frc/controller/LTVUnicycleController.h>
#include <frc/filter/SlewRateLimiter.h>
#include <frc/trajectory/TrajectoryGenerator.h>
@@ -29,7 +29,7 @@ class Robot : public frc::TimedRobot {
void AutonomousPeriodic() override {
auto elapsed = m_timer.Get();
auto reference = m_trajectory.Sample(elapsed);
auto speeds = m_ramsete.Calculate(m_drive.GetPose(), reference);
auto speeds = m_feedback.Calculate(m_drive.GetPose(), reference);
m_drive.Drive(speeds.vx, speeds.omega);
}
@@ -61,7 +61,7 @@ class Robot : public frc::TimedRobot {
Drivetrain m_drive;
frc::Trajectory m_trajectory;
frc::RamseteController m_ramsete;
frc::LTVUnicycleController m_feedback{20_ms};
frc::Timer m_timer;
};

13
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp
View File

@@ -1,13 +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.
#include "Constants.h"
using namespace DriveConstants;
const frc::DifferentialDriveKinematics DriveConstants::kDriveKinematics(
kTrackwidth);
const frc::LinearSystem<2, 2, 2> DriveConstants::kDrivetrainPlant =
frc::LinearSystemId::IdentifyDrivetrainSystem(kv, ka, kvAngular, kaAngular);

87
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp
View File

@@ -1,87 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "Robot.h"
#include <frc/simulation/BatterySim.h>
#include <frc/simulation/RoboRioSim.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc2/command/CommandScheduler.h>
void Robot::RobotInit() {}
/**
* This function is called every 20 ms, no matter the mode. Use
* this for items like diagnostics that you want to run during disabled,
* autonomous, teleoperated and test.
*
* <p> This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
void Robot::RobotPeriodic() {
frc2::CommandScheduler::GetInstance().Run();
}
/**
* This function is called once each time the robot enters Disabled mode. You
* can use it to reset any subsystem information you want to clear when the
* robot is disabled.
*/
void Robot::DisabledInit() {
frc2::CommandScheduler::GetInstance().CancelAll();
m_container.ZeroAllOutputs();
}
void Robot::DisabledPeriodic() {}
/**
* This autonomous runs the autonomous command selected by your {@link
* RobotContainer} class.
*/
void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
if (m_autonomousCommand != nullptr) {
m_autonomousCommand->Schedule();
}
}
void Robot::AutonomousPeriodic() {}
void Robot::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 != nullptr) {
m_autonomousCommand->Cancel();
m_autonomousCommand = nullptr;
}
}
/**
* This function is called periodically during operator control.
*/
void Robot::TeleopPeriodic() {}
/**
* This function is called periodically during test mode.
*/
void Robot::TestPeriodic() {}
void Robot::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.
auto current = m_container.GetRobotDrive().GetCurrentDraw();
auto loadedVoltage = frc::sim::BatterySim::Calculate({current});
frc::sim::RoboRioSim::SetVInVoltage(loadedVoltage);
}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
}
#endif

106
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
View File

@@ -1,106 +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.
#include "RobotContainer.h"
#include <utility>
#include <frc/controller/PIDController.h>
#include <frc/controller/RamseteController.h>
#include <frc/shuffleboard/Shuffleboard.h>
#include <frc/trajectory/Trajectory.h>
#include <frc/trajectory/TrajectoryGenerator.h>
#include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
#include <frc2/command/InstantCommand.h>
#include <frc2/command/RamseteCommand.h>
#include <frc2/command/SequentialCommandGroup.h>
#include <frc2/command/button/JoystickButton.h>
#include "Constants.h"
RobotContainer::RobotContainer() {
// Initialize all of your commands and subsystems here
// Configure the button bindings
ConfigureButtonBindings();
// Set up default drive command
// 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
m_drive.SetDefaultCommand(frc2::RunCommand(
[this] {
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
-m_driverController.GetLeftX());
},
{&m_drive}));
}
void RobotContainer::ZeroAllOutputs() {
m_drive.TankDriveVolts(0_V, 0_V);
}
const DriveSubsystem& RobotContainer::GetRobotDrive() const {
return m_drive;
}
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
// While holding the shoulder button, drive at half speed
frc2::JoystickButton(&m_driverController,
frc::XboxController::Button::kRightBumper)
.OnTrue(&m_driveHalfSpeed)
.OnFalse(&m_driveFullSpeed);
}
frc2::Command* RobotContainer::GetAutonomousCommand() {
// Create a voltage constraint to ensure we don't accelerate too fast
frc::DifferentialDriveVoltageConstraint autoVoltageConstraint(
frc::SimpleMotorFeedforward<units::meters>(
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
DriveConstants::kDriveKinematics, 10_V);
// Set up config for trajectory
frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
AutoConstants::kMaxAcceleration);
// Add kinematics to ensure max speed is actually obeyed
config.SetKinematics(DriveConstants::kDriveKinematics);
// Apply the voltage constraint
config.AddConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
// Start at (1, 2) facing the +X direction
frc::Pose2d{1_m, 2_m, 0_deg},
// Pass through these two interior waypoints, making an 's' curve path
{frc::Translation2d{2_m, 3_m}, frc::Translation2d{3_m, 1_m}},
// End 3 meters straight ahead of where we started, facing forward
frc::Pose2d{4_m, 2_m, 0_deg},
// Pass the config
config);
frc2::RamseteCommand ramseteCommand(
exampleTrajectory, [this] { return m_drive.GetPose(); },
frc::RamseteController{AutoConstants::kRamseteB,
AutoConstants::kRamseteZeta},
frc::SimpleMotorFeedforward<units::meters>(
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
DriveConstants::kDriveKinematics,
[this] { return m_drive.GetWheelSpeeds(); },
frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
[this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
{&m_drive});
// Reset odometry to starting pose of trajectory.
m_drive.ResetOdometry(exampleTrajectory.InitialPose());
// no auto
return new frc2::SequentialCommandGroup(
std::move(ramseteCommand),
frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
}

119
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -1,119 +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.
#include "subsystems/DriveSubsystem.h"
#include <frc/RobotController.h>
#include <frc/SPI.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
#include <frc/simulation/SimDeviceSim.h>
#include <frc/smartdashboard/SmartDashboard.h>
using namespace DriveConstants;
DriveSubsystem::DriveSubsystem() {
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
m_left1.AddFollower(m_left2);
m_right1.AddFollower(m_right2);
// 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_right1.SetInverted(true);
// Set the distance per pulse for the encoders
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
ResetEncoders();
frc::SmartDashboard::PutData("Field", &m_fieldSim);
}
void DriveSubsystem::Periodic() {
// Implementation of subsystem periodic method goes here.
m_odometry.Update(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()});
m_fieldSim.SetRobotPose(m_odometry.GetPose());
}
void DriveSubsystem::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(
units::volt_t{m_left1.Get()} * frc::RobotController::GetInputVoltage(),
units::volt_t{m_right1.Get()} * frc::RobotController::GetInputVoltage());
m_drivetrainSimulator.Update(20_ms);
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
m_rightEncoderSim.SetDistance(
m_drivetrainSimulator.GetRightPosition().value());
m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees());
}
units::ampere_t DriveSubsystem::GetCurrentDraw() const {
return m_drivetrainSimulator.GetCurrentDraw();
}
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
m_drive.ArcadeDrive(fwd, rot);
}
void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
m_left1.SetVoltage(left);
m_right1.SetVoltage(right);
m_drive.Feed();
}
void DriveSubsystem::ResetEncoders() {
m_leftEncoder.Reset();
m_rightEncoder.Reset();
}
double DriveSubsystem::GetAverageEncoderDistance() {
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() {
return m_leftEncoder;
}
frc::Encoder& DriveSubsystem::GetRightEncoder() {
return m_rightEncoder;
}
void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
units::degree_t DriveSubsystem::GetHeading() const {
return m_gyro.GetRotation2d().Degrees();
}
double DriveSubsystem::GetTurnRate() {
return -m_gyro.GetRate();
}
frc::Pose2d DriveSubsystem::GetPose() {
return m_odometry.GetPose();
}
frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
return {units::meters_per_second_t{m_leftEncoder.GetRate()},
units::meters_per_second_t{m_rightEncoder.GetRate()}};
}
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
m_drivetrainSimulator.SetPose(pose);
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}, pose);
}

84
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
View File

@@ -1,84 +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.
#include <numbers>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
#include <units/voltage.h>
#pragma once
/**
* The Constants header provides a convenient place for teams to hold robot-wide
* numerical or bool constants. This should not be used for any other purpose.
*
* It is generally a good idea to place constants into subsystem- or
* command-specific namespaces within this header, which can then be used where
* they are needed.
*/
namespace DriveConstants {
inline constexpr int kLeftMotor1Port = 0;
inline constexpr int kLeftMotor2Port = 1;
inline constexpr int kRightMotor1Port = 2;
inline constexpr int kRightMotor2Port = 3;
inline constexpr int kLeftEncoderPorts[]{0, 1};
inline constexpr int kRightEncoderPorts[]{2, 3};
inline constexpr bool kLeftEncoderReversed = false;
inline constexpr bool kRightEncoderReversed = true;
inline constexpr auto kTrackwidth = 0.69_m;
extern const frc::DifferentialDriveKinematics kDriveKinematics;
inline constexpr int kEncoderCPR = 1024;
inline constexpr auto kWheelDiameter = 6_in;
inline constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameter.value() * std::numbers::pi) /
static_cast<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.
inline constexpr auto ks = 0.22_V;
inline constexpr auto kv = 1.98 * 1_V / 1_mps;
inline constexpr auto ka = 0.2 * 1_V / 1_mps_sq;
inline constexpr auto kvAngular = 1.5 * 1_V / 1_mps;
inline constexpr auto kaAngular = 0.3 * 1_V / 1_mps_sq;
extern const frc::LinearSystem<2, 2, 2> kDrivetrainPlant;
// Example values only -- use what's on your physical robot!
inline constexpr auto kDrivetrainGearbox = frc::DCMotor::CIM(2);
inline constexpr auto kDrivetrainGearing = 8.0;
// Example value only - as above, this must be tuned for your drive!
inline constexpr double kPDriveVel = 8.5;
} // namespace DriveConstants
namespace AutoConstants {
inline constexpr auto kMaxSpeed = 3_mps;
inline constexpr auto kMaxAcceleration = 3_mps_sq;
// Reasonable baseline values for a RAMSETE follower in units of meters and
// seconds
inline constexpr auto kRamseteB = 2 * 1_rad * 1_rad / (1_m * 1_m);
inline constexpr auto kRamseteZeta = 0.7 / 1_rad;
} // namespace AutoConstants
namespace OIConstants {
inline constexpr int kDriverControllerPort = 0;
} // namespace OIConstants

31
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h
View File

@@ -1,31 +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.
#pragma once
#include <frc/TimedRobot.h>
#include <frc2/command/Command.h>
#include "RobotContainer.h"
class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
void DisabledInit() override;
void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
void TeleopPeriodic() override;
void TestPeriodic() override;
void SimulationPeriodic() override;
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
frc2::Command* m_autonomousCommand = nullptr;
RobotContainer m_container;
};

52
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h
View File

@@ -1,52 +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.
#pragma once
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/smartdashboard/SendableChooser.h>
#include <frc2/command/Command.h>
#include <frc2/command/InstantCommand.h>
#include <frc2/command/PIDCommand.h>
#include <frc2/command/ParallelRaceGroup.h>
#include <frc2/command/RunCommand.h>
#include "Constants.h"
#include "subsystems/DriveSubsystem.h"
/**
* 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.
*/
class RobotContainer {
public:
RobotContainer();
void ZeroAllOutputs();
frc2::Command* GetAutonomousCommand();
const DriveSubsystem& GetRobotDrive() const;
private:
// The driver's controller
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
// The robot's subsystems and commands are defined here...
// The robot's subsystems
DriveSubsystem m_drive;
frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
// The chooser for the autonomous routines
frc::SendableChooser<frc2::Command*> m_chooser;
void ConfigureButtonBindings();
};

172
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
View File

@@ -1,172 +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.
#pragma once
#include <frc/ADXRS450_Gyro.h>
#include <frc/Encoder.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <frc/simulation/ADXRS450_GyroSim.h>
#include <frc/simulation/DifferentialDrivetrainSim.h>
#include <frc/simulation/EncoderSim.h>
#include <frc/smartdashboard/Field2d.h>
#include <frc2/command/SubsystemBase.h>
#include <units/voltage.h>
#include "Constants.h"
class DriveSubsystem : public frc2::SubsystemBase {
public:
DriveSubsystem();
/**
* Will be called periodically whenever the CommandScheduler runs.
*/
void Periodic() override;
/**
* Will be called periodically during simulation.
*/
void SimulationPeriodic() override;
// Subsystem methods go here.
units::ampere_t GetCurrentDraw() const;
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
void ArcadeDrive(double fwd, double rot);
/**
* Controls each side of the drive directly with a voltage.
*
* @param left the commanded left output
* @param right the commanded right output
*/
void TankDriveVolts(units::volt_t left, units::volt_t right);
/**
* Resets the drive encoders to currently read a position of 0.
*/
void ResetEncoders();
/**
* Gets the average distance of the TWO encoders.
*
* @return the average of the TWO encoder readings
*/
double GetAverageEncoderDistance();
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
frc::Encoder& GetLeftEncoder();
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
frc::Encoder& GetRightEncoder();
/**
* 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
*/
void SetMaxOutput(double maxOutput);
/**
* Returns the heading of the robot.
*
* @return the robot's heading in degrees, from -180 to 180
*/
units::degree_t GetHeading() const;
/**
* Returns the turn rate of the robot.
*
* @return The turn rate of the robot, in degrees per second
*/
double GetTurnRate();
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
frc::Pose2d GetPose();
/**
* Returns the current wheel speeds of the robot.
*
* @return The current wheel speeds.
*/
frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
/**
* Resets the odometry to the specified pose.
*
* @param pose The pose to which to set the odometry.
*/
void ResetOdometry(frc::Pose2d pose);
private:
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
// The motor controllers
frc::PWMSparkMax m_left1{DriveConstants::kLeftMotor1Port};
frc::PWMSparkMax m_left2{DriveConstants::kLeftMotor2Port};
frc::PWMSparkMax m_right1{DriveConstants::kRightMotor1Port};
frc::PWMSparkMax m_right2{DriveConstants::kRightMotor2Port};
// The robot's drive
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
[&](double output) { m_right1.Set(output); }};
// The left-side drive encoder
frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
DriveConstants::kLeftEncoderPorts[1],
DriveConstants::kLeftEncoderReversed};
// The right-side drive encoder
frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
DriveConstants::kRightEncoderPorts[1],
DriveConstants::kRightEncoderReversed};
// The gyro sensor
frc::ADXRS450_Gyro m_gyro;
// Odometry class for tracking robot pose
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
// These classes help simulate our drivetrain.
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
DriveConstants::kDrivetrainPlant,
DriveConstants::kTrackwidth,
DriveConstants::kDrivetrainGearbox,
DriveConstants::kDrivetrainGearing,
DriveConstants::kWheelDiameter / 2,
{0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
frc::sim::ADXRS450_GyroSim m_gyroSim{m_gyro};
// The Field2d class shows the field in the sim GUI.
frc::Field2d m_fieldSim;
};

49
wpilibcExamples/src/main/cpp/examples/examples.json
View File

@@ -502,24 +502,6 @@
"gradlebase": "cpp",
"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": "cpp",
"commandversion": 2
},
{
"name": "Arcade Drive Xbox Controller",
"description": "Control a differential drive with split-stick arcade drive in teleop.",
@@ -671,23 +653,6 @@
"gradlebase": "cpp",
"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": "cpp",
"commandversion": 2
},
{
"name": "RomiReference",
"description": "An example command-based robot program that can be used with the Romi reference robot design.",
@@ -887,20 +852,6 @@
"gradlebase": "cpp",
"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": "cpp",
"commandversion": 2
},
{
"name": "SwerveDrivePoseEstimator",
"description": "Combine swerve-drive odometry with vision data using SwerveDrivePoseEstimator.",

8
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
View File

@@ -11,8 +11,8 @@ import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.controller.LTVUnicycleController;
import edu.wpi.first.math.controller.LinearPlantInversionFeedforward;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
@@ -47,7 +47,7 @@ class DifferentialDrivetrainSimTest {
VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
var feedforward = new LinearPlantInversionFeedforward<>(plant, 0.020);
var ramsete = new RamseteController();
var feedback = new LTVUnicycleController(0.020);
feedforward.reset(VecBuilder.fill(0, 0));
// ground truth
@@ -63,9 +63,9 @@ class DifferentialDrivetrainSimTest {
for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) {
var state = traj.sample(t);
var ramseteOut = ramsete.calculate(sim.getPose(), state);
var feedbackOut = feedback.calculate(sim.getPose(), state);
var wheelSpeeds = kinematics.toWheelSpeeds(ramseteOut);
var wheelSpeeds = kinematics.toWheelSpeeds(feedbackOut);
var voltages =
feedforward.calculate(

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);
}
}

5
wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
View File

@@ -48,7 +48,9 @@ public class RamseteController {
* aggressive like a proportional term.
* @param zeta Tuning parameter (0 rad⁻¹ &lt; zeta &lt; 1 rad⁻¹) for which larger values provide
* more damping in response.
* @deprecated Use LTVUnicycleController instead.
*/
@Deprecated(since = "2024", forRemoval = true)
public RamseteController(double b, double zeta) {
m_b = b;
m_zeta = zeta;
@@ -57,7 +59,10 @@ public class RamseteController {
/**
* Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 rad²/m²
* and 0.7 rad⁻¹ have been well-tested to produce desirable results.
*
* @deprecated Use LTVUnicycleController instead.
*/
@Deprecated(since = "2024", forRemoval = true)
public RamseteController() {
this(2.0, 0.7);
}

6
wpimath/src/main/native/cpp/controller/RamseteController.cpp
View File

@@ -4,6 +4,8 @@
#include "frc/controller/RamseteController.h"
#include <wpi/deprecated.h>
#include "units/angle.h"
#include "units/math.h"
@@ -26,10 +28,14 @@ RamseteController::RamseteController(units::unit_t<b_unit> b,
units::unit_t<zeta_unit> zeta)
: m_b{b}, m_zeta{zeta} {}
WPI_IGNORE_DEPRECATED
RamseteController::RamseteController()
: RamseteController{units::unit_t<b_unit>{2.0},
units::unit_t<zeta_unit>{0.7}} {}
WPI_UNIGNORE_DEPRECATED
bool RamseteController::AtReference() const {
const auto& eTranslate = m_poseError.Translation();
const auto& eRotate = m_poseError.Rotation();

5
wpimath/src/main/native/include/frc/controller/RamseteController.h
View File

@@ -56,14 +56,19 @@ class WPILIB_DLLEXPORT RamseteController {
* convergence more aggressive like a proportional term.
* @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger
* values provide more damping in response.
* @deprecated Use LTVUnicycleController instead.
*/
[[deprecated("Use LTVUnicycleController instead.")]]
RamseteController(units::unit_t<b_unit> b, units::unit_t<zeta_unit> zeta);
/**
* Construct a Ramsete unicycle controller. The default arguments for
* b and zeta of 2.0 rad²/m² and 0.7 rad⁻¹ have been well-tested to produce
* desirable results.
*
* @deprecated Use LTVUnicycleController instead.
*/
[[deprecated("Use LTVUnicycleController instead.")]]
RamseteController();
/**

1
wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java
View File

@@ -20,6 +20,7 @@ class RamseteControllerTest {
private static final double kTolerance = 1 / 12.0;
private static final double kAngularTolerance = Math.toRadians(2);
@SuppressWarnings("removal")
@Test
void testReachesReference() {
final var controller = new RamseteController(2.0, 0.7);

5
wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
View File

@@ -3,6 +3,7 @@
// the WPILib BSD license file in the root directory of this project.
#include <gtest/gtest.h>
#include <wpi/deprecated.h>
#include "frc/MathUtil.h"
#include "frc/controller/RamseteController.h"
@@ -16,6 +17,8 @@ static constexpr units::meter_t kTolerance{1 / 12.0};
static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
WPI_IGNORE_DEPRECATED
TEST(RamseteControllerTest, ReachesReference) {
frc::RamseteController controller{2.0 * 1_rad * 1_rad / (1_m * 1_m),
0.7 / 1_rad};
@@ -43,3 +46,5 @@ TEST(RamseteControllerTest, ReachesReference) {
robotPose.Rotation().Radians()),
0_rad, kAngularTolerance);
}
WPI_UNIGNORE_DEPRECATED