diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp deleted file mode 100644 index 66f1a891db..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp +++ /dev/null @@ -1,74 +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() { - m_container.DisablePIDSubsystems(); -} - -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/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp deleted file mode 100644 index 1bf4dcf6df..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp +++ /dev/null @@ -1,65 +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 - -namespace ac = AutoConstants; - -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 - - // Move the arm to 2 radians above horizontal when the 'A' button is pressed. - m_driverController.A().OnTrue(frc2::cmd::RunOnce( - [this] { - m_arm.SetGoal(2_rad); - m_arm.Enable(); - }, - {&m_arm})); - - // Move the arm to neutral position when the 'B' button is pressed. - m_driverController.B().OnTrue(frc2::cmd::RunOnce( - [this] { - m_arm.SetGoal(ArmConstants::kArmOffset); - m_arm.Enable(); - }, - {&m_arm})); - - // Disable the arm controller when Y is pressed. - m_driverController.Y().OnTrue( - frc2::cmd::RunOnce([this] { m_arm.Disable(); }, {&m_arm})); - - // While holding the shoulder button, drive at half speed - m_driverController.RightBumper() - .OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); })) - .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1.0); })); -} - -void RobotContainer::DisablePIDSubsystems() { - m_arm.Disable(); -} - -frc2::CommandPtr RobotContainer::GetAutonomousCommand() { - return frc2::cmd::None(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp deleted file mode 100644 index 6d5eab67af..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp +++ /dev/null @@ -1,34 +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/ArmSubsystem.h" - -#include "Constants.h" - -using namespace ArmConstants; -using State = frc::TrapezoidProfile::State; - -ArmSubsystem::ArmSubsystem() - : frc2::ProfiledPIDSubsystem( - frc::ProfiledPIDController( - kP, 0, 0, {kMaxVelocity, kMaxAcceleration})), - m_motor(kMotorPort), - m_encoder(kEncoderPorts[0], kEncoderPorts[1]), - m_feedforward(kS, kG, kV, kA) { - m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.value()); - // Start arm in neutral position - SetGoal(State{kArmOffset, 0_rad_per_s}); -} - -void ArmSubsystem::UseOutput(double output, State setpoint) { - // Calculate the feedforward from the sepoint - units::volt_t feedforward = - m_feedforward.Calculate(setpoint.position, setpoint.velocity); - // Add the feedforward to the PID output to get the motor output - m_motor.SetVoltage(units::volt_t{output} + feedforward); -} - -units::radian_t ArmSubsystem::GetMeasurement() { - return units::radian_t{m_encoder.GetDistance()} + kArmOffset; -} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp deleted file mode 100644 index 13522950d8..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp +++ /dev/null @@ -1,59 +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); - - // Set the distance per pulse for the encoders - m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); - m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); - - // 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); -} - -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.; -} - -frc::Encoder& DriveSubsystem::GetLeftEncoder() { - return m_leftEncoder; -} - -frc::Encoder& DriveSubsystem::GetRightEncoder() { - return m_rightEncoder; -} - -void DriveSubsystem::SetMaxOutput(double maxOutput) { - m_drive.SetMaxOutput(maxOutput); -} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h deleted file mode 100644 index 556ee4c54b..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h +++ /dev/null @@ -1,74 +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 - -/** - * 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); -} // namespace DriveConstants - -namespace ArmConstants { -inline constexpr int kMotorPort = 4; - -inline constexpr double kP = 1; - -// These are fake gains; in actuality these must be determined individually for -// each robot -inline constexpr auto kS = 1_V; -inline constexpr auto kG = 1_V; -inline constexpr auto kV = 0.5_V * 1_s / 1_rad; -inline constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad; - -inline constexpr auto kMaxVelocity = 3_rad_per_s; -inline constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s); - -inline constexpr int kEncoderPorts[]{4, 5}; -inline constexpr int kEncoderPPR = 256; -inline constexpr auto kEncoderDistancePerPulse = - 2.0_rad * std::numbers::pi / kEncoderPPR; - -// The offset of the arm from the horizontal in its neutral position, -// measured from the horizontal -inline constexpr auto kArmOffset = 0.5_rad; -} // namespace ArmConstants - -namespace AutoConstants { -inline constexpr auto kAutoTimeoutSeconds = 12_s; -inline constexpr auto kAutoShootTimeSeconds = 7_s; -} // namespace AutoConstants - -namespace OIConstants { -inline constexpr int kDriverControllerPort = 0; -} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h deleted file mode 100644 index 2405694447..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/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/ArmBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h deleted file mode 100644 index ab77985e54..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h +++ /dev/null @@ -1,46 +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 "Constants.h" -#include "subsystems/ArmSubsystem.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(); - - /** - * Disables all ProfiledPIDSubsystem and PIDSubsystem instances. - * This should be called on robot disable to prevent integral windup. - */ - void DisablePIDSubsystems(); - - frc2::CommandPtr GetAutonomousCommand(); - - private: - // The driver's controller - frc2::CommandXboxController m_driverController{ - OIConstants::kDriverControllerPort}; - - // The robot's subsystems and commands are defined here... - - // The robot's subsystems - DriveSubsystem m_drive; - ArmSubsystem m_arm; - - void ConfigureButtonBindings(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h deleted file mode 100644 index 45cd582117..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.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 -#include -#include - -/** - * A robot arm subsystem that moves with a motion profile. - */ -class ArmSubsystem : public frc2::ProfiledPIDSubsystem { - using State = frc::TrapezoidProfile::State; - - public: - ArmSubsystem(); - - void UseOutput(double output, State setpoint) override; - - units::radian_t GetMeasurement() override; - - private: - frc::PWMSparkMax m_motor; - frc::Encoder m_encoder; - frc::ArmFeedforward m_feedforward; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h deleted file mode 100644 index 47d3f3d49b..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h +++ /dev/null @@ -1,86 +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 "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); - - 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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp deleted file mode 100644 index 142d59fa6b..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/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/ArmBotOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp deleted file mode 100644 index a8704165a5..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp +++ /dev/null @@ -1,40 +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 - -RobotContainer::RobotContainer() { - // Initialize all of your commands and subsystems here - - // Configure the button bindings - ConfigureButtonBindings(); - - // Set up default drive command - m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand( - [this] { return -m_driverController.GetLeftY(); }, - [this] { return -m_driverController.GetRightX(); })); -} - -void RobotContainer::ConfigureButtonBindings() { - // Configure your button bindings here - - // Move the arm to 2 radians above horizontal when the 'A' button is pressed. - m_driverController.A().OnTrue(m_arm.SetArmGoalCommand(2_rad)); - - // Move the arm to neutral position when the 'B' button is pressed. - m_driverController.B().OnTrue( - m_arm.SetArmGoalCommand(ArmConstants::kArmOffset)); - - // While holding the shoulder button, drive at half speed - m_driverController.RightBumper() - .OnTrue(m_drive.SetMaxOutputCommand(0.5)) - .OnFalse(m_drive.SetMaxOutputCommand(1.0)); -} - -frc2::CommandPtr RobotContainer::GetAutonomousCommand() { - return frc2::cmd::None(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp deleted file mode 100644 index 5603c680a2..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp +++ /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. - -#include "subsystems/ArmSubsystem.h" - -#include "Constants.h" - -using namespace ArmConstants; -using State = frc::TrapezoidProfile::State; - -ArmSubsystem::ArmSubsystem() - : frc2::TrapezoidProfileSubsystem( - {kMaxVelocity, kMaxAcceleration}, kArmOffset), - m_motor(kMotorPort), - m_feedforward(kS, kG, kV, kA) { - m_motor.SetPID(kP, 0, 0); -} - -void ArmSubsystem::UseState(State setpoint) { - // Calculate the feedforward from the sepoint - units::volt_t feedforward = - m_feedforward.Calculate(setpoint.position, setpoint.velocity); - // Add the feedforward to the PID output to get the motor output - m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, - setpoint.position.value(), feedforward / 12_V); -} - -frc2::CommandPtr ArmSubsystem::SetArmGoalCommand(units::radian_t goal) { - return frc2::cmd::RunOnce([this, goal] { this->SetGoal(goal); }, {this}); -} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp deleted file mode 100644 index e5705f1b70..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp +++ /dev/null @@ -1,67 +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 - -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); - - // Set the distance per pulse for the encoders - m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); - m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); - - // 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); -} - -void DriveSubsystem::Periodic() { - // Implementation of subsystem periodic method goes here. -} - -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; -} - -frc2::CommandPtr DriveSubsystem::SetMaxOutputCommand(double maxOutput) { - return frc2::cmd::RunOnce( - [this, maxOutput] { m_drive.SetMaxOutput(maxOutput); }); -} - -frc2::CommandPtr DriveSubsystem::ArcadeDriveCommand( - std::function fwd, std::function rot) { - return frc2::cmd::Run( - [this, fwd = std::move(fwd), rot = std::move(rot)] { - m_drive.ArcadeDrive(fwd(), rot()); - }, - {this}); -} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h deleted file mode 100644 index b91fe195c4..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h +++ /dev/null @@ -1,74 +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 - -/** - * 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); -} // namespace DriveConstants - -namespace ArmConstants { -inline constexpr int kMotorPort = 4; - -inline constexpr double kP = 1; - -// These are fake gains; in actuality these must be determined individually for -// each robot -inline constexpr auto kS = 1_V; -inline constexpr auto kG = 1_V; -inline constexpr auto kV = 0.5_V * 1_s / 1_rad; -inline constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad; - -inline constexpr auto kMaxVelocity = 3_rad_per_s; -inline constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s); - -inline constexpr int kEncoderPorts[]{4, 5}; -inline constexpr int kEncoderPPR = 256; -inline constexpr auto kEncoderDistancePerPulse = - 2.0_rad * std::numbers::pi / kEncoderPPR; - -// The offset of the arm from the horizontal in its neutral position, -// measured from the horizontal -inline constexpr auto kArmOffset = 0.5_rad; -} // namespace ArmConstants - -namespace AutoConstants { -inline constexpr auto kAutoTimeoutSeconds = 12_s; -inline constexpr auto kAutoShootTimeSeconds = 7_s; -} // namespace AutoConstants - -namespace OIConstants { -constexpr int kDriverControllerPort = 0; -} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h deleted file mode 100644 index bd48d88e0b..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h +++ /dev/null @@ -1,80 +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 - -/** - * A simplified stub class that simulates the API of a common "smart" motor - * controller. - * - *

