diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp index d3e0c0c6ab..4a956f6106 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp @@ -36,4 +36,4 @@ void PIDCommand::Execute() { void PIDCommand::End(bool interrupted) { m_useOutput(0); } -PIDController& PIDCommand::getController() { return m_controller; } +PIDController& PIDCommand::GetController() { return m_controller; } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h index 3f17b0d15e..1089fd176d 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h @@ -72,7 +72,7 @@ class PIDCommand : public CommandHelper { * * @return The PIDController */ - PIDController& getController(); + PIDController& GetController(); protected: PIDController m_controller; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp new file mode 100644 index 0000000000..cd19aeb7d8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, 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/ArmBotOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..aec6b1d7a7 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#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.GetY(frc::GenericHID::kLeftHand), + m_driverController.GetX(frc::GenericHID::kRightHand)); + }, + {&m_drive})); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // Move the arm to 2 radians above horizontal when the 'A' button is pressed. + frc2::JoystickButton(&m_driverController, 1) + .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm}); + + // Move the arm to neutral position when the 'B' button is pressed. + frc2::JoystickButton(&m_driverController, 1) + .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); }, + {&m_arm}); + + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed([this] { m_drive.SetMaxOutput(.5); }) + .WhenReleased([this] { m_drive.SetMaxOutput(1); }); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Runs the chosen command in autonomous + return new frc2::InstantCommand([] {}); +} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp new file mode 100644 index 0000000000..54a59ed28f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the 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, kCos, 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.to(), feedforward / 12_V); +} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..47c898e7dd --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the 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]} { + // 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/ArmBotOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h new file mode 100644 index 0000000000..674633c56d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#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 { +constexpr int kLeftMotor1Port = 0; +constexpr int kLeftMotor2Port = 1; +constexpr int kRightMotor1Port = 2; +constexpr int kRightMotor2Port = 3; + +constexpr int kLeftEncoderPorts[]{0, 1}; +constexpr int kRightEncoderPorts[]{2, 3}; +constexpr bool kLeftEncoderReversed = false; +constexpr bool kRightEncoderReversed = true; + +constexpr int kEncoderCPR = 1024; +constexpr double kWheelDiameterInches = 6; +constexpr double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); +} // namespace DriveConstants + +namespace ArmConstants { +constexpr int kMotorPort = 4; + +constexpr double kP = 1; + +// These are fake gains; in actuality these must be determined individually for +// each robot +constexpr auto kS = 1_V; +constexpr auto kCos = 1_V; +constexpr auto kV = 0.5_V * 1_s / 1_rad; +constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad; + +constexpr auto kMaxVelocity = 3_rad_per_s; +constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s); + +constexpr int kEncoderPorts[]{4, 5}; +constexpr int kEncoderPPR = 256; +constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR; + +// The offset of the arm from the horizontal in its neutral position, +// measured from the horizontal +constexpr auto kArmOffset = 0.5_rad; +} // namespace ArmConstants + +namespace AutoConstants { +constexpr auto kAutoTimeoutSeconds = 12_s; +constexpr auto kAutoShootTimeSeconds = 7_s; +} // namespace AutoConstants + +namespace OIConstants { +constexpr int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h new file mode 100644 index 0000000000..7c8d261a64 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h @@ -0,0 +1,87 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +/** + * A simplified stub class that simulates the API of a common "smart" motor + * controller. + * + *

Has no actual functionality. + */ +class ExampleSmartMotorController : public frc::SpeedController { + 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 master The master to follow. + */ + void Follow(ExampleSmartMotorController master) {} + + /** + * 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) override {} + + double Get() const override { return 0; } + + void SetInverted(bool isInverted) override {} + + bool GetInverted() const override { return false; } + + void Disable() override {} + + void StopMotor() override {} + + void PIDWrite(double output) override {} +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h new file mode 100644 index 0000000000..fa173d39e1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "RobotContainer.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TestPeriodic() override; + + 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/ArmBotOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h new file mode 100644 index 0000000000..fa07359c84 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#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::Command* GetAutonomousCommand(); + + private: + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + ArmSubsystem m_arm; + + // The chooser for the autonomous routines + + 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 new file mode 100644 index 0000000000..52f15f1130 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#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; + + 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 new file mode 100644 index 0000000000..3ed1357f8f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h @@ -0,0 +1,95 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#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); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + // The motor controllers + frc::PWMVictorSPX m_left1; + frc::PWMVictorSPX m_left2; + frc::PWMVictorSPX m_right1; + frc::PWMVictorSPX m_right2; + + // The motors on the left side of the drive + frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2}; + + // The motors on the right side of the drive + frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2}; + + // The robot's drive + frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors}; + + // 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/DriveDistanceOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp new file mode 100644 index 0000000000..cd19aeb7d8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, 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/DriveDistanceOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..78bb48a4b4 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include +#include + +#include "commands/DriveDistanceProfiled.h" + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.ArcadeDrive( + m_driverController.GetY(frc::GenericHID::kLeftHand), + m_driverController.GetX(frc::GenericHID::kRightHand)); + }, + {&m_drive})); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed(&m_driveHalfSpeed) + .WhenReleased(&m_driveFullSpeed); + + // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of + // 10 seconds + frc2::JoystickButton(&m_driverController, 1) + .WhenPressed(DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s)); + + // Do the same thing as above when the 'B' button is pressed, but defined + // inline + frc2::JoystickButton(&m_driverController, 2) + .WhenPressed( + frc2::TrapezoidProfileCommand( + frc::TrapezoidProfile( + // Limit the max acceleration and velocity + {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}, + // End at desired position in meters; implicitly starts at 0 + {3_m, 0_mps}), + // Pipe the profile state to the drive + [this](auto setpointState) { + m_drive.SetDriveStates(setpointState, setpointState); + }, + // Require the drive + {&m_drive}) + .BeforeStarting([this]() { m_drive.ResetEncoders(); }) + .WithTimeout(10_s)); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Runs the chosen command in autonomous + return new frc2::InstantCommand([] {}); +} diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp new file mode 100644 index 0000000000..7de607c210 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/DriveDistanceProfiled.h" + +#include "Constants.h" + +using namespace DriveConstants; + +DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance, + DriveSubsystem* drive) + : CommandHelper( + frc::TrapezoidProfile( + // Limit the max acceleration and velocity + {kMaxSpeed, kMaxAcceleration}, + // End at desired position in meters; implicitly starts at 0 + {distance, 0_mps}), + // Pipe the profile state to the drive + [drive](auto setpointState) { + drive->SetDriveStates(setpointState, setpointState); + }, + // Require the drive + {drive}) { + // Reset drive encoders since we're starting at 0 + drive->ResetEncoders(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..cd8b3e4e29 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/DriveSubsystem.h" + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() + : m_leftMaster{kLeftMotor1Port}, + m_leftSlave{kLeftMotor2Port}, + m_rightMaster{kRightMotor1Port}, + m_rightSlave{kRightMotor2Port}, + m_feedforward{ks, kv, ka} { + m_leftSlave.Follow(m_leftMaster); + m_rightSlave.Follow(m_rightMaster); + + m_leftMaster.SetPID(kp, 0, 0); + m_rightMaster.SetPID(kp, 0, 0); +} + +void DriveSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. +} + +void DriveSubsystem::SetDriveStates( + frc::TrapezoidProfile::State left, + frc::TrapezoidProfile::State right) { + m_leftMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, + left.position.to(), + m_feedforward.Calculate(left.velocity) / 12_V); + m_rightMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, + right.position.to(), + m_feedforward.Calculate(right.velocity) / 12_V); +} + +void DriveSubsystem::ArcadeDrive(double fwd, double rot) { + m_drive.ArcadeDrive(fwd, rot); +} + +void DriveSubsystem::ResetEncoders() { + m_leftMaster.ResetEncoder(); + m_rightMaster.ResetEncoder(); +} + +units::meter_t DriveSubsystem::GetLeftEncoderDistance() { + return units::meter_t(m_leftMaster.GetEncoderDistance()); +} + +units::meter_t DriveSubsystem::GetRightEncoderDistance() { + return units::meter_t(m_rightMaster.GetEncoderDistance()); +} + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h new file mode 100644 index 0000000000..bcf3356e0a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#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 { +constexpr int kLeftMotor1Port = 0; +constexpr int kLeftMotor2Port = 1; +constexpr int kRightMotor1Port = 2; +constexpr int kRightMotor2Port = 3; + +// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! +// These characterization values MUST be determined either experimentally or +// theoretically for *your* robot's drive. The RobotPy Characterization +// Toolsuite provides a convenient tool for obtaining these values for your +// robot. +constexpr auto ks = 1_V; +constexpr auto kv = 0.8_V * 1_s / 1_m; +constexpr auto ka = 0.15_V * 1_s * 1_s / 1_m; + +constexpr double kp = 1; + +constexpr auto kMaxSpeed = 3_mps; +constexpr auto kMaxAcceleration = 3_mps_sq; + +} // namespace DriveConstants + +namespace OIConstants { +constexpr int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h new file mode 100644 index 0000000000..7c8d261a64 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h @@ -0,0 +1,87 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +/** + * A simplified stub class that simulates the API of a common "smart" motor + * controller. + * + *

Has no actual functionality. + */ +class ExampleSmartMotorController : public frc::SpeedController { + 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 master The master to follow. + */ + void Follow(ExampleSmartMotorController master) {} + + /** + * 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) override {} + + double Get() const override { return 0; } + + void SetInverted(bool isInverted) override {} + + bool GetInverted() const override { return false; } + + void Disable() override {} + + void StopMotor() override {} + + void PIDWrite(double output) override {} +}; diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h new file mode 100644 index 0000000000..fa173d39e1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "RobotContainer.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TestPeriodic() override; + + 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/DriveDistanceOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h new file mode 100644 index 0000000000..af35be4e0b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "subsystems/DriveSubsystem.h" + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +class RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + + frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h new file mode 100644 index 0000000000..2199590723 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h @@ -0,0 +1,20 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/DriveSubsystem.h" + +class DriveDistanceProfiled + : public frc2::CommandHelper, + DriveDistanceProfiled> { + public: + DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..f68cbbd5ca --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h @@ -0,0 +1,90 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "ExampleSmartMotorController.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. + + /** + * Attempts to follow the given drive states using offboard PID. + * + * @param left The left wheel state. + * @param right The right wheel state. + */ + void SetDriveStates(frc::TrapezoidProfile::State left, + frc::TrapezoidProfile::State right); + + /** + * 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 distance of the left encoder. + * + * @return the average of the TWO encoder readings + */ + units::meter_t GetLeftEncoderDistance(); + + /** + * Gets the distance of the right encoder. + * + * @return the average of the TWO encoder readings + */ + units::meter_t GetRightEncoderDistance(); + + /** + * 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 + ExampleSmartMotorController m_leftMaster; + ExampleSmartMotorController m_leftSlave; + ExampleSmartMotorController m_rightMaster; + ExampleSmartMotorController m_rightSlave; + + // A feedforward component for the drive + frc::SimpleMotorFeedforward m_feedforward; + + // The robot's drive + frc::DifferentialDrive m_drive{m_leftMaster, m_rightMaster}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp index 5a6dcab400..5853054054 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp @@ -5,20 +5,22 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include -#include #include -#include +#include #include +#include #include +#include "ExampleSmartMotorController.h" + class Robot : public frc::TimedRobot { public: static constexpr units::second_t kDt = 20_ms; Robot() { - m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5); + // Note: These gains are fake, and will have to be tuned for your robot. + m_motor.SetPID(1.3, 0.0, 0.7); } void TeleopPeriodic() override { @@ -38,17 +40,18 @@ class Robot : public frc::TimedRobot { // toward the goal while obeying the constraints. m_setpoint = profile.Calculate(kDt); - // Run controller with profiled setpoint and update motor output - double output = m_controller.Calculate(m_encoder.GetDistance(), - m_setpoint.position.to()); - m_motor.Set(output); + // Send setpoint to offboard controller PID + m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, + m_setpoint.position.to(), + m_feedforward.Calculate(m_setpoint.velocity) / 12_V); } private: frc::Joystick m_joystick{1}; - frc::Encoder m_encoder{1, 2}; - frc::PWMVictorSPX m_motor{1}; - frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt}; + ExampleSmartMotorController m_motor{1}; + frc::SimpleMotorFeedforward m_feedforward{ + // Note: These gains are fake, and will have to be tuned for your robot. + 1_V, 1.5_V * 1_s / 1_m}; frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq}; diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h new file mode 100644 index 0000000000..7c8d261a64 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h @@ -0,0 +1,87 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +/** + * A simplified stub class that simulates the API of a common "smart" motor + * controller. + * + *

Has no actual functionality. + */ +class ExampleSmartMotorController : public frc::SpeedController { + 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 master The master to follow. + */ + void Follow(ExampleSmartMotorController master) {} + + /** + * 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) override {} + + double Get() const override { return 0; } + + void SetInverted(bool isInverted) override {} + + bool GetInverted() const override { return false; } + + void Disable() override {} + + void StopMotor() override {} + + void PIDWrite(double output) override {} +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp index aa1117a4a3..62133337d6 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp @@ -8,6 +8,9 @@ #include "RobotContainer.h" #include +#include +#include +#include #include RobotContainer::RobotContainer() { @@ -29,16 +32,39 @@ RobotContainer::RobotContainer() { void RobotContainer::ConfigureButtonBindings() { // Configure your button bindings here + // Assorted commands to be bound to buttons + // Stabilize robot to drive straight with gyro when left bumper is held - frc2::JoystickButton(&m_driverController, 5).WhenHeld(&m_stabilizeDriving); + frc2::JoystickButton(&m_driverController, 5) + .WhenHeld(frc2::PIDCommand{ + frc2::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.GetY( + frc::GenericHID::JoystickHand::kLeftHand), + output); + }, + // Require the robot drive + {&m_drive}}); // Turn to 90 degrees when the 'X' button is pressed - frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_turnTo90); + frc2::JoystickButton(&m_driverController, 3) + .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s)); + + // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5 + // second timeout + frc2::JoystickButton(&m_driverController, 1) + .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s)); // While holding the shoulder button, drive at half speed frc2::JoystickButton(&m_driverController, 6) - .WhenPressed(&m_driveHalfSpeed) - .WhenReleased(&m_driveFullSpeed); + .WhenPressed(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(0.5); }}) + .WhenReleased(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(1); }}); } frc2::Command* RobotContainer::GetAutonomousCommand() { diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp index cadb7f9031..988535cb7e 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp @@ -11,12 +11,12 @@ using namespace DriveConstants; -TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive) +TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive) : CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD), // Close loop on heading - [drive] { return drive->GetHeading(); }, + [drive] { return drive->GetHeading().to(); }, // Set reference to target - targetAngleDegrees, + target.to(), // Pipe output to turn robot [drive](double output) { drive->ArcadeDrive(0, output); }, // Require the drive @@ -26,9 +26,10 @@ TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive) // 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(kTurnToleranceDeg, kTurnRateToleranceDegPerS); + m_controller.SetTolerance(kTurnTolerance.to(), + kTurnRateTolerance.to()); AddRequirements({drive}); } -bool TurnToAngle::IsFinished() { return m_controller.AtSetpoint(); } +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 new file mode 100644 index 0000000000..c2baa90090 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the 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 index d5da9c7b3b..c6d8d8c574 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp @@ -46,8 +46,9 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) { m_drive.SetMaxOutput(maxOutput); } -double DriveSubsystem::GetHeading() { - return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1.0 : 1.0); +units::degree_t DriveSubsystem::GetHeading() { + return units::degree_t(std::remainder(m_gyro.GetAngle(), 360) * + (kGyroReversed ? -1.0 : 1.0)); } double DriveSubsystem::GetTurnRate() { diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h index c7bcb56567..f423640566 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h @@ -7,6 +7,7 @@ #pragma once +#include #include /** @@ -45,8 +46,11 @@ constexpr double kTurnP = 1; constexpr double kTurnI = 0; constexpr double kTurnD = 0; -constexpr double kTurnToleranceDeg = 5; -constexpr double kTurnRateToleranceDegPerS = 10; // degrees per second +constexpr auto kTurnTolerance = 5_deg; +constexpr auto kTurnRateTolerance = 10_deg_per_s; + +constexpr auto kMaxTurnRate = 100_deg_per_s; +constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s; } // namespace DriveConstants namespace AutoConstants { diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h index 501aa75c30..37c7e4f98e 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h @@ -12,9 +12,6 @@ #include #include #include -#include -#include -#include #include "Constants.h" #include "commands/TurnToAngle.h" @@ -45,34 +42,6 @@ class RobotContainer { // The robot's subsystems DriveSubsystem m_drive; - // Assorted commands to be bound to buttons - - // Turn to 90 degrees, with a 5 second timeout - frc2::ParallelRaceGroup m_turnTo90 = - TurnToAngle{90, &m_drive}.WithTimeout(5_s); - - // Stabilize the robot while driving - frc2::PIDCommand m_stabilizeDriving{ - frc2::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.GetY(frc::GenericHID::JoystickHand::kLeftHand), - output); - }, - // Require the robot drive - {&m_drive}}; - - frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); }, - {}}; - frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, - {}}; - // The chooser for the autonomous routines frc::SendableChooser m_chooser; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h index c7e875e87e..4afa20ea70 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h @@ -23,7 +23,7 @@ class TurnToAngle : public frc2::CommandHelper { * @param targetAngleDegrees The angle to turn to * @param drive The drive subsystem to use */ - TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive); + 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 new file mode 100644 index 0000000000..0b52044180 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the 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 index cada81669e..c4cd21c71d 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h @@ -13,6 +13,7 @@ #include #include #include +#include #include "Constants.h" @@ -74,7 +75,7 @@ class DriveSubsystem : public frc2::SubsystemBase { * * @return the robot's heading in degrees, from 180 to 180 */ - double GetHeading(); + units::degree_t GetHeading(); /** * Returns the turn rate of the robot. diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 73ebab526d..4aa9f0ef91 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -508,5 +508,29 @@ "foldername": "ArmBot", "gradlebase": "cpp", "commandversion": 2 + }, + { + "name": "ArmBotOffboard", + "description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.", + "tags": [ + "ArmBotOffboard", + "PID", + "Motion Profile" + ], + "foldername": "ArmBotOffboard", + "gradlebase": "cpp", + "commandversion": 2 + }, + { + "name": "DriveDistanceOffboard", + "description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.", + "tags": [ + "DriveDistance", + "PID", + "Motion Profile" + ], + "foldername": "DriveDistanceOffboard", + "gradlebase": "cpp", + "commandversion": 2 } ] 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 new file mode 100644 index 0000000000..baa15b6e1a --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the 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) / (double) 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 kCosVolts = 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 = 1; + } +} 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 new file mode 100644 index 0000000000..0fa7daedd1 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java @@ -0,0 +1,115 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbotoffboard; + +import edu.wpi.first.wpilibj.SpeedController; + +/** + * A simplified stub class that simulates the API of a common "smart" motor controller. + * + *

Has no actual functionality. + */ +public class ExampleSmartMotorController implements SpeedController { + 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 master The master to follow. + */ + public void follow(ExampleSmartMotorController master) { + } + + /** + * 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() { + } + + @Override + public void set(double speed) { + } + + @Override + public double get() { + return 0; + } + + @Override + public void setInverted(boolean isInverted) { + + } + + @Override + public boolean getInverted() { + return false; + } + + @Override + public void disable() { + } + + @Override + public void stopMotor() { + } + + @Override + public void pidWrite(double output) { + } +} 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 new file mode 100644 index 0000000000..ff9e928364 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the 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 new file mode 100644 index 0000000000..8a7f99b0ea --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the 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 VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, 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 new file mode 100644 index 0000000000..285bd58746 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java @@ -0,0 +1,86 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbotoffboard; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem; +import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.XboxController.Button; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants.kDriverControllerPort; + +/** + * 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 + XboxController m_driverController = new XboxController(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.getY(GenericHID.Hand.kLeft), + m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link 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. + new JoystickButton(m_driverController, Button.kA.value) + .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm); + + // Move the arm to neutral position when the 'B' button is pressed. + new JoystickButton(m_driverController, Button.kB.value) + .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm); + + // Drive at half speed when the bumper is held + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(() -> m_robotDrive.setMaxOutput(0.5)) + .whenReleased(() -> 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 new InstantCommand(); + } +} 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 new file mode 100644 index 0000000000..2d2698cb91 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems; + +import edu.wpi.first.wpilibj.controller.ArmFeedforward; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; + +import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController; + +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kAVoltSecondSquaredPerRad; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kArmOffsetRads; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kCosVolts; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxVelocityRadPerSecond; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMotorPort; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kP; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kSVolts; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kVVoltSecondPerRad; + +/** + * A robot arm subsystem that moves with a motion profile. + */ +public class ArmSubsystem extends TrapezoidProfileSubsystem { + private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(kMotorPort); + private final ArmFeedforward m_feedforward = + new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad); + + /** + * Create a new ArmSubsystem. + */ + public ArmSubsystem() { + super(new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond, + kMaxAccelerationRadPerSecSquared), + kArmOffsetRads); + m_motor.setPID(kP, 0, 0); + } + + @Override + public void useState(TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double 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, + feedforward / 12.0); + } +} 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 new file mode 100644 index 0000000000..974f51467a --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderPorts; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderReversed; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor1Port; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor2Port; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderPorts; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderReversed; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor1Port; +import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor2Port; + +public class DriveSubsystem extends SubsystemBase { + // The motors on the left side of the drive. + private final SpeedControllerGroup m_leftMotors = + new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), + new PWMVictorSPX(kLeftMotor2Port)); + + // The motors on the right side of the drive. + private final SpeedControllerGroup m_rightMotors = + new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), + new PWMVictorSPX(kRightMotor2Port)); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + private final Encoder m_leftEncoder = + new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + + // The right-side drive encoder + private final Encoder m_rightEncoder = + new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(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/drivedistanceoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java new file mode 100644 index 0000000000..5f4bbeb6e1 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.drivedistanceoffboard; + +/** + * 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; + + // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! + // These characterization values MUST be determined either experimentally or theoretically + // for *your* robot's drive. + // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these + // values for your robot. + public static final double ksVolts = 1; + public static final double kvVoltSecondsPerMeter = 0.8; + public static final double kaVoltSecondsSquaredPerMeter = 0.15; + + public static final double kp = 1; + + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 1; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java new file mode 100644 index 0000000000..6e1902048d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java @@ -0,0 +1,115 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.drivedistanceoffboard; + +import edu.wpi.first.wpilibj.SpeedController; + +/** + * A simplified stub class that simulates the API of a common "smart" motor controller. + * + *

Has no actual functionality. + */ +public class ExampleSmartMotorController implements SpeedController { + 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 master The master to follow. + */ + public void follow(ExampleSmartMotorController master) { + } + + /** + * 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() { + } + + @Override + public void set(double speed) { + } + + @Override + public double get() { + return 0; + } + + @Override + public void setInverted(boolean isInverted) { + + } + + @Override + public boolean getInverted() { + return false; + } + + @Override + public void disable() { + } + + @Override + public void stopMotor() { + } + + @Override + public void pidWrite(double output) { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java new file mode 100644 index 0000000000..89ab5538a8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.drivedistanceoffboard; + +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/drivedistanceoffboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java new file mode 100644 index 0000000000..e8ac2851c8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.drivedistanceoffboard; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, 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/drivedistanceoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java new file mode 100644 index 0000000000..a0eb86af3d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java @@ -0,0 +1,103 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.drivedistanceoffboard; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled; +import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.XboxController.Button; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants.kDriverControllerPort; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + + // The driver's controller + XboxController m_driverController = new XboxController(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.getY(GenericHID.Hand.kLeft), + m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link 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() { + // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds + new JoystickButton(m_driverController, Button.kA.value) + .whenPressed(new DriveDistanceProfiled(3, m_robotDrive).withTimeout(10)); + + // Do the same thing as above when the 'B' button is pressed, but defined inline + new JoystickButton(m_driverController, Button.kB.value) + .whenPressed( + new TrapezoidProfileCommand( + new TrapezoidProfile( + // Limit the max acceleration and velocity + new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond, + kMaxAccelerationMetersPerSecondSquared), + // End at desired position in meters; implicitly starts at 0 + new TrapezoidProfile.State(3, 0)), + // Pipe the profile state to the drive + setpointState -> m_robotDrive.setDriveStates( + setpointState, + setpointState), + // Require the drive + m_robotDrive) + .beforeStarting(m_robotDrive::resetEncoders) + .withTimeout(10)); + + // Drive at half speed when the bumper is held + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(() -> m_robotDrive.setMaxOutput(0.5)) + .whenReleased(() -> 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 new InstantCommand(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java new file mode 100644 index 0000000000..f89b36a9c9 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands; + +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand; + +import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond; + +/** + * Drives a set distance using a motion profile. + */ +public class DriveDistanceProfiled extends TrapezoidProfileCommand { + /** + * Creates a new DriveDistanceProfiled command. + * + * @param meters The distance to drive. + * @param drive The drive subsystem to use. + */ + public DriveDistanceProfiled(double meters, DriveSubsystem drive) { + super( + new TrapezoidProfile( + // Limit the max acceleration and velocity + new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond, + kMaxAccelerationMetersPerSecondSquared), + // End at desired position in meters; implicitly starts at 0 + new TrapezoidProfile.State(meters, 0)), + // Pipe the profile state to the drive + setpointState -> drive.setDriveStates(setpointState, setpointState), + // Require the drive + drive); + // Reset drive encoders since we're starting at 0 + drive.resetEncoders(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java new file mode 100644 index 0000000000..c8b98f7a0c --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java @@ -0,0 +1,118 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems; + +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController; + +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor1Port; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor2Port; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor1Port; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor2Port; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kp; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.ksVolts; +import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kvVoltSecondsPerMeter; + +public class DriveSubsystem extends SubsystemBase { + // The motors on the left side of the drive. + private final ExampleSmartMotorController m_leftMaster = + new ExampleSmartMotorController(kLeftMotor1Port); + + private final ExampleSmartMotorController m_leftSlave = + new ExampleSmartMotorController(kLeftMotor2Port); + + private final ExampleSmartMotorController m_rightMaster = + new ExampleSmartMotorController(kRightMotor1Port); + + private final ExampleSmartMotorController m_rightSlave = + new ExampleSmartMotorController(kRightMotor2Port); + + private final SimpleMotorFeedforward m_feedforward = + new SimpleMotorFeedforward(ksVolts, + kvVoltSecondsPerMeter, + kaVoltSecondsSquaredPerMeter); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMaster, m_rightMaster); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + m_leftSlave.follow(m_leftMaster); + m_rightSlave.follow(m_rightMaster); + + m_leftMaster.setPID(kp, 0, 0); + m_rightMaster.setPID(kp, 0, 0); + } + + /** + * 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); + } + + /** + * Attempts to follow the given drive states using offboard PID. + * + * @param left The left wheel state. + * @param right The right wheel state. + */ + public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) { + m_leftMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, + left.position, + m_feedforward.calculate(left.velocity)); + m_rightMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, + right.position, + m_feedforward.calculate(right.velocity)); + } + + /** + * Returns the left encoder distance. + * + * @return the left encoder distance + */ + public double getLeftEncoderDistance() { + return m_leftMaster.getEncoderDistance(); + } + + /** + * Returns the right encoder distance. + * + * @return the right encoder distance + */ + public double getRightEncoderDistance() { + return m_rightMaster.getEncoderDistance(); + } + + /** + * Resets the drive encoders. + */ + public void resetEncoders() { + m_leftMaster.resetEncoder(); + m_rightMaster.resetEncoder(); + } + + /** + * 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/elevatortrapezoidprofile/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java new file mode 100644 index 0000000000..d6a84ef753 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java @@ -0,0 +1,115 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile; + +import edu.wpi.first.wpilibj.SpeedController; + +/** + * A simplified stub class that simulates the API of a common "smart" motor controller. + * + *

Has no actual functionality. + */ +public class ExampleSmartMotorController implements SpeedController { + 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 master The master to follow. + */ + public void follow(ExampleSmartMotorController master) { + } + + /** + * 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() { + } + + @Override + public void set(double speed) { + } + + @Override + public double get() { + return 0; + } + + @Override + public void setInverted(boolean isInverted) { + + } + + @Override + public boolean getInverted() { + return false; + } + + @Override + public void disable() { + } + + @Override + public void stopMotor() { + } + + @Override + public void pidWrite(double output) { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java index c6e40c6d9f..918d3d3d9a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java @@ -7,21 +7,18 @@ package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile; -import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.PWMVictorSPX; -import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; public class Robot extends TimedRobot { private static double kDt = 0.02; private final Joystick m_joystick = new Joystick(1); - private final Encoder m_encoder = new Encoder(1, 2); - private final SpeedController m_motor = new PWMVictorSPX(1); - private final PIDController m_controller = new PIDController(1.3, 0.0, 0.7, kDt); + private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1); + // Note: These gains are fake, and will have to be tuned for your robot. + private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5); private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(1.75, 0.75); @@ -30,7 +27,8 @@ public class Robot extends TimedRobot { @Override public void robotInit() { - m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5); + // Note: These gains are fake, and will have to be tuned for your robot. + m_motor.setPID(1.3, 0.0, 0.7); } @Override @@ -50,10 +48,8 @@ public class Robot extends TimedRobot { // toward the goal while obeying the constraints. m_setpoint = profile.calculate(kDt); - double output = m_controller.calculate(m_encoder.getDistance(), - m_setpoint.position); - - // Run controller with profiled setpoint and update motor output - m_motor.set(output); + // Send setpoint to offboard controller PID + m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position, + m_feedforward.calculate(m_setpoint.velocity) / 12.0); } } 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 e775e20d4f..6dd4178cc2 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 @@ -485,7 +485,7 @@ }, { "name": "ArmBot", - "description": "An example command-based robot demonstrating the use of a ProfiledPIDCommand to control an arm.", + "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.", "tags": [ "ArmBot", "PID", @@ -496,6 +496,32 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "ArmBotOffboard", + "description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.", + "tags": [ + "ArmBotOffboard", + "PID", + "Motion Profile" + ], + "foldername": "armbotoffboard", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "DriveDistanceOffboard", + "description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.", + "tags": [ + "DriveDistance", + "PID", + "Motion Profile" + ], + "foldername": "drivedistanceoffboard", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "MecanumControllerCommand", "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.", 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 index ca6a75df99..473d6e18af 100644 --- 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 @@ -43,6 +43,9 @@ public final class Constants { 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 } 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 index 85d8ab7e95..44b09b8cec 100644 --- 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 @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; 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 static edu.wpi.first.wpilibj.XboxController.Button; @@ -85,6 +86,10 @@ public class RobotContainer { // Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout new JoystickButton(m_driverController, Button.kX.value) .whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5)); + + // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5 second timeout + new JoystickButton(m_driverController, Button.kA.value) + .whenPressed(new TurnToAngleProfiled(-90, m_robotDrive).withTimeout(5)); } 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 new file mode 100644 index 0000000000..8fb4320da0 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands; + +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand; + +import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnRateDegPerS; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg; + +/** + * 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(kTurnP, kTurnI, kTurnD, + new TrapezoidProfile.Constraints( + kMaxTurnRateDegPerS, + 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(kTurnToleranceDeg, kTurnRateToleranceDegPerS); + } + + @Override + public boolean isFinished() { + // End when the controller is at the reference. + return getController().atGoal(); + } +}