diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index 48b37563dc..050e97048d 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -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, diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h index 56fb8ff92d..5a15f54883 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h @@ -69,7 +69,9 @@ class RamseteCommand : public CommandHelper { * @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 pose, frc::RamseteController controller, frc::SimpleMotorFeedforward feedforward, @@ -95,13 +97,14 @@ class RamseteCommand : public CommandHelper { * @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 pose, frc::RamseteController controller, frc::DifferentialDriveKinematics kinematics, std::function - output, + units::meters_per_second_t)> output, Requirements requirements = {}); void Initialize() override; diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp index a942d16e3d..9ed23624de 100644 --- a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp @@ -7,8 +7,8 @@ #include #include +#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()}); diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp deleted file mode 100644 index d462c243cf..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp +++ /dev/null @@ -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); diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp deleted file mode 100644 index c4c1661d69..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp +++ /dev/null @@ -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 -#include - -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. - * - *

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(); -} -#endif diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp deleted file mode 100644 index feacbcc2a7..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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{ - 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{ - 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); }, {})); -} diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp deleted file mode 100644 index d367607aec..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp +++ /dev/null @@ -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 -#include - -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); -} diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h deleted file mode 100644 index d2ca25be94..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include - -#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(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 diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h deleted file mode 100644 index 889bfdf3fa..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h +++ /dev/null @@ -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 - -#include -#include - -#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 m_autonomousCommand; - - RobotContainer m_container; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h deleted file mode 100644 index b01e6e242a..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h +++ /dev/null @@ -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 -#include -#include -#include -#include - -#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(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h deleted file mode 100644 index e1580e1b48..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include - -#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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp deleted file mode 100644 index 9ce08e9b9a..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp +++ /dev/null @@ -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(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp deleted file mode 100644 index d7902f17d5..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include - -#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 m_speedLimiter{3 / 1_s}; - frc::SlewRateLimiter 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(); -} -#endif diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h deleted file mode 100644 index 39511c7a4b..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * 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 m_feedforward{1_V, 3_V / 1_mps}; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp index 523bb7b343..fa1cfe1bdb 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include @@ -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; }; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp deleted file mode 100644 index be6bfe0351..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp +++ /dev/null @@ -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); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp deleted file mode 100644 index fce5aececc..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp +++ /dev/null @@ -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 -#include -#include -#include - -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. - * - *

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(); -} -#endif diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp deleted file mode 100644 index 8c73dc879b..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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( - 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( - 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); }, {})); -} diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp deleted file mode 100644 index 721481db29..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include - -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); -} diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h deleted file mode 100644 index 103204a75f..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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(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 diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h deleted file mode 100644 index 00271e6e21..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h +++ /dev/null @@ -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 -#include - -#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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h deleted file mode 100644 index 012e3c6b35..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include - -#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 m_chooser; - - void ConfigureButtonBindings(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h deleted file mode 100644 index 5e412019bc..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 486e2ec62b..0a165d6e8f 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -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.", diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java index cbb8d18c86..554d967ab1 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java @@ -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( diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index b1ddb6c840..4dcc45d3a5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -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.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java deleted file mode 100644 index d5140ee13a..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java +++ /dev/null @@ -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. - * - *

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; - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java deleted file mode 100644 index 462edd450e..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java +++ /dev/null @@ -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. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java deleted file mode 100644 index b072443cee..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java +++ /dev/null @@ -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. - * - *

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() {} -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java deleted file mode 100644 index 4bbf45c29b..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java +++ /dev/null @@ -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))); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java deleted file mode 100644 index a59c139a53..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java +++ /dev/null @@ -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(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java deleted file mode 100644 index 98d185694e..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java +++ /dev/null @@ -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(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java deleted file mode 100644 index 0b59d82f95..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java +++ /dev/null @@ -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. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java deleted file mode 100644 index ef73d106da..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java +++ /dev/null @@ -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); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java index 3ad50a4d60..553ac9cd19 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java @@ -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); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java deleted file mode 100644 index 2177bd7145..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java +++ /dev/null @@ -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. - * - *

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 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; - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java deleted file mode 100644 index bccc25aa5c..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java +++ /dev/null @@ -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. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java deleted file mode 100644 index 8eca311821..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java +++ /dev/null @@ -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(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java deleted file mode 100644 index 6cb6e0238e..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java +++ /dev/null @@ -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)); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java deleted file mode 100644 index 01360f2fb2..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java +++ /dev/null @@ -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); - } -} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java index 5683987bc6..3853776e6b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java @@ -48,7 +48,9 @@ public class RamseteController { * 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(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); } diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp index d3bc708d97..70a64af1b5 100644 --- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp +++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp @@ -4,6 +4,8 @@ #include "frc/controller/RamseteController.h" +#include + #include "units/angle.h" #include "units/math.h" @@ -26,10 +28,14 @@ RamseteController::RamseteController(units::unit_t b, units::unit_t zeta) : m_b{b}, m_zeta{zeta} {} +WPI_IGNORE_DEPRECATED + RamseteController::RamseteController() : RamseteController{units::unit_t{2.0}, units::unit_t{0.7}} {} +WPI_UNIGNORE_DEPRECATED + bool RamseteController::AtReference() const { const auto& eTranslate = m_poseError.Translation(); const auto& eRotate = m_poseError.Rotation(); diff --git a/wpimath/src/main/native/include/frc/controller/RamseteController.h b/wpimath/src/main/native/include/frc/controller/RamseteController.h index d0311019c3..8d8ec3fdae 100644 --- a/wpimath/src/main/native/include/frc/controller/RamseteController.h +++ b/wpimath/src/main/native/include/frc/controller/RamseteController.h @@ -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, units::unit_t 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(); /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java index 813bf429ce..c68272d881 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java @@ -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); diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp index d39c67913b..6794b9cb02 100644 --- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #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