Has no actual functionality. - */ -class ExampleSmartMotorController { - public: - enum PIDMode { kPosition, kVelocity, kMovementWitchcraft }; - - /** - * Creates a new ExampleSmartMotorController. - * - * @param port The port for the controller. - */ - explicit ExampleSmartMotorController(int port) {} - - /** - * Example method for setting the PID gains of the smart controller. - * - * @param kp The proportional gain. - * @param ki The integral gain. - * @param kd The derivative gain. - */ - void SetPID(double kp, double ki, double kd) {} - - /** - * Example method for setting the setpoint of the smart controller in PID - * mode. - * - * @param mode The mode of the PID controller. - * @param setpoint The controller setpoint. - * @param arbFeedforward An arbitrary feedforward output (from -1 to 1). - */ - void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {} - - /** - * Places this motor controller in follower mode. - * - * @param leader The leader to follow. - */ - void Follow(ExampleSmartMotorController leader) {} - - /** - * Returns the encoder distance. - * - * @return The current encoder distance. - */ - double GetEncoderDistance() { return 0; } - - /** - * Returns the encoder rate. - * - * @return The current encoder rate. - */ - double GetEncoderRate() { return 0; } - - /** - * Resets the encoder to zero distance. - */ - void ResetEncoder() {} - - void Set(double speed) {} - - double Get() const { return 0; } - - void SetInverted(bool isInverted) {} - - bool GetInverted() const { return false; } - - void Disable() {} - - void StopMotor() {} -}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h deleted file mode 100644 index 2405694447..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/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/ArmBotOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h deleted file mode 100644 index 7668c03e9a..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h +++ /dev/null @@ -1,42 +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 "Constants.h" -#include "subsystems/ArmSubsystem.h" -#include "subsystems/DriveSubsystem.h" - -namespace ac = AutoConstants; - -/** - * 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 - frc2::CommandXboxController m_driverController{ - OIConstants::kDriverControllerPort}; - - // The robot's subsystems and commands are defined here... - - // The robot's subsystems - DriveSubsystem m_drive; - ArmSubsystem m_arm; - - void ConfigureButtonBindings(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h deleted file mode 100644 index ea2dc644e8..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.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 -#include - -#include "ExampleSmartMotorController.h" - -/** - * A robot arm subsystem that moves with a motion profile. - */ -class ArmSubsystem : public frc2::TrapezoidProfileSubsystem { - using State = frc::TrapezoidProfile::State; - - public: - ArmSubsystem(); - - void UseState(State setpoint) override; - - frc2::CommandPtr SetArmGoalCommand(units::radian_t goal); - - private: - ExampleSmartMotorController m_motor; - frc::ArmFeedforward m_feedforward; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h deleted file mode 100644 index ca7e28e829..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.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. - -#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. - - /** - * 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 - */ - frc2::CommandPtr SetMaxOutputCommand(double maxOutput); - - frc2::CommandPtr ArcadeDriveCommand(std::function fwd, - std::function rot); - - 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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp deleted file mode 100644 index 580decec83..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/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 != 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() {} - -#ifndef RUNNING_FRC_TESTS -int main() { - return frc::StartRobot(); -} -#endif diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp deleted file mode 100644 index 449defdb28..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.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 "RobotContainer.h" - -#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::cmd::Run( - [this] { - m_drive.ArcadeDrive(-m_driverController.GetLeftY(), - -m_driverController.GetRightX()); - }, - {&m_drive})); -} - -void RobotContainer::ConfigureButtonBindings() { - // Configure your button bindings here - - // We can bind commands while keeping their ownership in RobotContainer - - // Spin up the shooter when the 'A' button is pressed - m_driverController.A().OnTrue(m_spinUpShooter.get()); - - // Turn off the shooter when the 'B' button is pressed - m_driverController.B().OnTrue(m_stopShooter.get()); - - // We can also *move* command ownership to the scheduler - // Note that we won't be able to access these commands after moving them! - - // Shoots if the shooter wheel has reached the target speed - frc2::CommandPtr shoot = frc2::cmd::Either( - // Run the feeder - frc2::cmd::RunOnce([this] { m_shooter.RunFeeder(); }, {&m_shooter}), - // Do nothing - frc2::cmd::None(), - // Determine which of the above to do based on whether the shooter has - // reached the desired speed - [this] { return m_shooter.AtSetpoint(); }); - - frc2::CommandPtr stopFeeder = - frc2::cmd::RunOnce([this] { m_shooter.StopFeeder(); }, {&m_shooter}); - - // Shoot when the 'X' button is pressed - m_driverController.X() - .OnTrue(std::move(shoot)) - .OnFalse(std::move(stopFeeder)); - - // We can also define commands inline at the binding! - // (ownership will be passed to the scheduler) - - // While holding the shoulder button, drive at half speed - m_driverController.RightBumper() - .OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {})) - .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {})); -} - -frc2::Command* RobotContainer::GetAutonomousCommand() { - // Runs the chosen command in autonomous - return m_autonomousCommand.get(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp deleted file mode 100644 index 0e4068f22e..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp +++ /dev/null @@ -1,59 +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_left1.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); -} diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp deleted file mode 100644 index 297ccc1eaa..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp +++ /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. - -#include "subsystems/ShooterSubsystem.h" - -#include -#include - -#include "Constants.h" - -using namespace ShooterConstants; - -ShooterSubsystem::ShooterSubsystem() - : PIDSubsystem{frc::PIDController{kP, kI, kD}}, - m_shooterMotor(kShooterMotorPort), - m_feederMotor(kFeederMotorPort), - m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]), - m_shooterFeedforward(kS, kV) { - m_controller.SetTolerance(kShooterToleranceRPS.value()); - m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); - SetSetpoint(kShooterTargetRPS.value()); -} - -void ShooterSubsystem::UseOutput(double output, double setpoint) { - m_shooterMotor.SetVoltage(units::volt_t{output} + - m_shooterFeedforward.Calculate(kShooterTargetRPS)); -} - -bool ShooterSubsystem::AtSetpoint() { - return m_controller.AtSetpoint(); -} - -double ShooterSubsystem::GetMeasurement() { - return m_shooterEncoder.GetRate(); -} - -void ShooterSubsystem::RunFeeder() { - m_feederMotor.Set(kFeederSpeed); -} - -void ShooterSubsystem::StopFeeder() { - m_feederMotor.Set(0); -} diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h deleted file mode 100644 index a056c2dda0..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h +++ /dev/null @@ -1,78 +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 - -/** - * 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); -} // namespace DriveConstants - -namespace ShooterConstants { -inline constexpr int kEncoderPorts[]{4, 5}; -inline constexpr bool kEncoderReversed = false; -inline constexpr int kEncoderCPR = 1024; -inline constexpr double kEncoderDistancePerPulse = - // Distance units will be rotations - 1.0 / static_cast(kEncoderCPR); - -inline constexpr int kShooterMotorPort = 4; -inline constexpr int kFeederMotorPort = 5; - -inline constexpr auto kShooterFreeRPS = 5300_tr / 1_s; -inline constexpr auto kShooterTargetRPS = 4000_tr / 1_s; -inline constexpr auto kShooterToleranceRPS = 50_tr / 1_s; - -inline constexpr double kP = 1; -inline constexpr double kI = 0; -inline constexpr double kD = 0; - -// On a real robot the feedforward constants should be empirically determined; -// these are reasonable guesses. -inline constexpr auto kS = 0.05_V; -inline constexpr auto kV = - // Should have value 12V at free speed... - 12_V / kShooterFreeRPS; - -inline constexpr double kFeederSpeed = 0.5; -} // namespace ShooterConstants - -namespace AutoConstants { -inline constexpr auto kAutoTimeoutSeconds = 12_s; -inline constexpr auto kAutoShootTimeSeconds = 7_s; -} // namespace AutoConstants - -namespace OIConstants { -constexpr int kDriverControllerPort = 0; -} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h deleted file mode 100644 index b670df4cd8..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.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 "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. - frc2::Command* m_autonomousCommand = nullptr; - - RobotContainer m_container; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h deleted file mode 100644 index 40091f1a77..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.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. - -#pragma once - -#include -#include -#include -#include - -#include "Constants.h" -#include "subsystems/DriveSubsystem.h" -#include "subsystems/ShooterSubsystem.h" - -namespace ac = AutoConstants; - -/** - * 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(); - - // The chooser for the autonomous routines - frc2::Command* GetAutonomousCommand(); - - private: - // The driver's controller - frc2::CommandXboxController m_driverController{ - OIConstants::kDriverControllerPort}; - - // The robot's subsystems - DriveSubsystem m_drive; - ShooterSubsystem m_shooter; - - // RobotContainer-owned commands - // (These variables will still be valid after binding, because we don't move - // ownership) - - frc2::CommandPtr m_spinUpShooter = - frc2::cmd::RunOnce([this] { m_shooter.Enable(); }, {&m_shooter}); - - frc2::CommandPtr m_stopShooter = - frc2::cmd::RunOnce([this] { m_shooter.Disable(); }, {&m_shooter}); - - // An autonomous routine that shoots the loaded frisbees - frc2::CommandPtr m_autonomousCommand = - frc2::cmd::Sequence( - // Start the command by spinning up the shooter... - frc2::cmd::RunOnce([this] { m_shooter.Enable(); }, {&m_shooter}), - // Wait until the shooter is at speed before feeding the frisbees - frc2::cmd::WaitUntil([this] { return m_shooter.AtSetpoint(); }), - // Start running the feeder - frc2::cmd::RunOnce([this] { m_shooter.RunFeeder(); }, {&m_shooter}), - // Shoot for the specified time - frc2::cmd::Wait(ac::kAutoShootTimeSeconds)) - // Add a timeout (will end the command if, for instance, the shooter - // never gets up to speed) - .WithTimeout(ac::kAutoTimeoutSeconds) - // When the command ends, turn off the shooter and the feeder - .AndThen(frc2::cmd::RunOnce([this] { - m_shooter.Disable(); - m_shooter.StopFeeder(); - })); - - void ConfigureButtonBindings(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h deleted file mode 100644 index 47d3f3d49b..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h +++ /dev/null @@ -1,86 +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 "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); - - 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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h deleted file mode 100644 index d808d64cc7..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.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 -#include - -class ShooterSubsystem : public frc2::PIDSubsystem { - public: - ShooterSubsystem(); - - void UseOutput(double output, double setpoint) override; - - double GetMeasurement() override; - - bool AtSetpoint(); - - void RunFeeder(); - - void StopFeeder(); - - private: - frc::PWMSparkMax m_shooterMotor; - frc::PWMSparkMax m_feederMotor; - frc::Encoder m_shooterEncoder; - frc::SimpleMotorFeedforward m_shooterFeedforward; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp deleted file mode 100644 index 580decec83..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/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 != 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() {} - -#ifndef RUNNING_FRC_TESTS -int main() { - return frc::StartRobot(); -} -#endif diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp deleted file mode 100644 index c08faee741..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp +++ /dev/null @@ -1,54 +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 "commands/CloseClaw.h" -#include "commands/OpenClaw.h" -#include "commands/Pickup.h" -#include "commands/Place.h" -#include "commands/PrepareToPickup.h" -#include "commands/SetElevatorSetpoint.h" -#include "commands/TankDrive.h" - -RobotContainer::RobotContainer() - : m_autonomousCommand(m_claw, m_wrist, m_elevator, m_drivetrain) { - frc::SmartDashboard::PutData(&m_drivetrain); - frc::SmartDashboard::PutData(&m_elevator); - frc::SmartDashboard::PutData(&m_wrist); - frc::SmartDashboard::PutData(&m_claw); - - m_drivetrain.SetDefaultCommand( - TankDrive([this] { return -m_joy.GetLeftY(); }, - [this] { return -m_joy.GetRightY(); }, m_drivetrain)); - - // Configure the button bindings - ConfigureButtonBindings(); -} - -void RobotContainer::ConfigureButtonBindings() { - // Configure your button bindings here - frc2::JoystickButton(&m_joy, 5).OnTrue( - SetElevatorSetpoint(0.25, m_elevator).ToPtr()); - frc2::JoystickButton(&m_joy, 6).OnTrue(CloseClaw(m_claw).ToPtr()); - frc2::JoystickButton(&m_joy, 7).OnTrue( - SetElevatorSetpoint(0.0, m_elevator).ToPtr()); - frc2::JoystickButton(&m_joy, 8).OnTrue(OpenClaw(m_claw).ToPtr()); - frc2::JoystickButton(&m_joy, 9).OnTrue( - Autonomous(m_claw, m_wrist, m_elevator, m_drivetrain).ToPtr()); - frc2::JoystickButton(&m_joy, 10) - .OnTrue(Pickup(m_claw, m_wrist, m_elevator).ToPtr()); - frc2::JoystickButton(&m_joy, 11) - .OnTrue(Place(m_claw, m_wrist, m_elevator).ToPtr()); - frc2::JoystickButton(&m_joy, 12) - .OnTrue(PrepareToPickup(m_claw, m_wrist, m_elevator).ToPtr()); -} - -frc2::Command* RobotContainer::GetAutonomousCommand() { - // An example command will be run in autonomous - return &m_autonomousCommand; -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp deleted file mode 100644 index d6434d1da7..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp +++ /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. - -#include "commands/Autonomous.h" - -#include - -#include "commands/CloseClaw.h" -#include "commands/DriveStraight.h" -#include "commands/Pickup.h" -#include "commands/Place.h" -#include "commands/PrepareToPickup.h" -#include "commands/SetDistanceToBox.h" -#include "commands/SetWristSetpoint.h" - -Autonomous::Autonomous(Claw& claw, Wrist& wrist, Elevator& elevator, - Drivetrain& drivetrain) { - SetName("Autonomous"); - AddCommands( - // clang-format off - PrepareToPickup(claw, wrist, elevator), - Pickup(claw, wrist, elevator), - SetDistanceToBox(0.10, drivetrain), - // DriveStraight(4, drivetrain) // Use encoders if ultrasonic is broken - Place(claw, wrist, elevator), - SetDistanceToBox(0.6, drivetrain), - // DriveStraight(-2, drivetrain) // Use encoders if ultrasonic is broken - frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist), - CloseClaw(claw))); - // clang-format on -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp deleted file mode 100644 index a3c1469f9a..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp +++ /dev/null @@ -1,34 +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/CloseClaw.h" - -#include - -#include "Robot.h" - -CloseClaw::CloseClaw(Claw& claw) : m_claw(&claw) { - SetName("CloseClaw"); - AddRequirements(m_claw); -} - -// Called just before this Command runs the first time -void CloseClaw::Initialize() { - m_claw->Close(); -} - -// Make this return true when this Command no longer needs to run execute() -bool CloseClaw::IsFinished() { - return m_claw->IsGripping(); -} - -// Called once after isFinished returns true -void CloseClaw::End(bool) { - // NOTE: Doesn't stop in simulation due to lower friction causing the can to - // fall out + there is no need to worry about stalling the motor or crushing - // the can. - if constexpr (frc::RobotBase::IsSimulation()) { - m_claw->Stop(); - } -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp deleted file mode 100644 index 6325307c2b..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp +++ /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. - -#include "commands/DriveStraight.h" - -#include - -#include "Robot.h" - -DriveStraight::DriveStraight(double distance, Drivetrain& drivetrain) - : frc2::CommandHelper{ - frc::PIDController{4, 0, 0}, - [&drivetrain] { return drivetrain.GetDistance(); }, - distance, - [&drivetrain](double output) { drivetrain.Drive(output, output); }, - {&drivetrain}}, - m_drivetrain{&drivetrain} { - m_controller.SetTolerance(0.01); -} - -// Called just before this Command runs the first time -void DriveStraight::Initialize() { - // Get everything in a safe starting state. - m_drivetrain->Reset(); - frc2::PIDCommand::Initialize(); -} - -bool DriveStraight::IsFinished() { - return m_controller.AtSetpoint(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp deleted file mode 100644 index 371ba85b99..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp +++ /dev/null @@ -1,24 +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/OpenClaw.h" - -#include "Robot.h" - -OpenClaw::OpenClaw(Claw& claw) - : frc2::CommandHelper(1_s), m_claw(&claw) { - SetName("OpenClaw"); - AddRequirements(m_claw); -} - -// Called just before this Command runs the first time -void OpenClaw::Initialize() { - m_claw->Open(); - frc2::WaitCommand::Initialize(); -} - -// Called once after isFinished returns true -void OpenClaw::End(bool) { - m_claw->Stop(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp deleted file mode 100644 index 3349f6256f..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp +++ /dev/null @@ -1,18 +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/Pickup.h" - -#include - -#include "commands/CloseClaw.h" -#include "commands/SetElevatorSetpoint.h" -#include "commands/SetWristSetpoint.h" - -Pickup::Pickup(Claw& claw, Wrist& wrist, Elevator& elevator) { - SetName("Pickup"); - AddCommands(CloseClaw(claw), - frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist), - SetElevatorSetpoint(0.25, elevator))); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp deleted file mode 100644 index 2fae0f27b0..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp +++ /dev/null @@ -1,18 +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/Place.h" - -#include "commands/OpenClaw.h" -#include "commands/SetElevatorSetpoint.h" -#include "commands/SetWristSetpoint.h" - -Place::Place(Claw& claw, Wrist& wrist, Elevator& elevator) { - SetName("Place"); - // clang-format off - AddCommands(SetElevatorSetpoint(0.25, elevator), - SetWristSetpoint(0, wrist), - OpenClaw(claw)); - // clang-format on -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp deleted file mode 100644 index 6f01c8ac88..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp +++ /dev/null @@ -1,18 +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/PrepareToPickup.h" - -#include - -#include "commands/OpenClaw.h" -#include "commands/SetElevatorSetpoint.h" -#include "commands/SetWristSetpoint.h" - -PrepareToPickup::PrepareToPickup(Claw& claw, Wrist& wrist, Elevator& elevator) { - SetName("PrepareToPickup"); - AddCommands(OpenClaw(claw), - frc2::ParallelCommandGroup(SetElevatorSetpoint(0, elevator), - SetWristSetpoint(0, wrist))); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp deleted file mode 100644 index 1ef5cbbf4f..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp +++ /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. - -#include "commands/SetDistanceToBox.h" - -#include - -#include "Robot.h" - -SetDistanceToBox::SetDistanceToBox(double distance, Drivetrain& drivetrain) - : frc2::CommandHelper{ - frc::PIDController{-2, 0, 0}, - [&drivetrain] { return drivetrain.GetDistanceToObstacle(); }, - distance, - [&drivetrain](double output) { drivetrain.Drive(output, output); }, - {&drivetrain}}, - m_drivetrain{&drivetrain} { - m_controller.SetTolerance(0.01); -} - -// Called just before this Command runs the first time -void SetDistanceToBox::Initialize() { - // Get everything in a safe starting state. - m_drivetrain->Reset(); - frc2::PIDCommand::Initialize(); -} - -bool SetDistanceToBox::IsFinished() { - return m_controller.AtSetpoint(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp deleted file mode 100644 index d8a2f08d4d..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp +++ /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. - -#include "commands/SetElevatorSetpoint.h" - -#include - -#include "Robot.h" - -SetElevatorSetpoint::SetElevatorSetpoint(double setpoint, Elevator& elevator) - : m_setpoint(setpoint), m_elevator(&elevator) { - SetName("SetElevatorSetpoint"); - AddRequirements(m_elevator); -} - -// Called just before this Command runs the first time -void SetElevatorSetpoint::Initialize() { - m_elevator->SetSetpoint(m_setpoint); - m_elevator->Enable(); -} - -// Make this return true when this Command no longer needs to run execute() -bool SetElevatorSetpoint::IsFinished() { - return m_elevator->GetController().AtSetpoint(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp deleted file mode 100644 index bed2ffe86d..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp +++ /dev/null @@ -1,24 +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/SetWristSetpoint.h" - -#include "Robot.h" - -SetWristSetpoint::SetWristSetpoint(double setpoint, Wrist& wrist) - : m_setpoint(setpoint), m_wrist(&wrist) { - SetName("SetWristSetpoint"); - AddRequirements(m_wrist); -} - -// Called just before this Command runs the first time -void SetWristSetpoint::Initialize() { - m_wrist->SetSetpoint(m_setpoint); - m_wrist->Enable(); -} - -// Make this return true when this Command no longer needs to run execute() -bool SetWristSetpoint::IsFinished() { - return m_wrist->GetController().AtSetpoint(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp deleted file mode 100644 index 7021f9d5fb..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.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/TankDrive.h" - -#include - -#include "Robot.h" - -TankDrive::TankDrive(std::function left, - std::function right, Drivetrain& drivetrain) - : m_left(std::move(left)), - m_right(std::move(right)), - m_drivetrain(&drivetrain) { - SetName("TankDrive"); - AddRequirements(m_drivetrain); -} - -// Called repeatedly when this Command is scheduled to run -void TankDrive::Execute() { - m_drivetrain->Drive(m_left(), m_right()); -} - -// Make this return true when this Command no longer needs to run execute() -bool TankDrive::IsFinished() { - return false; -} - -// Called once after isFinished returns true -void TankDrive::End(bool) { - m_drivetrain->Drive(0, 0); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp deleted file mode 100644 index fa0e94df5c..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp +++ /dev/null @@ -1,37 +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/Claw.h" - -#include - -Claw::Claw() { - // Let's show everything on the LiveWindow - SetName("Claw"); - AddChild("Motor", &m_motor); -} - -void Claw::Open() { - m_motor.Set(-1); -} - -void Claw::Close() { - m_motor.Set(1); -} - -void Claw::Stop() { - m_motor.Set(0); -} - -bool Claw::IsGripping() const { - return m_contact.Get(); -} - -void Claw::Log() { - frc::SmartDashboard::PutBoolean("Claw switch", IsGripping()); -} - -void Claw::Periodic() { - Log(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp deleted file mode 100644 index e6109bf61a..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp +++ /dev/null @@ -1,86 +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/Drivetrain.h" - -#include - -#include -#include -#include -#include - -Drivetrain::Drivetrain() { - wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft); - wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight); - - m_frontLeft.AddFollower(m_rearLeft); - m_frontRight.AddFollower(m_rearRight); - - // 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_frontRight.SetInverted(true); - - // Encoders may measure differently in the real world and in simulation. In - // this example the robot moves 0.042 barleycorns per tick in the real world, - // but the simulated encoders simulate 360 tick encoders. This if statement - // allows for the real robot to handle this difference in devices. - if constexpr (frc::RobotBase::IsSimulation()) { - m_leftEncoder.SetDistancePerPulse(0.042); - m_rightEncoder.SetDistancePerPulse(0.042); - } else { - // Circumference = diameter * pi. 360 tick simulated encoders. - m_leftEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() * - std::numbers::pi / 360.0); - m_rightEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() * - std::numbers::pi / 360.0); - } - SetName("Drivetrain"); - // Let's show everything on the LiveWindow - AddChild("Front_Left Motor", &m_frontLeft); - AddChild("Rear Left Motor", &m_rearLeft); - AddChild("Front Right Motor", &m_frontRight); - AddChild("Rear Right Motor", &m_rearRight); - AddChild("Left Encoder", &m_leftEncoder); - AddChild("Right Encoder", &m_rightEncoder); - AddChild("Rangefinder", &m_rangefinder); - AddChild("Gyro", &m_gyro); -} - -void Drivetrain::Log() { - frc::SmartDashboard::PutNumber("Left Distance", m_leftEncoder.GetDistance()); - frc::SmartDashboard::PutNumber("Right Distance", - m_rightEncoder.GetDistance()); - frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate()); - frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate()); - frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle()); -} - -void Drivetrain::Drive(double left, double right) { - m_robotDrive.TankDrive(left, right); -} - -double Drivetrain::GetHeading() const { - return m_gyro.GetAngle(); -} - -void Drivetrain::Reset() { - m_gyro.Reset(); - m_leftEncoder.Reset(); - m_rightEncoder.Reset(); -} - -double Drivetrain::GetDistance() const { - return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0; -} - -double Drivetrain::GetDistanceToObstacle() const { - // Really meters in simulation since it's a rangefinder... - return m_rangefinder.GetAverageVoltage(); -} - -void Drivetrain::Periodic() { - Log(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp deleted file mode 100644 index d8c88d641d..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp +++ /dev/null @@ -1,40 +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/Elevator.h" - -#include -#include -#include - -Elevator::Elevator() - : frc2::PIDSubsystem{frc::PIDController{kP_real, kI_real, 0}} { - if constexpr (frc::RobotBase::IsSimulation()) { - // Check for simulation and update PID values - GetController().SetPID(kP_simulation, kI_simulation, 0); - } - - m_controller.SetTolerance(0.005); - - SetName("Elevator"); - // Let's show everything on the LiveWindow - AddChild("Motor", &m_motor); - AddChild("Pot", &m_pot); -} - -void Elevator::Log() { - frc::SmartDashboard::PutData("Wrist Pot", &m_pot); -} - -double Elevator::GetMeasurement() { - return m_pot.Get(); -} - -void Elevator::UseOutput(double output, double setpoint) { - m_motor.Set(output); -} - -void Elevator::Periodic() { - Log(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp deleted file mode 100644 index c0e06af754..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.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 "subsystems/Wrist.h" - -#include -#include - -Wrist::Wrist() : frc2::PIDSubsystem{frc::PIDController{kP, 0, 0}} { - m_controller.SetTolerance(2.5); - - SetName("Wrist"); - // Let's show everything on the LiveWindow - AddChild("Motor", &m_motor); - AddChild("Pot", &m_pot); -} - -void Wrist::Log() { - frc::SmartDashboard::PutNumber("Wrist Angle", GetMeasurement()); -} - -double Wrist::GetMeasurement() { - return m_pot.Get(); -} - -void Wrist::UseOutput(double output, double setpoint) { - m_motor.Set(output); -} - -void Wrist::Periodic() { - Log(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h deleted file mode 100644 index b670df4cd8..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.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 "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. - frc2::Command* m_autonomousCommand = nullptr; - - RobotContainer m_container; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h deleted file mode 100644 index ae1ec9d159..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h +++ /dev/null @@ -1,41 +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 "commands/Autonomous.h" -#include "subsystems/Claw.h" -#include "subsystems/Drivetrain.h" -#include "subsystems/Elevator.h" -#include "subsystems/Wrist.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::Command* GetAutonomousCommand(); - - private: - // The robot's subsystems and commands are defined here... - frc::XboxController m_joy{0}; - - Claw m_claw; - Wrist m_wrist; - Elevator m_elevator; - Drivetrain m_drivetrain; - - Autonomous m_autonomousCommand; - - void ConfigureButtonBindings(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h deleted file mode 100644 index a9a5bf47e6..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h +++ /dev/null @@ -1,23 +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/Claw.h" -#include "subsystems/Drivetrain.h" -#include "subsystems/Elevator.h" -#include "subsystems/Wrist.h" - -/** - * The main autonomous command to pickup and deliver the soda to the box. - */ -class Autonomous - : public frc2::CommandHelper { - public: - Autonomous(Claw& claw, Wrist& wrist, Elevator& elevator, - Drivetrain& drivetrain); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h deleted file mode 100644 index 31e2b92064..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h +++ /dev/null @@ -1,24 +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/Claw.h" - -/** - * Closes the claw until the limit switch is tripped. - */ -class CloseClaw : public frc2::CommandHelper { - public: - explicit CloseClaw(Claw& claw); - void Initialize() override; - bool IsFinished() override; - void End(bool interrupted) override; - - private: - Claw* m_claw; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h deleted file mode 100644 index c196232c56..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h +++ /dev/null @@ -1,27 +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/Drivetrain.h" - -/** - * Drive the given distance straight (negative values go backwards). - * Uses a local PID controller to run a simple PID loop that is only - * enabled while this command is running. The input is the averaged - * values of the left and right encoders. - */ -class DriveStraight - : public frc2::CommandHelper { - public: - explicit DriveStraight(double distance, Drivetrain& drivetrain); - void Initialize() override; - bool IsFinished() override; - - private: - Drivetrain* m_drivetrain; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h deleted file mode 100644 index e6fe9623a9..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h +++ /dev/null @@ -1,24 +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/Claw.h" - -/** - * Opens the claw for one second. Real robots should use sensors, stalling - * motors is BAD! - */ -class OpenClaw : public frc2::CommandHelper { - public: - explicit OpenClaw(Claw& claw); - void Initialize() override; - void End(bool interrupted) override; - - private: - Claw* m_claw; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h deleted file mode 100644 index ba0b02f288..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h +++ /dev/null @@ -1,22 +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/Claw.h" -#include "subsystems/Elevator.h" -#include "subsystems/Wrist.h" - -/** - * Pickup a soda can (if one is between the open claws) and - * get it in a safe state to drive around. - */ -class Pickup - : public frc2::CommandHelper { - public: - Pickup(Claw& claw, Wrist& wrist, Elevator& elevator); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h deleted file mode 100644 index 02b043f7a8..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h +++ /dev/null @@ -1,20 +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/Claw.h" -#include "subsystems/Elevator.h" -#include "subsystems/Wrist.h" - -/** - * Place a held soda can onto the platform. - */ -class Place : public frc2::CommandHelper { - public: - Place(Claw& claw, Wrist& wrist, Elevator& elevator); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h deleted file mode 100644 index b219cf05de..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h +++ /dev/null @@ -1,21 +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/Claw.h" -#include "subsystems/Elevator.h" -#include "subsystems/Wrist.h" - -/** - * Make sure the robot is in a state to pickup soda cans. - */ -class PrepareToPickup : public frc2::CommandHelper { - public: - PrepareToPickup(Claw& claw, Wrist& wrist, Elevator& elevator); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h deleted file mode 100644 index 1d2672a602..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h +++ /dev/null @@ -1,27 +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/Drivetrain.h" - -/** - * Drive until the robot is the given distance away from the box. Uses a local - * PID controller to run a simple PID loop that is only enabled while this - * command is running. The input is the averaged values of the left and right - * encoders. - */ -class SetDistanceToBox - : public frc2::CommandHelper { - public: - explicit SetDistanceToBox(double distance, Drivetrain& drivetrain); - void Initialize() override; - bool IsFinished() override; - - private: - Drivetrain* m_drivetrain; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h deleted file mode 100644 index 3e8c18c33f..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h +++ /dev/null @@ -1,29 +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/Elevator.h" - -/** - * Move the elevator to a given location. This command finishes when it is - * within - * the tolerance, but leaves the PID loop running to maintain the position. - * Other - * commands using the elevator should make sure they disable PID! - */ -class SetElevatorSetpoint - : public frc2::CommandHelper { - public: - explicit SetElevatorSetpoint(double setpoint, Elevator& elevator); - void Initialize() override; - bool IsFinished() override; - - private: - double m_setpoint; - Elevator* m_elevator; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h deleted file mode 100644 index 1494ad6dcc..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h +++ /dev/null @@ -1,27 +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/Wrist.h" - -/** - * Move the wrist to a given angle. This command finishes when it is within - * the tolerance, but leaves the PID loop running to maintain the position. - * Other commands using the wrist should make sure they disable PID! - */ -class SetWristSetpoint - : public frc2::CommandHelper { - public: - explicit SetWristSetpoint(double setpoint, Wrist& wrist); - void Initialize() override; - bool IsFinished() override; - - private: - double m_setpoint; - Wrist* m_wrist; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h deleted file mode 100644 index 932c427d5c..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h +++ /dev/null @@ -1,29 +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 "subsystems/Drivetrain.h" - -/** - * Have the robot drive tank style using the PS3 Joystick until interrupted. - */ -class TankDrive : public frc2::CommandHelper { - public: - TankDrive(std::function left, std::function right, - Drivetrain& drivetrain); - void Execute() override; - bool IsFinished() override; - void End(bool interrupted) override; - - private: - std::function m_left; - std::function m_right; - Drivetrain* m_drivetrain; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h deleted file mode 100644 index c6f17d84a2..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h +++ /dev/null @@ -1,55 +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 claw subsystem is a simple system with a motor for opening and closing. - * If using stronger motors, you should probably use a sensor so that the - * motors don't stall. - */ -class Claw : public frc2::SubsystemBase { - public: - Claw(); - - /** - * Set the claw motor to move in the open direction. - */ - void Open(); - - /** - * Set the claw motor to move in the close direction. - */ - void Close(); - - /** - * Stops the claw motor from moving. - */ - void Stop(); - - /** - * Return true when the robot is grabbing an object hard enough - * to trigger the limit switch. - */ - bool IsGripping() const; - - /** - * The log method puts interesting information to the SmartDashboard. - */ - void Log(); - - /** - * Log the data periodically. This method is automatically called - * by the subsystem. - */ - void Periodic() override; - - private: - frc::PWMSparkMax m_motor{7}; - frc::DigitalInput m_contact{5}; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h deleted file mode 100644 index cc3c95f9b3..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h +++ /dev/null @@ -1,80 +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 - -namespace frc { -class Joystick; -} // namespace frc - -/** - * The Drivetrain subsystem incorporates the sensors and actuators attached to - * the robots chassis. These include four drive motors, a left and right encoder - * and a gyro. - */ -class Drivetrain : public frc2::SubsystemBase { - public: - Drivetrain(); - - /** - * The log method puts interesting information to the SmartDashboard. - */ - void Log(); - - /** - * Tank style driving for the Drivetrain. - * @param left Speed in range [-1,1] - * @param right Speed in range [-1,1] - */ - void Drive(double left, double right); - - /** - * @return The robots heading in degrees. - */ - double GetHeading() const; - - /** - * Reset the robots sensors to the zero states. - */ - void Reset(); - - /** - * @return The distance driven (average of left and right encoders). - */ - double GetDistance() const; - - /** - * @return The distance to the obstacle detected by the rangefinder. - */ - double GetDistanceToObstacle() const; - - /** - * Log the data periodically. This method is automatically called - * by the subsystem. - */ - void Periodic() override; - - private: - frc::PWMSparkMax m_frontLeft{1}; - frc::PWMSparkMax m_rearLeft{2}; - - frc::PWMSparkMax m_frontRight{3}; - frc::PWMSparkMax m_rearRight{4}; - - frc::DifferentialDrive m_robotDrive{ - [&](double output) { m_frontLeft.Set(output); }, - [&](double output) { m_frontRight.Set(output); }}; - - frc::Encoder m_leftEncoder{1, 2}; - frc::Encoder m_rightEncoder{3, 4}; - frc::AnalogInput m_rangefinder{6}; - frc::AnalogGyro m_gyro{1}; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h deleted file mode 100644 index a4c2c7dce4..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h +++ /dev/null @@ -1,59 +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 - -/** - * The elevator subsystem uses PID to go to a given height. Unfortunately, in - * it's current - * state PID values for simulation are different than in the real world do to - * minor differences. - */ -class Elevator : public frc2::PIDSubsystem { - public: - Elevator(); - - /** - * The log method puts interesting information to the SmartDashboard. - */ - void Log(); - - /** - * Use the potentiometer as the PID sensor. This method is automatically - * called by the subsystem. - */ - double GetMeasurement() override; - - /** - * Use the motor as the PID output. This method is automatically called - * by - * the subsystem. - */ - void UseOutput(double output, double setpoint) override; - - /** - * Log the data periodically. This method is automatically called - * by the subsystem. - */ - void Periodic() override; - - private: - frc::PWMSparkMax m_motor{5}; - // Conversion value of potentiometer varies between the real world and - // simulation - frc::AnalogPotentiometer m_pot = - frc::RobotBase::IsReal() - ? frc::AnalogPotentiometer{2, -2.0 / 5} - : frc::AnalogPotentiometer{2}; // Defaults to meters - - static constexpr double kP_real = 4; - static constexpr double kI_real = 0.07; - static constexpr double kP_simulation = 18; - static constexpr double kI_simulation = 0.2; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h deleted file mode 100644 index 2222d5e44a..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h +++ /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. - -#pragma once - -#include -#include -#include -#include - -/** - * The wrist subsystem is like the elevator, but with a rotational joint instead - * of a linear joint. - */ -class Wrist : public frc2::PIDSubsystem { - public: - Wrist(); - - /** - * The log method puts interesting information to the SmartDashboard. - */ - void Log(); - - /** - * Use the potentiometer as the PID sensor. This method is automatically - * called by the subsystem. - */ - double GetMeasurement() override; - - /** - * Use the motor as the PID output. This method is automatically called - * by the subsystem. - */ - void UseOutput(double output, double setpoint) override; - - /** - * Log the data periodically. This method is automatically called - * by the subsystem. - */ - void Periodic() override; - - private: - frc::PWMSparkMax m_motor{6}; - // Conversion value of potentiometer varies between the real world and - // simulation - frc::AnalogPotentiometer m_pot = - frc::RobotBase::IsReal() - ? frc::AnalogPotentiometer{3, -270.0 / 5} - : frc::AnalogPotentiometer{3}; // Defaults to degrees - - static constexpr double kP = 1; -}; 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 142d59fa6b..0000000000 --- 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 7b076840b9..0000000000 --- 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 6d1272d17b..0000000000 --- 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 464e0cf5cb..0000000000 --- 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 a97a5c071f..0000000000 --- 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 4d25b0bfb9..0000000000 --- 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 2405694447..0000000000 --- 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 0ecb7286d9..0000000000 --- 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 6df79bfabc..0000000000 --- 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 f3136aafcd..0000000000 --- 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 d7cce6c05c..0000000000 --- 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/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 2aed399020..fd846b8468 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -319,24 +319,6 @@ "gradlebase": "cpp", "commandversion": 2 }, - { - "name": "GearsBot", - "description": "A fully functional Command-Based program for WPI's GearsBot robot.", - "tags": [ - "Complete Robot", - "Command-based", - "Differential Drive", - "Elevator", - "Arm", - "Analog", - "Digital Input", - "SmartDashboard", - "XboxController" - ], - "foldername": "GearsBot", - "gradlebase": "cpp", - "commandversion": 2 - }, { "name": "HAL", "description": "Use the low-level HAL C functions. This example is for advanced users.", @@ -428,38 +410,6 @@ "gradlebase": "cpp", "commandversion": 2 }, - { - "name": "Frisbeebot", - "description": "A simple frisbee shooter for the 2013 game, demonstrating use of PIDSubsystem.", - "tags": [ - "Complete Robot", - "Command-based", - "Differential Drive", - "Flywheel", - "Encoder", - "XboxController", - "PID" - ], - "foldername": "Frisbeebot", - "gradlebase": "cpp", - "commandversion": 2 - }, - { - "name": "Gyro Drive Commands", - "description": "Control a robot's angle with PID and a gyro, in command-based.", - "tags": [ - "Command-based", - "Differential Drive", - "Encoder", - "PS4Controller", - "PID", - "Profiled PID", - "Gyro" - ], - "foldername": "GyroDriveCommands", - "gradlebase": "cpp", - "commandversion": 2 - }, { "name": "SwerveBot", "description": "Use kinematics and odometry with a swerve drive.", @@ -609,36 +559,6 @@ "gradlebase": "cpp", "commandversion": 2 }, - { - "name": "ArmBot", - "description": "Control an arm with ProfiledPIDSubsystem.", - "tags": [ - "Command-based", - "Arm", - "Encoder", - "Profiled PID", - "XboxController", - "Differential Drive" - ], - "foldername": "ArmBot", - "gradlebase": "cpp", - "commandversion": 2 - }, - { - "name": "ArmBotOffboard", - "description": "Control an arm with TrapezoidProfileSubsystem and smart motor controller PID.", - "tags": [ - "Command-based", - "Arm", - "Smart Motor Controller", - "Trapezoid Profile", - "XboxController", - "Differential Drive" - ], - "foldername": "ArmBotOffboard", - "gradlebase": "cpp", - "commandversion": 2 - }, { "name": "DriveDistanceOffboard", "description": "Drive a differential drivetrain a set distance using TrapezoidProfileCommand and smart motor controller PID.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java deleted file mode 100644 index 0fe85f3243..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java +++ /dev/null @@ -1,65 +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.armbot; - -/** - * 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 class ArmConstants { - public static final int kMotorPort = 4; - - public static final double kP = 1; - - // These are fake gains; in actuality these must be determined individually for each robot - public static final double kSVolts = 1; - public static final double kGVolts = 1; - public static final double kVVoltSecondPerRad = 0.5; - public static final double kAVoltSecondSquaredPerRad = 0.1; - - public static final double kMaxVelocityRadPerSecond = 3; - public static final double kMaxAccelerationRadPerSecSquared = 10; - - public static final int[] kEncoderPorts = new int[] {4, 5}; - public static final int kEncoderPPR = 256; - public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR; - - // The offset of the arm from the horizontal in its neutral position, - // measured from the horizontal - public static final double kArmOffsetRads = 0.5; - } - - public static final class AutoConstants { - public static final double kAutoTimeoutSeconds = 12; - public static final double kAutoShootTimeSeconds = 7; - } - - public static final class OIConstants { - public static final int kDriverControllerPort = 0; - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java deleted file mode 100644 index 4d353a6fe4..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/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.armbot; - -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/armbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java deleted file mode 100644 index 7bb15954cc..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/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.armbot; - -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() { - m_robotContainer.disablePIDSubsystems(); - } - - @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/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java deleted file mode 100644 index 1875d64dd1..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java +++ /dev/null @@ -1,103 +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.armbot; - -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants; -import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem; -import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -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(); - private final ArmSubsystem m_robotArm = new ArmSubsystem(); - - // The driver's controller - CommandXboxController m_driverController = - new CommandXboxController(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. - Commands.run( - () -> - 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 passing it to a {@link - * JoystickButton}. - */ - private void configureButtonBindings() { - // Move the arm to 2 radians above horizontal when the 'A' button is pressed. - m_driverController - .a() - .onTrue( - Commands.runOnce( - () -> { - m_robotArm.setGoal(2); - m_robotArm.enable(); - }, - m_robotArm)); - - // Move the arm to neutral position when the 'B' button is pressed. - m_driverController - .b() - .onTrue( - Commands.runOnce( - () -> { - m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads); - m_robotArm.enable(); - }, - m_robotArm)); - - // Disable the arm controller when Y is pressed. - m_driverController.y().onTrue(Commands.runOnce(m_robotArm::disable)); - - // Drive at half speed when the bumper is held - m_driverController - .rightBumper() - .onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5))) - .onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1.0))); - } - - /** - * Disables all ProfiledPIDSubsystem and PIDSubsystem instances. This should be called on robot - * disable to prevent integral windup. - */ - public void disablePIDSubsystems() { - m_robotArm.disable(); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return Commands.none(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java deleted file mode 100644 index 0ee307865c..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java +++ /dev/null @@ -1,60 +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.armbot.subsystems; - -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; - -/** A robot arm subsystem that moves with a motion profile. */ -public class ArmSubsystem extends ProfiledPIDSubsystem { - private final PWMSparkMax m_motor = new PWMSparkMax(ArmConstants.kMotorPort); - private final Encoder m_encoder = - new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]); - private final ArmFeedforward m_feedforward = - new ArmFeedforward( - ArmConstants.kSVolts, ArmConstants.kGVolts, - ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad); - - /** Create a new ArmSubsystem. */ - public ArmSubsystem() { - super( - new ProfiledPIDController( - ArmConstants.kP, - 0, - 0, - new TrapezoidProfile.Constraints( - ArmConstants.kMaxVelocityRadPerSecond, - ArmConstants.kMaxAccelerationRadPerSecSquared)), - 0); - m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse); - // Start arm at rest in neutral position - setGoal(ArmConstants.kArmOffsetRads); - } - - @Override - public void useOutput(double output, TrapezoidProfile.State setpoint) { - // Calculate the feedforward from the sepoint - double feedforward = - m_feedforward - .calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity)) - .in(Volts); - // Add the feedforward to the PID output to get the motor output - m_motor.setVoltage(output + feedforward); - } - - @Override - public double getMeasurement() { - return m_encoder.getDistance() + ArmConstants.kArmOffsetRads; - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java deleted file mode 100644 index 246b5c4118..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java +++ /dev/null @@ -1,110 +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.armbot.subsystems; - -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.examples.armbot.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); - - /** Creates a new DriveSubsystem. */ - public DriveSubsystem() { - SendableRegistry.addChild(m_drive, m_leftLeader); - SendableRegistry.addChild(m_drive, m_rightLeader); - - // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); - - 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); - } - - /** - * 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); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java deleted file mode 100644 index f27aebafe4..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java +++ /dev/null @@ -1,56 +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.armbotoffboard; - -/** - * 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 class ArmConstants { - public static final int kMotorPort = 4; - - public static final double kP = 1; - - // These are fake gains; in actuality these must be determined individually for each robot - public static final double kSVolts = 1; - public static final double kGVolts = 1; - public static final double kVVoltSecondPerRad = 0.5; - public static final double kAVoltSecondSquaredPerRad = 0.1; - - public static final double kMaxVelocityRadPerSecond = 3; - public static final double kMaxAccelerationRadPerSecSquared = 10; - - // The offset of the arm from the horizontal in its neutral position, - // measured from the horizontal - public static final double kArmOffsetRads = 0.5; - } - - public static final class OIConstants { - public static final int kDriverControllerPort = 0; - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java deleted file mode 100644 index 7801a1fa8a..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.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.armbotoffboard; - -/** - * A simplified stub class that simulates the API of a common "smart" motor controller. - * - *

