From 445fdbcc591bd05d39a2248a9ece3bb01399ae67 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Wed, 4 Sep 2024 10:57:52 -0400 Subject: [PATCH] Remove GyroDriveCommands --- .../examples/GyroDriveCommands/cpp/Robot.cpp | 72 --------- .../GyroDriveCommands/cpp/RobotContainer.cpp | 70 --------- .../cpp/commands/TurnToAngle.cpp | 33 ----- .../cpp/commands/TurnToAngleProfiled.cpp | 38 ----- .../cpp/subsystems/DriveSubsystem.cpp | 68 --------- .../GyroDriveCommands/include/Constants.h | 64 -------- .../GyroDriveCommands/include/Robot.h | 32 ---- .../include/RobotContainer.h | 44 ------ .../include/commands/TurnToAngle.h | 26 ---- .../include/commands/TurnToAngleProfiled.h | 30 ---- .../include/subsystems/DriveSubsystem.h | 105 -------------- .../cpp/subsystems/Drive.cpp | 2 +- .../examples/gyrodrivecommands/Constants.java | 53 ------- .../examples/gyrodrivecommands/Main.java | 25 ---- .../examples/gyrodrivecommands/Robot.java | 100 ------------- .../gyrodrivecommands/RobotContainer.java | 99 ------------- .../commands/TurnToAngle.java | 45 ------ .../commands/TurnToAngleProfiled.java | 52 ------- .../subsystems/DriveSubsystem.java | 137 ------------------ 19 files changed, 1 insertion(+), 1094 deletions(-) delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp deleted file mode 100644 index 142d59fa6be..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/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 - -Robot::Robot() {} - -/** - * 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/GyroDriveCommands/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp deleted file mode 100644 index 7b076840b9b..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp +++ /dev/null @@ -1,70 +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 - -RobotContainer::RobotContainer() { - // Initialize all of your commands and subsystems here - - // Configure the button bindings - ConfigureButtonBindings(); - - // Set up default drive command - m_drive.SetDefaultCommand(frc2::RunCommand( - [this] { - m_drive.ArcadeDrive(-m_driverController.GetLeftY(), - -m_driverController.GetRightX()); - }, - {&m_drive})); -} - -void RobotContainer::ConfigureButtonBindings() { - // Configure your button bindings here - - // Assorted commands to be bound to buttons - - // Stabilize robot to drive straight with gyro when L1 is held - frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kL1) - .WhileTrue( - frc2::PIDCommand( - frc::PIDController{dc::kStabilizationP, dc::kStabilizationI, - dc::kStabilizationD}, - // Close the loop on the turn rate - [this] { return m_drive.GetTurnRate(); }, - // Setpoint is 0 - 0, - // Pipe the output to the turning controls - [this](double output) { - m_drive.ArcadeDrive(m_driverController.GetLeftY(), output); - }, - // Require the robot drive - {&m_drive}) - .ToPtr()); - - // Turn to 90 degrees when the 'Cross' button is pressed - frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kCross) - .OnTrue(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s)); - - // Turn to -90 degrees with a profile when the 'Square' button is pressed, - // with a 5 second timeout - frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kSquare) - .OnTrue(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s)); - - // While holding R1, drive at half speed - frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kR1) - .OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {})) - .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {})); -} - -frc2::CommandPtr RobotContainer::GetAutonomousCommand() { - // no auto - return frc2::cmd::None(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp deleted file mode 100644 index 6d1272d17ba..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp +++ /dev/null @@ -1,33 +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 "commands/TurnToAngle.h" - -#include - -using namespace DriveConstants; - -TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive) - : CommandHelper{frc::PIDController{kTurnP, kTurnI, kTurnD}, - // Close loop on heading - [drive] { return drive->GetHeading().value(); }, - // Set reference to target - target.value(), - // Pipe output to turn robot - [drive](double output) { drive->ArcadeDrive(0, output); }, - // Require the drive - {drive}} { - // Set the controller to be continuous (because it is an angle controller) - m_controller.EnableContinuousInput(-180, 180); - // Set the controller tolerance - the delta tolerance ensures the robot is - // stationary at the setpoint before it is considered as having reached the - // reference - m_controller.SetTolerance(kTurnTolerance.value(), kTurnRateTolerance.value()); - - AddRequirements(drive); -} - -bool TurnToAngle::IsFinished() { - return GetController().AtSetpoint(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp deleted file mode 100644 index 464e0cf5cbe..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.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 "commands/TurnToAngleProfiled.h" - -#include - -using namespace DriveConstants; - -TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target, - DriveSubsystem* drive) - : CommandHelper{ - frc::ProfiledPIDController{ - kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}}, - // Close loop on heading - [drive] { return drive->GetHeading(); }, - // Set reference to target - target, - // Pipe output to turn robot - [drive](double output, auto setpointState) { - drive->ArcadeDrive(0, output); - }, - // Require the drive - {drive}} { - // Set the controller to be continuous (because it is an angle controller) - GetController().EnableContinuousInput(-180_deg, 180_deg); - // Set the controller tolerance - the delta tolerance ensures the robot is - // stationary at the setpoint before it is considered as having reached the - // reference - GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance); - - AddRequirements(drive); -} - -bool TurnToAngleProfiled::IsFinished() { - return GetController().AtGoal(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp deleted file mode 100644 index a97a5c071f6..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp +++ /dev/null @@ -1,68 +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" - -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]} { - 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); -} - -void DriveSubsystem::Periodic() { - // Implementation of subsystem periodic method goes here. -} - -void DriveSubsystem::ArcadeDrive(double fwd, double rot) { - m_drive.ArcadeDrive(fwd, rot); -} - -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() { - return units::degree_t{std::remainder(m_gyro.GetAngle(), 360) * - (kGyroReversed ? -1.0 : 1.0)}; -} - -double DriveSubsystem::GetTurnRate() { - return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h deleted file mode 100644 index 4d25b0bfb90..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h +++ /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. - -#pragma once - -#include - -#include -#include - -/** - * 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 int kEncoderCPR = 1024; -inline constexpr double kWheelDiameterInches = 6; -inline constexpr double kEncoderDistancePerPulse = - // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterInches * std::numbers::pi) / - static_cast(kEncoderCPR); - -inline constexpr bool kGyroReversed = true; - -inline constexpr double kStabilizationP = 1; -inline constexpr double kStabilizationI = 0.5; -inline constexpr double kStabilizationD = 0; - -inline constexpr double kTurnP = 1; -inline constexpr double kTurnI = 0; -inline constexpr double kTurnD = 0; - -inline constexpr auto kTurnTolerance = 5_deg; -inline constexpr auto kTurnRateTolerance = 10_deg_per_s; - -inline constexpr auto kMaxTurnRate = 100_deg_per_s; -inline constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s; -} // namespace DriveConstants - -namespace AutoConstants { -inline constexpr double kAutoDriveDistanceInches = 60; -inline constexpr double kAutoBackupDistanceInches = 20; -inline constexpr double kAutoDriveSpeed = 0.5; -} // namespace AutoConstants - -namespace OIConstants { -inline constexpr int kDriverControllerPort = 0; -} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h deleted file mode 100644 index 24056944479..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h +++ /dev/null @@ -1,32 +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: - Robot(); - 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 null by default so that if testing teleop it - // doesn't have undefined behavior and potentially crash. - std::optional m_autonomousCommand; - - RobotContainer m_container; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h deleted file mode 100644 index 0ecb7286d9f..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h +++ /dev/null @@ -1,44 +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 "Constants.h" -#include "commands/TurnToAngle.h" -#include "subsystems/DriveSubsystem.h" - -namespace ac = AutoConstants; -namespace dc = DriveConstants; - -/** - * 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::PS4Controller m_driverController{OIConstants::kDriverControllerPort}; - - // The robot's subsystems and commands are defined here... - - // The robot's subsystems - DriveSubsystem m_drive; - - void ConfigureButtonBindings(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h deleted file mode 100644 index 6df79bfabc1..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h +++ /dev/null @@ -1,26 +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 "subsystems/DriveSubsystem.h" - -/** - * A command that will turn the robot to the specified angle. - */ -class TurnToAngle : public frc2::CommandHelper { - public: - /** - * Turns to robot to the specified angle. - * - * @param targetAngleDegrees The angle to turn to - * @param drive The drive subsystem to use - */ - TurnToAngle(units::degree_t target, DriveSubsystem* drive); - - bool IsFinished() override; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h deleted file mode 100644 index f3136aafcd9..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h +++ /dev/null @@ -1,30 +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 "subsystems/DriveSubsystem.h" - -/** - * A command that will turn the robot to the specified angle using a motion - * profile. - */ -class TurnToAngleProfiled - : public frc2::CommandHelper, - TurnToAngleProfiled> { - public: - /** - * Turns to robot to the specified angle using a motion profile. - * - * @param targetAngleDegrees The angle to turn to - * @param drive The drive subsystem to use - */ - TurnToAngleProfiled(units::degree_t targetAngleDegrees, - DriveSubsystem* drive); - - bool IsFinished() override; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h deleted file mode 100644 index d7cce6c05c2..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h +++ /dev/null @@ -1,105 +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 "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); - - /** - * 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(); - - /** - * Returns the turn rate of the robot. - * - * @return The turn rate of the robot, in degrees per second - */ - double GetTurnRate(); - - 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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp index 534a919e442..67eee460675 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp @@ -60,5 +60,5 @@ frc2::CommandPtr Drive::TurnToAngleCommand(units::degree_t angle) { angle)); }) .Until([this] { return m_controller.AtGoal(); }) - .FinallyDo([this] { m_drive.ArcadeDrive(0, 0) }); + .FinallyDo([this] { m_drive.ArcadeDrive(0, 0); }); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java deleted file mode 100644 index 5c5f0b23b17..00000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java +++ /dev/null @@ -1,53 +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.gyrodrivecommands; - -/** - * 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 int kEncoderCPR = 1024; - public static final double kWheelDiameterInches = 6; - public static final double kEncoderDistancePerPulse = - // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterInches * Math.PI) / kEncoderCPR; - - public static final boolean kGyroReversed = false; - - public static final double kStabilizationP = 1; - public static final double kStabilizationI = 0.5; - public static final double kStabilizationD = 0; - - public static final double kTurnP = 1; - public static final double kTurnI = 0; - public static final double kTurnD = 0; - - public static final double kMaxTurnRateDegPerS = 100; - public static final double kMaxTurnAccelerationDegPerSSquared = 300; - - public static final double kTurnToleranceDeg = 5; - public static final double kTurnRateToleranceDegPerS = 10; // degrees per second - } - - public static final class OIConstants { - public static final int kDriverControllerPort = 0; - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java deleted file mode 100644 index 9e6bfe73a53..00000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/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.gyrodrivecommands; - -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/gyrodrivecommands/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java deleted file mode 100644 index 6b5b1105528..00000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java +++ /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. - -package edu.wpi.first.wpilibj.examples.gyrodrivecommands; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -/** - * The methods in this class are called automatically corresponding to each mode, as described in - * the TimedRobot documentation. If you change the name of this class or the package after creating - * this project, you must also update the Main.java file in the project. - */ -public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private final RobotContainer m_robotContainer; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - public Robot() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); - } - - /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. - * - *

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/gyrodrivecommands/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java deleted file mode 100644 index 5caf497170f..00000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java +++ /dev/null @@ -1,99 +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.gyrodrivecommands; - -import static edu.wpi.first.wpilibj.PS4Controller.Button; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.PS4Controller; -import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants; -import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants; -import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle; -import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled; -import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PIDCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; - -/** - * 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 - PS4Controller m_driverController = new PS4Controller(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 PS4Controller}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - // Drive at half speed when the right bumper is held - new JoystickButton(m_driverController, Button.kR1.value) - .onTrue(new InstantCommand(() -> m_robotDrive.setMaxOutput(0.5))) - .onFalse(new InstantCommand(() -> m_robotDrive.setMaxOutput(1))); - - // Stabilize robot to drive straight with gyro when left bumper is held - new JoystickButton(m_driverController, Button.kL1.value) - .whileTrue( - new PIDCommand( - new PIDController( - DriveConstants.kStabilizationP, - DriveConstants.kStabilizationI, - DriveConstants.kStabilizationD), - // Close the loop on the turn rate - m_robotDrive::getTurnRate, - // Setpoint is 0 - 0, - // Pipe the output to the turning controls - output -> m_robotDrive.arcadeDrive(-m_driverController.getLeftY(), output), - // Require the robot drive - m_robotDrive)); - - // Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout - new JoystickButton(m_driverController, Button.kCross.value) - .onTrue(new TurnToAngle(90, m_robotDrive).withTimeout(5)); - - // Turn to -90 degrees with a profile when the Circle button is pressed, with a 5 second timeout - new JoystickButton(m_driverController, Button.kCircle.value) - .onTrue(new TurnToAngleProfiled(-90, m_robotDrive).withTimeout(5)); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // no auto - return new InstantCommand(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java deleted file mode 100644 index 053c64430f8..00000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java +++ /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. - -package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants; -import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem; -import edu.wpi.first.wpilibj2.command.PIDCommand; - -/** A command that will turn the robot to the specified angle. */ -public class TurnToAngle extends PIDCommand { - /** - * Turns to robot to the specified angle. - * - * @param targetAngleDegrees The angle to turn to - * @param drive The drive subsystem to use - */ - public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) { - super( - new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD), - // Close loop on heading - drive::getHeading, - // Set reference to target - targetAngleDegrees, - // Pipe output to turn robot - output -> drive.arcadeDrive(0, output), - // Require the drive - drive); - - // Set the controller to be continuous (because it is an angle controller) - getController().enableContinuousInput(-180, 180); - // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the - // setpoint before it is considered as having reached the reference - getController() - .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS); - } - - @Override - public boolean isFinished() { - // End when the controller is at the reference. - return getController().atSetpoint(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java deleted file mode 100644 index 1481fab5cd0..00000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java +++ /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. - -package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands; - -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants; -import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem; -import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand; - -/** A command that will turn the robot to the specified angle using a motion profile. */ -public class TurnToAngleProfiled extends ProfiledPIDCommand { - /** - * Turns to robot to the specified angle using a motion profile. - * - * @param targetAngleDegrees The angle to turn to - * @param drive The drive subsystem to use - */ - public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) { - super( - new ProfiledPIDController( - DriveConstants.kTurnP, - DriveConstants.kTurnI, - DriveConstants.kTurnD, - new TrapezoidProfile.Constraints( - DriveConstants.kMaxTurnRateDegPerS, - DriveConstants.kMaxTurnAccelerationDegPerSSquared)), - // Close loop on heading - drive::getHeading, - // Set reference to target - targetAngleDegrees, - // Pipe output to turn robot - (output, setpoint) -> drive.arcadeDrive(0, output), - // Require the drive - drive); - - // Set the controller to be continuous (because it is an angle controller) - getController().enableContinuousInput(-180, 180); - // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the - // setpoint before it is considered as having reached the reference - getController() - .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS); - } - - @Override - public boolean isFinished() { - // End when the controller is at the reference. - return getController().atGoal(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java deleted file mode 100644 index db874104b6b..00000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java +++ /dev/null @@ -1,137 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems; - -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.gyrodrivecommands.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(); - - /** 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); - } - - /** - * 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); - } - - /** 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); - } - - /** - * Returns the turn rate of the robot. - * - * @return The turn rate of the robot, in degrees per second - */ - public double getTurnRate() { - return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0); - } -}