Has no actual functionality. - */ -public class ExampleSmartMotorController { - public enum PIDMode { - kPosition, - kVelocity, - kMovementWitchcraft - } - - /** - * Creates a new ExampleSmartMotorController. - * - * @param port The port for the controller. - */ - @SuppressWarnings("PMD.UnusedFormalParameter") - public ExampleSmartMotorController(int port) {} - - /** - * Example method for setting the PID gains of the smart controller. - * - * @param kp The proportional gain. - * @param ki The integral gain. - * @param kd The derivative gain. - */ - public void setPID(double kp, double ki, double kd) {} - - /** - * Example method for setting the setpoint of the smart controller in PID mode. - * - * @param mode The mode of the PID controller. - * @param setpoint The controller setpoint. - * @param arbFeedforward An arbitrary feedforward output (from -1 to 1). - */ - public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {} - - /** - * Places this motor controller in follower mode. - * - * @param leader The leader to follow. - */ - public void follow(ExampleSmartMotorController leader) {} - - /** - * Returns the encoder distance. - * - * @return The current encoder distance. - */ - public double getEncoderDistance() { - return 0; - } - - /** - * Returns the encoder rate. - * - * @return The current encoder rate. - */ - public double getEncoderRate() { - return 0; - } - - /** Resets the encoder to zero distance. */ - public void resetEncoder() {} - - public void set(double speed) {} - - public double get() { - return 0; - } - - public void setInverted(boolean isInverted) {} - - public boolean getInverted() { - return false; - } - - public void disable() {} - - public void stopMotor() {} -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java deleted file mode 100644 index ad36fce5a8..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/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.armbotoffboard; - -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/armbotoffboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java deleted file mode 100644 index fb416f0eaf..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/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.armbotoffboard; - -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/armbotoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java deleted file mode 100644 index 03f775c9b0..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java +++ /dev/null @@ -1,75 +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.armbotoffboard; - -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants; -import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem; -import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -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(); - private final ArmSubsystem m_robotArm = new ArmSubsystem(); - - // The driver's controller - CommandXboxController m_driverController = - new CommandXboxController(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. - m_robotDrive.arcadeDriveCommand( - () -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX())); - } - - /** - * 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 passing it to a {@link - * JoystickButton}. - */ - private void configureButtonBindings() { - // Move the arm to 2 radians above horizontal when the 'A' button is pressed. - m_driverController.a().onTrue(m_robotArm.setArmGoalCommand(2)); - - // Move the arm to neutral position when the 'B' button is pressed. - m_driverController - .b() - .onTrue(m_robotArm.setArmGoalCommand(Constants.ArmConstants.kArmOffsetRads)); - - // Drive at half speed when the bumper is held - m_driverController - .rightBumper() - .onTrue(m_robotDrive.limitOutputCommand(0.5)) - .onFalse(m_robotDrive.limitOutputCommand(1)); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return Commands.none(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java deleted file mode 100644 index ed533dff25..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.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.armbotoffboard.subsystems; - -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants; -import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; - -/** A robot arm subsystem that moves with a motion profile. */ -public class ArmSubsystem extends TrapezoidProfileSubsystem { - private final ExampleSmartMotorController m_motor = - new ExampleSmartMotorController(ArmConstants.kMotorPort); - private final ArmFeedforward m_feedforward = - new ArmFeedforward( - ArmConstants.kSVolts, ArmConstants.kGVolts, - ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad); - - /** Create a new ArmSubsystem. */ - public ArmSubsystem() { - super( - new TrapezoidProfile.Constraints( - ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared), - ArmConstants.kArmOffsetRads); - m_motor.setPID(ArmConstants.kP, 0, 0); - } - - @Override - public void useState(TrapezoidProfile.State setpoint) { - // Calculate the feedforward from the sepoint - double feedforward = - m_feedforward - .calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity)) - .in(Volts); - // Add the feedforward to the PID output to get the motor output - m_motor.setSetpoint( - ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0); - } - - public Command setArmGoalCommand(double kArmOffsetRads) { - return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java deleted file mode 100644 index f01ad9987d..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java +++ /dev/null @@ -1,114 +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.armbotoffboard.subsystems; - -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.DoubleSupplier; - -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); - - /** Creates a new DriveSubsystem. */ - public DriveSubsystem() { - SendableRegistry.addChild(m_drive, m_leftLeader); - SendableRegistry.addChild(m_drive, m_rightLeader); - - // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); - - 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); - } - - /** - * A split-stick arcade command, with forward/backward controlled by the left hand, and turning - * controlled by the right. - * - * @param fwd supplier for the commanded forward movement - * @param rot supplier for the commanded rotation - */ - public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) { - return Commands.run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()), this); - } - - /** 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 Command limitOutputCommand(double maxOutput) { - return Commands.runOnce(() -> m_drive.setMaxOutput(maxOutput)); - } -} 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 720ab3c0be..1b513b4fdc 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 @@ -276,25 +276,6 @@ "mainclass": "Main", "commandversion": 2 }, - { - "name": "GearsBot", - "description": "A fully functional Command-Based program for WPI's GearsBot robot.", - "tags": [ - "Complete Robot", - "Command-based", - "Differential Drive", - "Elevator", - "Arm", - "Analog", - "Digital Input", - "SmartDashboard", - "XboxController" - ], - "foldername": "gearsbot", - "gradlebase": "java", - "mainclass": "Main", - "commandversion": 2 - }, { "name": "Simple Vision", "description": "Use the CameraServer class to stream from a USB Webcam without processing the images.", @@ -425,40 +406,6 @@ "mainclass": "Main", "commandversion": 2 }, - { - "name": "Frisbeebot", - "description": "A simple frisbee shooter for the 2013 game, demonstrating use of PIDSubsystem.", - "tags": [ - "Complete Robot", - "Command-based", - "Differential Drive", - "Flywheel", - "Encoder", - "XboxController", - "PID" - ], - "foldername": "frisbeebot", - "gradlebase": "java", - "mainclass": "Main", - "commandversion": 2 - }, - { - "name": "Gyro Drive Commands", - "description": "Control a robot's angle with PID and a gyro, in command-based.", - "tags": [ - "Command-based", - "Differential Drive", - "Encoder", - "PS4Controller", - "PID", - "Profiled PID", - "Gyro" - ], - "foldername": "gyrodrivecommands", - "gradlebase": "java", - "mainclass": "Main", - "commandversion": 2 - }, { "name": "SwerveBot", "description": "Use kinematics and odometry with a swerve drive.", @@ -583,38 +530,6 @@ "mainclass": "Main", "commandversion": 2 }, - { - "name": "ArmBot", - "description": "Control an arm with ProfiledPIDSubsystem.", - "tags": [ - "Command-based", - "Arm", - "Encoder", - "Profiled PID", - "XboxController", - "Differential Drive" - ], - "foldername": "armbot", - "gradlebase": "java", - "mainclass": "Main", - "commandversion": 2 - }, - { - "name": "ArmBotOffboard", - "description": "Control an arm with TrapezoidProfileSubsystem and smart motor controller PID.", - "tags": [ - "Command-based", - "Arm", - "Smart Motor Controller", - "Trapezoid Profile", - "XboxController", - "Differential Drive" - ], - "foldername": "armbotoffboard", - "gradlebase": "java", - "mainclass": "Main", - "commandversion": 2 - }, { "name": "DriveDistanceOffboard", "description": "Drive a differential drivetrain a set distance using TrapezoidProfileCommand and smart motor controller PID.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java deleted file mode 100644 index e3126cdd23..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java +++ /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. - -package edu.wpi.first.wpilibj.examples.frisbeebot; - -/** - * 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 class ShooterConstants { - public static final int[] kEncoderPorts = new int[] {4, 5}; - public static final boolean kEncoderReversed = false; - public static final int kEncoderCPR = 1024; - public static final double kEncoderDistancePerPulse = - // Distance units will be rotations - 1.0 / kEncoderCPR; - - public static final int kShooterMotorPort = 4; - public static final int kFeederMotorPort = 5; - - public static final double kShooterFreeRPS = 5300; - public static final double kShooterTargetRPS = 4000; - public static final double kShooterToleranceRPS = 50; - - // These are not real PID gains, and will have to be tuned for your specific robot. - public static final double kP = 1; - public static final double kI = 0; - public static final double kD = 0; - - // On a real robot the feedforward constants should be empirically determined; these are - // reasonable guesses. - public static final double kSVolts = 0.05; - public static final double kVVoltSecondsPerRotation = - // Should have value 12V at free speed... - 12.0 / kShooterFreeRPS; - - public static final double kFeederSpeed = 0.5; - } - - public static final class AutoConstants { - public static final double kAutoTimeoutSeconds = 12; - public static final double kAutoShootTimeSeconds = 7; - } - - public static final class OIConstants { - public static final int kDriverControllerPort = 0; - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java deleted file mode 100644 index 48c3f40e49..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/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.frisbeebot; - -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/frisbeebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java deleted file mode 100644 index 176f4ea56d..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/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.frisbeebot; - -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/frisbeebot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java deleted file mode 100644 index 8e052bbd52..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java +++ /dev/null @@ -1,124 +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.frisbeebot; - -import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants; -import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants; -import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem; -import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; - -/** - * 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(); - private final ShooterSubsystem m_shooter = new ShooterSubsystem(); - - private final Command m_spinUpShooter = Commands.runOnce(m_shooter::enable, m_shooter); - private final Command m_stopShooter = Commands.runOnce(m_shooter::disable, m_shooter); - - // An autonomous routine that shoots the loaded frisbees - Command m_autonomousCommand = - Commands.sequence( - // Start the command by spinning up the shooter... - Commands.runOnce(m_shooter::enable, m_shooter), - // Wait until the shooter is at speed before feeding the frisbees - Commands.waitUntil(m_shooter::atSetpoint), - // Start running the feeder - Commands.runOnce(m_shooter::runFeeder, m_shooter), - // Shoot for the specified time - Commands.waitSeconds(AutoConstants.kAutoShootTimeSeconds)) - // Add a timeout (will end the command if, for instance, the shooter - // never gets up to speed) - .withTimeout(AutoConstants.kAutoTimeoutSeconds) - // When the command ends, turn off the shooter and the feeder - .andThen( - Commands.runOnce( - () -> { - m_shooter.disable(); - m_shooter.stopFeeder(); - })); - - // The driver's controller - CommandXboxController m_driverController = - new CommandXboxController(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. - Commands.run( - () -> - m_robotDrive.arcadeDrive( - -m_driverController.getLeftY(), -m_driverController.getRightX()), - m_robotDrive)); - } - - /** - * Use this method to define your button->command mappings. Buttons can be created via the button - * factories on {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID} or one of its - * subclasses ({@link edu.wpi.first.wpilibj2.command.button.CommandJoystick} or {@link - * CommandXboxController}). - */ - private void configureButtonBindings() { - // Configure your button bindings here - - // We can bind commands while retaining references to them in RobotContainer - - // Spin up the shooter when the 'A' button is pressed - m_driverController.a().onTrue(m_spinUpShooter); - - // Turn off the shooter when the 'B' button is pressed - m_driverController.b().onTrue(m_stopShooter); - - // We can also write them as temporary variables outside the bindings - - // Shoots if the shooter wheel has reached the target speed - Command shoot = - Commands.either( - // Run the feeder - Commands.runOnce(m_shooter::runFeeder, m_shooter), - // Do nothing - Commands.none(), - // Determine which of the above to do based on whether the shooter has reached the - // desired speed - m_shooter::atSetpoint); - - Command stopFeeder = Commands.runOnce(m_shooter::stopFeeder, m_shooter); - - // Shoot when the 'X' button is pressed - m_driverController.x().onTrue(shoot).onFalse(stopFeeder); - - // We can also define commands inline at the binding! - - // While holding the shoulder button, drive at half speed - m_driverController - .rightBumper() - .onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5))) - .onFalse(Commands.runOnce(() -> 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() { - return m_autonomousCommand; - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java deleted file mode 100644 index dc3a3c6cc1..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java +++ /dev/null @@ -1,110 +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.frisbeebot.subsystems; - -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.examples.frisbeebot.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); - - /** 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); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java deleted file mode 100644 index a8f644d52e..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java +++ /dev/null @@ -1,62 +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.frisbeebot.subsystems; - -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj2.command.PIDSubsystem; - -public class ShooterSubsystem extends PIDSubsystem { - private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort); - private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort); - private final Encoder m_shooterEncoder = - new Encoder( - ShooterConstants.kEncoderPorts[0], - ShooterConstants.kEncoderPorts[1], - ShooterConstants.kEncoderReversed); - private final SimpleMotorFeedforward m_shooterFeedforward = - new SimpleMotorFeedforward( - ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation); - - /** The shooter subsystem for the robot. */ - public ShooterSubsystem() { - super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD)); - getController().setTolerance(ShooterConstants.kShooterToleranceRPS); - m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse); - setSetpoint(ShooterConstants.kShooterTargetRPS); - } - - @Override - public void useOutput(double output, double setpoint) { - m_shooterMotor.setVoltage( - output - + m_shooterFeedforward - .calculate(RadiansPerSecond.of(ShooterConstants.kShooterTargetRPS)) - .in(Volts)); - } - - @Override - public double getMeasurement() { - return m_shooterEncoder.getRate(); - } - - public boolean atSetpoint() { - return m_controller.atSetpoint(); - } - - public void runFeeder() { - m_feederMotor.set(ShooterConstants.kFeederSpeed); - } - - public void stopFeeder() { - m_feederMotor.set(0); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Constants.java deleted file mode 100644 index 8ffa56c681..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Constants.java +++ /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. - -package edu.wpi.first.wpilibj.examples.gearsbot; - -public final class Constants { - public static final class DriveConstants { - public static final int kLeftMotorPort1 = 0; - public static final int kLeftMotorPort2 = 1; - - public static final int kRightMotorPort1 = 2; - public static final int kRightMotorPort2 = 3; - - public static final int[] kLeftEncoderPorts = {0, 1}; - public static final int[] kRightEncoderPorts = {2, 3}; - public static final boolean kLeftEncoderReversed = false; - public static final boolean kRightEncoderReversed = false; - - public static final int kRangeFinderPort = 6; - public static final int kAnalogGyroPort = 1; - - 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 class ClawConstants { - public static final int kMotorPort = 7; - public static final int kContactPort = 5; - } - - public static final class WristConstants { - public static final int kMotorPort = 6; - - // these pid constants are not real, and will need to be tuned - public static final double kP = 0.1; - public static final double kI = 0.0; - public static final double kD = 0.0; - - public static final double kTolerance = 2.5; - - public static final int kPotentiometerPort = 3; - } - - public static final class ElevatorConstants { - public static final int kMotorPort = 5; - public static final int kPotentiometerPort = 2; - - // these pid constants are not real, and will need to be tuned - public static final double kP_real = 4; - public static final double kI_real = 0.007; - - public static final double kP_simulation = 18; - public static final double kI_simulation = 0.2; - - public static final double kD = 0.0; - - public static final double kTolerance = 0.005; - } - - public static final class AutoConstants { - public static final double kDistToBox1 = 0.10; - public static final double kDistToBox2 = 0.60; - - public static final double kWristSetpoint = -45.0; - } - - public static final class DriveStraightConstants { - // these pid constants are not real, and will need to be tuned - public static final double kP = 4.0; - public static final double kI = 0.0; - public static final double kD = 0.0; - } - - public static final class Positions { - public static final class Pickup { - public static final double kWristSetpoint = -45.0; - public static final double kElevatorSetpoint = 0.25; - } - - public static final class Place { - public static final double kWristSetpoint = 0.0; - public static final double kElevatorSetpoint = 0.25; - } - - public static final class PrepareToPickup { - public static final double kWristSetpoint = 0.0; - public static final double kElevatorSetpoint = 0.0; - } - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java deleted file mode 100644 index 8bf98fd23a..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/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.gearsbot; - -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/gearsbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java deleted file mode 100644 index f38ff9682f..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java +++ /dev/null @@ -1,93 +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.gearsbot; - -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(); - - // 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/gearsbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java deleted file mode 100644 index 28c76b7705..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java +++ /dev/null @@ -1,109 +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.gearsbot; - -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous; -import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw; -import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw; -import edu.wpi.first.wpilibj.examples.gearsbot.commands.Pickup; -import edu.wpi.first.wpilibj.examples.gearsbot.commands.Place; -import edu.wpi.first.wpilibj.examples.gearsbot.commands.PrepareToPickup; -import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetElevatorSetpoint; -import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint; -import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDrive; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -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 and commands are defined here... - private final Drivetrain m_drivetrain = new Drivetrain(); - private final Elevator m_elevator = new Elevator(); - private final Wrist m_wrist = new Wrist(); - private final Claw m_claw = new Claw(); - - private final XboxController m_joystick = new XboxController(0); - - private final Command m_autonomousCommand = - new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator); - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - // Put Some buttons on the SmartDashboard - SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0, m_elevator)); - SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.25, m_elevator)); - - SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0, m_wrist)); - SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45, m_wrist)); - - SmartDashboard.putData("Open Claw", new OpenClaw(m_claw)); - SmartDashboard.putData("Close Claw", new CloseClaw(m_claw)); - - SmartDashboard.putData( - "Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator)); - - // Assign default commands - m_drivetrain.setDefaultCommand( - new TankDrive(() -> -m_joystick.getLeftY(), () -> -m_joystick.getRightY(), m_drivetrain)); - - // Show what command your subsystem is running on the SmartDashboard - SmartDashboard.putData(m_drivetrain); - SmartDashboard.putData(m_elevator); - SmartDashboard.putData(m_wrist); - SmartDashboard.putData(m_claw); - - // Configure the button bindings - configureButtonBindings(); - } - - /** - * 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 passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - // Create some buttons - final JoystickButton dpadUp = new JoystickButton(m_joystick, 5); - final JoystickButton dpadRight = new JoystickButton(m_joystick, 6); - final JoystickButton dpadDown = new JoystickButton(m_joystick, 7); - final JoystickButton dpadLeft = new JoystickButton(m_joystick, 8); - final JoystickButton l2 = new JoystickButton(m_joystick, 9); - final JoystickButton r2 = new JoystickButton(m_joystick, 10); - final JoystickButton l1 = new JoystickButton(m_joystick, 11); - final JoystickButton r1 = new JoystickButton(m_joystick, 12); - - // Connect the buttons to commands - dpadUp.onTrue(new SetElevatorSetpoint(0.25, m_elevator)); - dpadDown.onTrue(new SetElevatorSetpoint(0.0, m_elevator)); - dpadRight.onTrue(new CloseClaw(m_claw)); - dpadLeft.onTrue(new OpenClaw(m_claw)); - - r1.onTrue(new PrepareToPickup(m_claw, m_wrist, m_elevator)); - r2.onTrue(new Pickup(m_claw, m_wrist, m_elevator)); - l1.onTrue(new Place(m_claw, m_wrist, m_elevator)); - l2.onTrue(new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator)); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return m_autonomousCommand; - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java deleted file mode 100644 index 63b55f680f..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java +++ /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. - -package edu.wpi.first.wpilibj.examples.gearsbot.commands; - -import edu.wpi.first.wpilibj.examples.gearsbot.Constants.AutoConstants; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -/** The main autonomous command to pickup and deliver the soda to the box. */ -public class Autonomous extends SequentialCommandGroup { - /** Create a new autonomous command. */ - public Autonomous(Drivetrain drive, Claw claw, Wrist wrist, Elevator elevator) { - addCommands( - new PrepareToPickup(claw, wrist, elevator), - new Pickup(claw, wrist, elevator), - new SetDistanceToBox(AutoConstants.kDistToBox1, drive), - // new DriveStraight(4), // Use encoders if ultrasonic is broken - new Place(claw, wrist, elevator), - new SetDistanceToBox(AutoConstants.kDistToBox2, drive), - // new DriveStraight(-2), // Use Encoders if ultrasonic is broken - Commands.parallel( - new SetWristSetpoint(AutoConstants.kWristSetpoint, wrist), new CloseClaw(claw))); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java deleted file mode 100644 index 1110e5728a..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java +++ /dev/null @@ -1,43 +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.gearsbot.commands; - -import edu.wpi.first.wpilibj.examples.gearsbot.Robot; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw; -import edu.wpi.first.wpilibj2.command.Command; - -/** Closes the claw until the limit switch is tripped. */ -public class CloseClaw extends Command { - private final Claw m_claw; - - public CloseClaw(Claw claw) { - m_claw = claw; - addRequirements(m_claw); - } - - // Called just before this Command runs the first time - @Override - public void initialize() { - m_claw.close(); - } - - // Make this return true when this Command no longer needs to run execute() - @Override - public boolean isFinished() { - return m_claw.isGrabbing(); - } - - // Called once after isFinished returns true - @Override - public void end(boolean interrupted) { - // NOTE: Doesn't stop in simulation due to lower friction causing the - // can to fall out - // + there is no need to worry about stalling the motor or crushing the - // can. - if (!Robot.isSimulation()) { - m_claw.stop(); - } - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java deleted file mode 100644 index 2430b61c3c..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.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.gearsbot.commands; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.examples.gearsbot.Constants.DriveStraightConstants; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain; -import edu.wpi.first.wpilibj2.command.PIDCommand; - -/** - * Drive the given distance straight (negative values go backwards). Uses a local PID controller to - * run a simple PID loop that is only enabled while this command is running. The input is the - * averaged values of the left and right encoders. - */ -public class DriveStraight extends PIDCommand { - private final Drivetrain m_drivetrain; - - /** - * Create a new DriveStraight command. - * - * @param distance The distance to drive - */ - public DriveStraight(double distance, Drivetrain drivetrain) { - super( - new PIDController( - DriveStraightConstants.kP, DriveStraightConstants.kI, DriveStraightConstants.kD), - drivetrain::getDistance, - distance, - d -> drivetrain.drive(d, d)); - - m_drivetrain = drivetrain; - addRequirements(m_drivetrain); - - getController().setTolerance(0.01); - } - - // Called just before this Command runs the first time - @Override - public void initialize() { - // Get everything in a safe starting state. - m_drivetrain.reset(); - super.initialize(); - } - - // Make this return true when this Command no longer needs to run execute() - @Override - public boolean isFinished() { - return getController().atSetpoint(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java deleted file mode 100644 index cf1754e549..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java +++ /dev/null @@ -1,37 +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.gearsbot.commands; - -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -/** Opens the claw for one second. Real robots should use sensors, stalling motors is BAD! */ -public class OpenClaw extends WaitCommand { - private final Claw m_claw; - - /** - * Creates a new OpenClaw command. - * - * @param claw The claw to use - */ - public OpenClaw(Claw claw) { - super(1); - m_claw = claw; - addRequirements(m_claw); - } - - // Called just before this Command runs the first time - @Override - public void initialize() { - m_claw.open(); - super.initialize(); - } - - // Called once after isFinished returns true - @Override - public void end(boolean interrupted) { - m_claw.stop(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java deleted file mode 100644 index 78f887956d..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java +++ /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. - -package edu.wpi.first.wpilibj.examples.gearsbot.commands; - -import edu.wpi.first.wpilibj.examples.gearsbot.Constants.Positions; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -/** - * Pickup a soda can (if one is between the open claws) and get it in a safe state to drive around. - */ -public class Pickup extends SequentialCommandGroup { - /** - * Create a new pickup command. - * - * @param claw The claw subsystem to use - * @param wrist The wrist subsystem to use - * @param elevator The elevator subsystem to use - */ - public Pickup(Claw claw, Wrist wrist, Elevator elevator) { - addCommands( - new CloseClaw(claw), - Commands.parallel( - new SetWristSetpoint(Positions.Pickup.kWristSetpoint, wrist), - new SetElevatorSetpoint(Positions.Pickup.kElevatorSetpoint, elevator))); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java deleted file mode 100644 index fad6de5a4d..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java +++ /dev/null @@ -1,28 +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.gearsbot.commands; - -import edu.wpi.first.wpilibj.examples.gearsbot.Constants.Positions; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -/** Place a held soda can onto the platform. */ -public class Place extends SequentialCommandGroup { - /** - * Create a new place command. - * - * @param claw The claw subsystem to use - * @param wrist The wrist subsystem to use - * @param elevator The elevator subsystem to use - */ - public Place(Claw claw, Wrist wrist, Elevator elevator) { - addCommands( - new SetElevatorSetpoint(Positions.Place.kElevatorSetpoint, elevator), - new SetWristSetpoint(Positions.Place.kWristSetpoint, wrist), - new OpenClaw(claw)); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java deleted file mode 100644 index d9ae53ca61..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java +++ /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. - -package edu.wpi.first.wpilibj.examples.gearsbot.commands; - -import edu.wpi.first.wpilibj.examples.gearsbot.Constants.Positions; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -/** Make sure the robot is in a state to pickup soda cans. */ -public class PrepareToPickup extends SequentialCommandGroup { - /** - * Create a new prepare to pickup command. - * - * @param claw The claw subsystem to use - * @param wrist The wrist subsystem to use - * @param elevator The elevator subsystem to use - */ - public PrepareToPickup(Claw claw, Wrist wrist, Elevator elevator) { - addCommands( - new OpenClaw(claw), - Commands.parallel( - new SetWristSetpoint(Positions.PrepareToPickup.kWristSetpoint, wrist), - new SetElevatorSetpoint(Positions.PrepareToPickup.kElevatorSetpoint, elevator))); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java deleted file mode 100644 index f8b291c49c..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java +++ /dev/null @@ -1,50 +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.gearsbot.commands; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain; -import edu.wpi.first.wpilibj2.command.PIDCommand; - -/** - * Drive until the robot is the given distance away from the box. Uses a local PID controller to run - * a simple PID loop that is only enabled while this command is running. The input is the averaged - * values of the left and right encoders. - */ -public class SetDistanceToBox extends PIDCommand { - private final Drivetrain m_drivetrain; - - /** - * Create a new set distance to box command. - * - * @param distance The distance away from the box to drive to - */ - public SetDistanceToBox(double distance, Drivetrain drivetrain) { - super( - new PIDController(-2, 0, 0), - drivetrain::getDistanceToObstacle, - distance, - d -> drivetrain.drive(d, d)); - - m_drivetrain = drivetrain; - addRequirements(m_drivetrain); - - getController().setTolerance(0.01); - } - - // Called just before this Command runs the first time - @Override - public void initialize() { - // Get everything in a safe starting state. - m_drivetrain.reset(); - super.initialize(); - } - - // Make this return true when this Command no longer needs to run execute() - @Override - public boolean isFinished() { - return getController().atSetpoint(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java deleted file mode 100644 index 66790c2cb5..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java +++ /dev/null @@ -1,43 +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.gearsbot.commands; - -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator; -import edu.wpi.first.wpilibj2.command.Command; - -/** - * Move the elevator to a given location. This command finishes when it is within the tolerance, but - * leaves the PID loop running to maintain the position. Other commands using the elevator should - * make sure they disable PID! - */ -public class SetElevatorSetpoint extends Command { - private final Elevator m_elevator; - private final double m_setpoint; - - /** - * Create a new SetElevatorSetpoint command. - * - * @param setpoint The setpoint to set the elevator to - * @param elevator The elevator to use - */ - public SetElevatorSetpoint(double setpoint, Elevator elevator) { - m_elevator = elevator; - m_setpoint = setpoint; - addRequirements(m_elevator); - } - - // Called just before this Command runs the first time - @Override - public void initialize() { - m_elevator.setSetpoint(m_setpoint); - m_elevator.enable(); - } - - // Make this return true when this Command no longer needs to run execute() - @Override - public boolean isFinished() { - return m_elevator.getController().atSetpoint(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java deleted file mode 100644 index a185ed5ff4..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java +++ /dev/null @@ -1,43 +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.gearsbot.commands; - -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist; -import edu.wpi.first.wpilibj2.command.Command; - -/** - * Move the wrist to a given angle. This command finishes when it is within the tolerance, but - * leaves the PID loop running to maintain the position. Other commands using the wrist should make - * sure they disable PID! - */ -public class SetWristSetpoint extends Command { - private final Wrist m_wrist; - private final double m_setpoint; - - /** - * Create a new SetWristSetpoint command. - * - * @param setpoint The setpoint to set the wrist to - * @param wrist The wrist to use - */ - public SetWristSetpoint(double setpoint, Wrist wrist) { - m_wrist = wrist; - m_setpoint = setpoint; - addRequirements(m_wrist); - } - - // Called just before this Command runs the first time - @Override - public void initialize() { - m_wrist.enable(); - m_wrist.setSetpoint(m_setpoint); - } - - // Make this return true when this Command no longer needs to run execute() - @Override - public boolean isFinished() { - return m_wrist.getController().atSetpoint(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java deleted file mode 100644 index 3830f82103..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java +++ /dev/null @@ -1,48 +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.gearsbot.commands; - -import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain; -import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.DoubleSupplier; - -/** Have the robot drive tank style. */ -public class TankDrive extends Command { - private final Drivetrain m_drivetrain; - private final DoubleSupplier m_left; - private final DoubleSupplier m_right; - - /** - * Creates a new TankDrive command. - * - * @param left The control input for the left side of the drive - * @param right The control input for the right sight of the drive - * @param drivetrain The drivetrain subsystem to drive - */ - public TankDrive(DoubleSupplier left, DoubleSupplier right, Drivetrain drivetrain) { - m_drivetrain = drivetrain; - m_left = left; - m_right = right; - addRequirements(m_drivetrain); - } - - // Called repeatedly when this Command is scheduled to run - @Override - public void execute() { - m_drivetrain.drive(m_left.getAsDouble(), m_right.getAsDouble()); - } - - // Make this return true when this Command no longer needs to run execute() - @Override - public boolean isFinished() { - return false; // Runs until interrupted - } - - // Called once after isFinished returns true - @Override - public void end(boolean interrupted) { - m_drivetrain.drive(0, 0); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java deleted file mode 100644 index 06c627e480..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.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.gearsbot.subsystems; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.examples.gearsbot.Constants.ClawConstants; -import edu.wpi.first.wpilibj.motorcontrol.Victor; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** - * The claw subsystem is a simple system with a motor for opening and closing. If using stronger - * motors, you should probably use a sensor so that the motors don't stall. - */ -public class Claw extends SubsystemBase { - private final Victor m_motor = new Victor(ClawConstants.kMotorPort); - private final DigitalInput m_contact = new DigitalInput(ClawConstants.kContactPort); - - /** Create a new claw subsystem. */ - public Claw() { - // Let's name everything on the LiveWindow - addChild("Motor", m_motor); - addChild("Limit Switch", m_contact); - } - - public void log() { - SmartDashboard.putData("Claw switch", m_contact); - } - - /** Set the claw motor to move in the open direction. */ - public void open() { - m_motor.set(-1); - } - - /** Set the claw motor to move in the close direction. */ - public void close() { - m_motor.set(1); - } - - /** Stops the claw motor from moving. */ - public void stop() { - m_motor.set(0); - } - - /** Return true when the robot is grabbing an object hard enough to trigger the limit switch. */ - public boolean isGrabbing() { - return m_contact.get(); - } - - /** Call log method every loop. */ - @Override - public void periodic() { - log(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java deleted file mode 100644 index b632f8d5ca..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java +++ /dev/null @@ -1,138 +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.gearsbot.subsystems; - -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.AnalogGyro; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.examples.gearsbot.Constants.DriveConstants; -import edu.wpi.first.wpilibj.examples.gearsbot.Robot; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Drivetrain extends SubsystemBase { - // The Drivetrain subsystem incorporates the sensors and actuators attached to the robots chassis. - // These include four drive motors, a left and right encoder and a gyro. - private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotorPort1); - private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotorPort2); - private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotorPort1); - private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotorPort2); - - private final DifferentialDrive m_drive = - new DifferentialDrive(m_leftLeader::set, m_rightLeader::set); - - private final Encoder m_leftEncoder = - new Encoder( - DriveConstants.kLeftEncoderPorts[0], - DriveConstants.kLeftEncoderPorts[1], - DriveConstants.kLeftEncoderReversed); - private final Encoder m_rightEncoder = - new Encoder( - DriveConstants.kRightEncoderPorts[0], - DriveConstants.kRightEncoderPorts[1], - DriveConstants.kRightEncoderReversed); - private final AnalogInput m_rangefinder = new AnalogInput(DriveConstants.kRangeFinderPort); - private final AnalogGyro m_gyro = new AnalogGyro(DriveConstants.kAnalogGyroPort); - - /** Create a new drivetrain subsystem. */ - public Drivetrain() { - super(); - - 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); - - // Encoders may measure differently in the real world and in - // simulation. In this example the robot moves 0.042 barleycorns - // per tick in the real world, but the simulated encoders - // simulate 360 tick encoders. This if statement allows for the - // real robot to handle this difference in devices. - if (Robot.isReal()) { - m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); - } else { - // Circumference = diameter in feet * pi. 360 tick simulated encoders. - m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0); - m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0); - } - - // Let's name the sensors on the LiveWindow - addChild("Drive", m_drive); - addChild("Left Encoder", m_leftEncoder); - addChild("Right Encoder", m_rightEncoder); - addChild("Rangefinder", m_rangefinder); - addChild("Gyro", m_gyro); - } - - /** The log method puts interesting information to the SmartDashboard. */ - public void log() { - SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance()); - SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance()); - SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate()); - SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate()); - SmartDashboard.putNumber("Gyro", m_gyro.getAngle()); - } - - /** - * Tank style driving for the Drivetrain. - * - * @param left Speed in range [-1,1] - * @param right Speed in range [-1,1] - */ - public void drive(double left, double right) { - m_drive.tankDrive(left, right); - } - - /** - * Get the robot's heading. - * - * @return The robots heading in degrees. - */ - public double getHeading() { - return m_gyro.getAngle(); - } - - /** Reset the robots sensors to the zero states. */ - public void reset() { - m_gyro.reset(); - m_leftEncoder.reset(); - m_rightEncoder.reset(); - } - - /** - * Get the average distance of the encoders since the last reset. - * - * @return The distance driven (average of left and right encoders). - */ - public double getDistance() { - return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2; - } - - /** - * Get the distance to the obstacle. - * - * @return The distance to the obstacle detected by the rangefinder. - */ - public double getDistanceToObstacle() { - // Really meters in simulation since it's a rangefinder... - return m_rangefinder.getAverageVoltage(); - } - - /** Call log method every loop. */ - @Override - public void periodic() { - log(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java deleted file mode 100644 index b3eba1fda7..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java +++ /dev/null @@ -1,77 +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.gearsbot.subsystems; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.AnalogPotentiometer; -import edu.wpi.first.wpilibj.examples.gearsbot.Constants.ElevatorConstants; -import edu.wpi.first.wpilibj.examples.gearsbot.Robot; -import edu.wpi.first.wpilibj.motorcontrol.Victor; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.PIDSubsystem; - -/** - * The elevator subsystem uses PID to go to a given height. Unfortunately, in it's current state PID - * values for simulation are different than in the real world do to minor differences. - */ -public class Elevator extends PIDSubsystem { - private final Victor m_motor = new Victor(ElevatorConstants.kMotorPort); - private final AnalogPotentiometer m_pot; - - /** Create a new elevator subsystem. */ - public Elevator() { - super( - new PIDController( - ElevatorConstants.kP_real, ElevatorConstants.kI_real, ElevatorConstants.kD)); - - if (Robot.isSimulation()) { // Check for simulation and update PID values - getController() - .setPID( - ElevatorConstants.kP_simulation, - ElevatorConstants.kI_simulation, - ElevatorConstants.kD); - } - getController().setTolerance(ElevatorConstants.kTolerance); - - // Conversion value of potentiometer varies between the real world and - // simulation - - if (Robot.isReal()) { - m_pot = new AnalogPotentiometer(ElevatorConstants.kPotentiometerPort, -2.0 / 5); - } else { - // Defaults to meters - m_pot = new AnalogPotentiometer(ElevatorConstants.kPotentiometerPort); - } - - // Let's name everything on the LiveWindow - addChild("Motor", m_motor); - addChild("Pot", m_pot); - } - - /** The log method puts interesting information to the SmartDashboard. */ - public void log() { - SmartDashboard.putData("Elevator Pot", m_pot); - } - - /** - * Use the potentiometer as the PID sensor. This method is automatically called by the subsystem. - */ - @Override - public double getMeasurement() { - return m_pot.get(); - } - - /** Use the motor as the PID output. This method is automatically called by the subsystem. */ - @Override - public void useOutput(double output, double setpoint) { - m_motor.set(output); - } - - /** Call log method every loop. */ - @Override - public void periodic() { - log(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java deleted file mode 100644 index d13df6419e..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java +++ /dev/null @@ -1,65 +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.gearsbot.subsystems; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.AnalogPotentiometer; -import edu.wpi.first.wpilibj.examples.gearsbot.Constants.WristConstants; -import edu.wpi.first.wpilibj.examples.gearsbot.Robot; -import edu.wpi.first.wpilibj.motorcontrol.Victor; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.PIDSubsystem; - -/** - * The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint. - */ -public class Wrist extends PIDSubsystem { - private final Victor m_motor = new Victor(WristConstants.kMotorPort); - private final AnalogPotentiometer m_pot; - - /** Create a new wrist subsystem. */ - public Wrist() { - super(new PIDController(WristConstants.kP, WristConstants.kI, WristConstants.kD)); - getController().setTolerance(WristConstants.kTolerance); - - // Conversion value of potentiometer varies between the real world and - // simulation - if (Robot.isReal()) { - m_pot = new AnalogPotentiometer(WristConstants.kPotentiometerPort, -270.0 / 5); - } else { - // Defaults to degrees - m_pot = new AnalogPotentiometer(WristConstants.kPotentiometerPort); - } - - // Let's name everything on the LiveWindow - addChild("Motor", m_motor); - addChild("Pot", m_pot); - } - - /** The log method puts interesting information to the SmartDashboard. */ - public void log() { - SmartDashboard.putData("Wrist Angle", m_pot); - } - - /** - * Use the potentiometer as the PID sensor. This method is automatically called by the subsystem. - */ - @Override - public double getMeasurement() { - return m_pot.get(); - } - - /** Use the motor as the PID output. This method is automatically called by the subsystem. */ - @Override - public void useOutput(double output, double setpoint) { - m_motor.set(output); - } - - /** Call log method every loop. */ - @Override - public void periodic() { - log(); - } -} 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 5c5f0b23b1..0000000000 --- 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 9e6bfe73a5..0000000000 --- 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 6b5b110552..0000000000 --- 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 5caf497170..0000000000 --- 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 053c64430f..0000000000 --- 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 1481fab5cd..0000000000 --- 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 db874104b6..0000000000 --- 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); - } -}