diff --git a/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp new file mode 100644 index 0000000000..0de739e7a0 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -0,0 +1,113 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/RamseteCommand.h" + +using namespace frc2; +using namespace units; + +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +RamseteCommand::RamseteCommand( + frc::Trajectory trajectory, std::function pose, + frc::RamseteController controller, volt_t ks, + units::unit_t kv, + units::unit_t ka, + frc::DifferentialDriveKinematics kinematics, + std::function leftSpeed, + std::function rightSpeed, + frc2::PIDController leftController, frc2::PIDController rightController, + std::function output, + std::initializer_list requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_controller(controller), + m_ks(ks), + m_kv(kv), + m_ka(ka), + m_kinematics(kinematics), + m_leftSpeed(leftSpeed), + m_rightSpeed(rightSpeed), + m_leftController(std::make_unique(leftController)), + m_rightController(std::make_unique(rightController)), + m_outputVolts(output) { + AddRequirements(requirements); +} + +RamseteCommand::RamseteCommand( + frc::Trajectory trajectory, std::function pose, + frc::RamseteController controller, + frc::DifferentialDriveKinematics kinematics, + std::function + output, + std::initializer_list requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_controller(controller), + m_ks(0), + m_kv(0), + m_ka(0), + m_kinematics(kinematics), + m_outputVel(output) { + AddRequirements(requirements); +} + +void RamseteCommand::Initialize() { + m_prevTime = 0_s; + auto initialState = m_trajectory.Sample(0_s); + m_prevSpeeds = m_kinematics.ToWheelSpeeds( + frc::ChassisSpeeds{initialState.velocity, 0_mps, + initialState.velocity * initialState.curvature}); + m_timer.Reset(); + m_timer.Start(); + m_leftController->Reset(); + m_rightController->Reset(); +} + +void RamseteCommand::Execute() { + auto curTime = m_timer.Get(); + auto dt = curTime - m_prevTime; + + auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds( + m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime))); + + if (m_leftController.get() != nullptr) { + auto leftFeedforward = + m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left + + m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt; + + auto rightFeedforward = + m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right + + m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt; + + auto leftOutput = + volt_t(m_leftController->Calculate( + m_leftSpeed().to(), targetWheelSpeeds.left.to())) + + leftFeedforward; + + auto rightOutput = volt_t(m_rightController->Calculate( + m_rightSpeed().to(), + targetWheelSpeeds.right.to())) + + rightFeedforward; + + m_outputVolts(leftOutput, rightOutput); + } else { + m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right); + } + + m_prevTime = curTime; + m_prevSpeeds = targetWheelSpeeds; +} + +void RamseteCommand::End(bool interrupted) { m_timer.Stop(); } + +bool RamseteCommand::IsFinished() { + return m_timer.HasPeriodPassed(m_trajectory.TotalTime()); +} diff --git a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h new file mode 100644 index 0000000000..592f194711 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h @@ -0,0 +1,147 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include + +#include "CommandBase.h" +#include "CommandHelper.h" +#include "frc/controller/PIDController.h" +#include "frc/controller/RamseteController.h" +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/trajectory/Trajectory.h" +#include "frc2/Timer.h" + +#pragma once + +namespace frc2 { +/** + * A command that uses a RAMSETE controller to follow a trajectory + * with a differential drive. + * + *

The command handles trajectory-following, PID calculations, and + * feedforwards internally. This is intended to be a more-or-less "complete + * solution" that can be used by teams without a great deal of controls + * expertise. + * + *

Advanced teams seeking more flexibility (for example, those who wish to + * use the onboard PID functionality of a "smart" motor controller) may use the + * secondary constructor that omits the PID and feedforward functionality, + * returning only the raw wheel speeds from the RAMSETE controller. + * + * @see RamseteController + * @see Trajectory + */ +class RamseteCommand : public CommandHelper { + using voltsecondspermeter = + units::compound_unit>; + using voltsecondssquaredpermeter = + units::compound_unit, + units::inverse>; + + public: + /** + * Constructs a new RamseteCommand that, when executed, will follow the + * provided trajectory. PID control and feedforward are handled internally, + * and outputs are scaled -1 to 1 for easy consumption by speed controllers. + * + *

Note: The controller will *not* set the outputVolts to zero upon + * completion of the path - this is left to the user, since it is not + * appropriate for paths with nonstationary endstates. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param controller The RAMSETE controller used to follow the + * trajectory. + * @param ks Constant feedforward term for the robot drive. + * @param kv Velocity-proportional feedforward term for the robot + * drive. + * @param ka Acceleration-proportional feedforward term for the + * robot drive. + * @param kinematics The kinematics for the robot drivetrain. + * @param leftSpeed A function that supplies the speed of the left side + * of the robot drive. + * @param rightSpeed A function that supplies the speed of the right side + * of the robot drive. + * @param leftController The PIDController for the left side of the robot + * drive. + * @param rightController The PIDController for the right side of the robot + * drive. + * @param output A function that consumes the computed left and right + * outputs (in volts) for the robot drive. + * @param requirements The subsystems to require. + */ + RamseteCommand(frc::Trajectory trajectory, std::function pose, + frc::RamseteController controller, units::volt_t ks, + units::unit_t kv, + units::unit_t ka, + frc::DifferentialDriveKinematics kinematics, + std::function leftSpeed, + std::function rightSpeed, + frc2::PIDController leftController, + frc2::PIDController rightController, + std::function output, + std::initializer_list requirements); + + /** + * Constructs a new RamseteCommand that, when executed, will follow the + * provided trajectory. Performs no PID control and calculates no + * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller, + * and will need to be converted into a usable form by the user. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param controller The RAMSETE controller used to follow the + * trajectory. + * @param kinematics The kinematics for the robot drivetrain. + * @param output A function that consumes the computed left and right + * outputs (in volts) for the robot drive. + * @param requirements The subsystems to require. + */ + RamseteCommand(frc::Trajectory trajectory, std::function pose, + frc::RamseteController controller, + frc::DifferentialDriveKinematics kinematics, + std::function + output, + std::initializer_list requirements); + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + private: + frc::Trajectory m_trajectory; + std::function m_pose; + frc::RamseteController m_controller; + const units::volt_t m_ks; + const units::unit_t m_kv; + const units::unit_t m_ka; + frc::DifferentialDriveKinematics m_kinematics; + std::function m_leftSpeed; + std::function m_rightSpeed; + std::unique_ptr m_leftController; + std::unique_ptr m_rightController; + std::function m_outputVolts; + std::function + m_outputVel; + + Timer m_timer; + units::second_t m_prevTime; + frc::DifferentialDriveWheelSpeeds m_prevSpeeds; +}; +} // namespace frc2 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 a5b7a61442..7e5ef96958 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp @@ -47,7 +47,7 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) { } double DriveSubsystem::GetHeading() { - return std::remainder(m_gyro.GetAngle(), 360); + return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.); } 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 820e8ce8b8..0349c718a4 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h @@ -33,7 +33,7 @@ const double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); -const bool kGyroReversed = false; +const bool kGyroReversed = true; const double kStabilizationP = 1; const double kStabilizationI = .5; diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp new file mode 100644 index 0000000000..cd19aeb7d8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/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/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..c5d22d0244 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.ArcadeDrive( + m_driverController.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); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Set up config for trajectory + frc::TrajectoryConfig config(AutoConstants::kMaxSpeed, + AutoConstants::kMaxAcceleration); + // Add kinematics to ensure max speed is actually obeyed + config.SetKinematics(DriveConstants::kDriveKinematics); + + // An example trajectory to follow. All units in meters. + auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory( + // Start at the origin facing the +X direction + frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)), + // Pass through these two interior waypoints, making an 's' curve path + {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)}, + // End 3 meters straight ahead of where we started, facing forward + frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)), + // Pass the config + config); + + frc2::RamseteCommand ramseteCommand( + exampleTrajectory, [this]() { return m_drive.GetPose(); }, + frc::RamseteController(AutoConstants::kRamseteB, + AutoConstants::kRamseteZeta), + DriveConstants::ks, DriveConstants::kv, DriveConstants::ka, + DriveConstants::kDriveKinematics, + [this] { + return units::meters_per_second_t(m_drive.GetLeftEncoder().GetRate()); + }, + [this] { + return units::meters_per_second_t(m_drive.GetRightEncoder().GetRate()); + }, + frc2::PIDController(DriveConstants::kPDriveVel, 0, 0), + frc2::PIDController(DriveConstants::kPDriveVel, 0, 0), + [this](auto left, auto right) { + m_drive.TankDrive(left / 12_V, right / 12_V); + }, + {&m_drive}); + + // no auto + return new frc2::SequentialCommandGroup( + std::move(ramseteCommand), + frc2::InstantCommand([this] { m_drive.TankDrive(0, 0); }, {})); +} diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..3d5307f337 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* 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" + +#include + +#include +#include + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() + : m_left1{kLeftMotor1Port}, + m_left2{kLeftMotor2Port}, + m_right1{kRightMotor1Port}, + m_right2{kRightMotor2Port}, + m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]}, + m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]}, + m_odometry{kDriveKinematics, frc::Pose2d()} { + // 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. + m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())), + frc::DifferentialDriveWheelSpeeds{ + units::meters_per_second_t(m_leftEncoder.GetRate()), + units::meters_per_second_t(m_rightEncoder.GetRate())}); +} + +void DriveSubsystem::ArcadeDrive(double fwd, double rot) { + m_drive.ArcadeDrive(fwd, rot); +} + +void DriveSubsystem::TankDrive(double left, double right) { + m_drive.TankDrive(left, right, false); +} + +void DriveSubsystem::ResetEncoders() { + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); +} + +double DriveSubsystem::GetAverageEncoderDistance() { + return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} + +double DriveSubsystem::GetHeading() { + return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.); +} + +double DriveSubsystem::GetTurnRate() { + return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.); +} + +frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } + +void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { + m_odometry.ResetPosition(pose); +} diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h new file mode 100644 index 0000000000..801e47930d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/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. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include +#include + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace DriveConstants { +const int kLeftMotor1Port = 0; +const int kLeftMotor2Port = 1; +const int kRightMotor1Port = 2; +const int kRightMotor2Port = 3; + +const int kLeftEncoderPorts[]{0, 1}; +const int kRightEncoderPorts[]{2, 3}; +const bool kLeftEncoderReversed = false; +const bool kRightEncoderReversed = true; + +const auto kTrackwidth = .6_m; +const frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth); + +const int kEncoderCPR = 1024; +const double kWheelDiameterInches = 6; +const double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); + +const bool kGyroReversed = true; + +// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! +// These characterization values MUST be determined either experimentally or +// theoretically for *your* robot's drive. The RobotPy Characterization +// Toolsuite provides a convenient tool for obtaining these values for your +// robot. +const auto ks = 1_V; +const auto kv = .8 * 1_V * 1_s / 1_m; +const auto ka = .15 * 1_V * 1_s * 1_s / 1_m; + +// Example value only - as above, this must be tuned for your drive! +const double kPDriveVel = .5; +} // namespace DriveConstants + +namespace AutoConstants { +const auto kMaxSpeed = 3_mps; +const auto kMaxAcceleration = 3_mps_sq; + +// Reasonable baseline values for a RAMSETE follower in units of meters and +// seconds +const double kRamseteB = 2; +const double kRamseteZeta = .7; +} // namespace AutoConstants + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h new file mode 100644 index 0000000000..fa173d39e1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/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/RamseteCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h new file mode 100644 index 0000000000..cc91e0dbf8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/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 "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(.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..620cfd88b8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h @@ -0,0 +1,141 @@ +/*----------------------------------------------------------------------------*/ +/* 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" + +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); + + /** + * Drives the robot using tank controls. Does not square inputs to enable + * composition with external controllers. + * + * @param left the commanded left output + * @param right the commanded right output + */ + void TankDrive(double left, double right); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Gets the average distance of the TWO encoders. + * + * @return the average of the TWO encoder readings + */ + double GetAverageEncoderDistance(); + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + frc::Encoder& GetLeftEncoder(); + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + frc::Encoder& GetRightEncoder(); + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive + * more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + void SetMaxOutput(double maxOutput); + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + double GetHeading(); + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + double GetTurnRate(); + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + frc::Pose2d GetPose(); + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + void ResetOdometry(frc::Pose2d pose); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + // The motor controllers + frc::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; + + // The gyro sensor + frc::ADXRS450_Gyro m_gyro; + + // Odometry class for tracking robot pose + frc::DifferentialDriveOdometry m_odometry; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 1a7925c10e..864ea1dc3e 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -355,5 +355,18 @@ ], "foldername": "DifferentialDriveBot", "gradlebase": "cpp" + }, + { + "name:": "RamseteCommand", + "description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.", + "tags": [ + "RamseteCommand", + "PID", + "Ramsete", + "Trajectory", + "Path following" + ], + "foldername": "RamseteCommand", + "gradlebase": "cpp" } ] diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java new file mode 100644 index 0000000000..754660c058 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -0,0 +1,223 @@ +/*----------------------------------------------------------------------------*/ +/* 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.wpilibj2.command; + +import java.util.function.BiConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that uses a RAMSETE controller ({@link RamseteController}) to follow a trajectory + * {@link Trajectory} with a differential drive. + * + *

The command handles trajectory-following, PID calculations, and feedforwards internally. This + * is intended to be a more-or-less "complete solution" that can be used by teams without a great + * deal of controls expertise. + * + *

Advanced teams seeking more flexibility (for example, those who wish to use the onboard + * PID functionality of a "smart" motor controller) may use the secondary constructor that omits + * the PID and feedforward functionality, returning only the raw wheel speeds from the RAMSETE + * controller. + */ +public class RamseteCommand extends CommandBase { + private final Timer m_timer = new Timer(); + private DifferentialDriveWheelSpeeds m_prevSpeeds; + private double m_prevTime; + + private final Trajectory m_trajectory; + private final Supplier m_pose; + private final RamseteController m_follower; + private final double m_ks; + private final double m_kv; + private final double m_ka; + private final DifferentialDriveKinematics m_kinematics; + private final DoubleSupplier m_leftSpeed; + private final DoubleSupplier m_rightSpeed; + private final PIDController m_leftController; + private final PIDController m_rightController; + private final BiConsumer m_output; + + /** + * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. + * PID control and feedforward are handled internally, and outputs are scaled -1 to 1 for easy + * consumption by speed controllers. + * + *

Note: The controller will *not* set the outputVolts to zero upon completion of the path - + * this + * is left to the user, since it is not appropriate for paths with nonstationary endstates. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param controller The RAMSETE controller used to follow the trajectory. + * @param ksVolts Constant feedforward term for the robot drive. + * @param kvVoltSecondsPerMeter Velocity-proportional feedforward term for the robot + * drive. + * @param kaVoltSecondsSquaredPerMeter Acceleration-proportional feedforward term for the robot + * drive. + * @param kinematics The kinematics for the robot drivetrain. + * @param leftWheelSpeedMetersPerSecond A function that supplies the speed of the left side of + * the robot drive. + * @param rightWheelSpeedMetersPerSecond A function that supplies the speed of the right side of + * the robot drive. + * @param leftController The PIDController for the left side of the robot drive. + * @param rightController The PIDController for the right side of the robot drive. + * @param outputVolts A function that consumes the computed left and right + * outputs (in volts) for the robot drive. + * @param requirements The subsystems to require. + */ + @SuppressWarnings("PMD.ExcessiveParameterList") + public RamseteCommand(Trajectory trajectory, + Supplier pose, + RamseteController controller, + double ksVolts, + double kvVoltSecondsPerMeter, + double kaVoltSecondsSquaredPerMeter, + DifferentialDriveKinematics kinematics, + DoubleSupplier leftWheelSpeedMetersPerSecond, + DoubleSupplier rightWheelSpeedMetersPerSecond, + PIDController leftController, + PIDController rightController, + BiConsumer outputVolts, + Subsystem... requirements) { + m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand"); + m_pose = requireNonNullParam(pose, "pose", "RamseteCommand"); + m_follower = requireNonNullParam(controller, "controller", "RamseteCommand"); + m_ks = ksVolts; + m_kv = kvVoltSecondsPerMeter; + m_ka = kaVoltSecondsSquaredPerMeter; + m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand"); + m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond, + "leftWheelSpeedMetersPerSecond", + "RamseteCommand"); + m_rightSpeed = requireNonNullParam(rightWheelSpeedMetersPerSecond, + "rightWheelSpeedMetersPerSecond", + "RamseteCommand"); + m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand"); + m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand"); + m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand"); + + addRequirements(requirements); + } + + /** + * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. + * Performs no PID control and calculates no feedforwards; outputs are the raw wheel speeds + * from the RAMSETE controller, and will need to be converted into a usable form by the user. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param follower The RAMSETE follower used to follow the trajectory. + * @param kinematics The kinematics for the robot drivetrain. + * @param outputMetersPerSecond A function that consumes the computed left and right + * wheel speeds. + * @param requirements The subsystems to require. + */ + public RamseteCommand(Trajectory trajectory, + Supplier pose, + RamseteController follower, + DifferentialDriveKinematics kinematics, + BiConsumer outputMetersPerSecond, + Subsystem... requirements) { + m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand"); + m_pose = requireNonNullParam(pose, "pose", "RamseteCommand"); + m_follower = requireNonNullParam(follower, "follower", "RamseteCommand"); + m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand"); + m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand"); + + m_ks = 0; + m_kv = 0; + m_ka = 0; + m_leftSpeed = null; + m_rightSpeed = null; + m_leftController = null; + m_rightController = null; + + addRequirements(requirements); + } + + @Override + public void initialize() { + m_prevTime = 0; + var initialState = m_trajectory.sample(0); + m_prevSpeeds = m_kinematics.toWheelSpeeds( + new ChassisSpeeds(initialState.velocityMetersPerSecond, + 0, + initialState.curvatureRadPerMeter + * initialState.velocityMetersPerSecond)); + m_timer.reset(); + m_timer.start(); + m_leftController.reset(); + m_rightController.reset(); + } + + @Override + public void execute() { + double curTime = m_timer.get(); + double dt = curTime - m_prevTime; + + var targetWheelSpeeds = m_kinematics.toWheelSpeeds( + m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime))); + + var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond; + var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond; + + double leftOutput; + double rightOutput; + + if (m_leftController != null) { + double leftFeedforward = + m_ks * Math.signum(leftSpeedSetpoint) + + m_kv * leftSpeedSetpoint + + m_ka * (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt; + + double rightFeedforward = + m_ks * Math.signum(rightSpeedSetpoint) + + m_kv * rightSpeedSetpoint + + m_ka * (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt; + + leftOutput = leftFeedforward + + m_leftController.calculate(m_leftSpeed.getAsDouble(), + leftSpeedSetpoint); + + rightOutput = rightFeedforward + + m_rightController.calculate(m_rightSpeed.getAsDouble(), + rightSpeedSetpoint); + } else { + leftOutput = leftSpeedSetpoint; + rightOutput = rightSpeedSetpoint; + } + + m_output.accept(leftOutput, rightOutput); + + m_prevTime = curTime; + m_prevSpeeds = targetWheelSpeeds; + } + + @Override + public void end(boolean interrupted) { + m_timer.stop(); + } + + @Override + public boolean isFinished() { + return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds()); + } +} 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 3452f93732..39840f7c1c 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 @@ -379,5 +379,19 @@ "foldername": "differentialdrivebot", "gradlebase": "java", "mainclass": "Main" + }, + { + "name:": "RamseteCommand", + "description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.", + "tags": [ + "RamseteCommand", + "PID", + "Ramsete", + "Trajectory", + "Path following" + ], + "foldername": "ramsetecommand", + "gradlebase": "java", + "mainclass": "Main" } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java new file mode 100644 index 0000000000..f7aecdd34e --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java @@ -0,0 +1,74 @@ +/*----------------------------------------------------------------------------*/ +/* 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.ramsetecommand; + +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kLeftMotor1Port = 0; + public static final int kLeftMotor2Port = 1; + public static final int kRightMotor1Port = 2; + public static final int kRightMotor2Port = 3; + + public static final int[] kLeftEncoderPorts = new int[]{0, 1}; + public static final int[] kRightEncoderPorts = new int[]{2, 3}; + public static final boolean kLeftEncoderReversed = false; + public static final boolean kRightEncoderReversed = true; + + public static final double kTrackwidthMeters = .6; + public static final DifferentialDriveKinematics kDriveKinematics = + new DifferentialDriveKinematics(kTrackwidthMeters); + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterMeters = .15; + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR; + + public static final boolean kGyroReversed = true; + + // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! + // These characterization values MUST be determined either experimentally or theoretically + // for *your* robot's drive. + // The 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 = .8; + public static final double kaVoltSecondsSquaredPerMeter = .15; + + // Example value only - as above, this must be tuned for your drive! + public static final double kPDriveVel = .5; + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 1; + } + + public static final class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + + public static final DifferentialDriveKinematicsConstraint kAutoPathConstraints = + new DifferentialDriveKinematicsConstraint(DriveConstants.kDriveKinematics, + kMaxSpeedMetersPerSecond); + + // Reasonable baseline values for a RAMSETE follower in units of meters and seconds + public static final double kRamseteB = 2; + public static final double kRamseteZeta = .7; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java new file mode 100644 index 0000000000..4cce085041 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/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.ramsetecommand; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java new file mode 100644 index 0000000000..18dc814f8b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/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.ramsetecommand; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every 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/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java new file mode 100644 index 0000000000..f8a3a25caa --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java @@ -0,0 +1,134 @@ +/*----------------------------------------------------------------------------*/ +/* 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.ramsetecommand; + +import java.util.List; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.XboxController.Button; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteB; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteZeta; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kPDriveVel; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.ksVolts; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kvVoltSecondsPerMeter; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.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 calling passing it to a + * {@link JoystickButton}. + */ + private void configureButtonBindings() { + // Drive at half speed when the right bumper is held + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(() -> m_robotDrive.setMaxOutput(.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() { + // Create config for trajectory + TrajectoryConfig config = + new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(kDriveKinematics); + + // An example trajectory to follow. All units in meters. + Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + List.of( + new Translation2d(1, 1), + new Translation2d(2, -1) + ), + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(3, 0, new Rotation2d(0)), + // Pass config + config + ); + + RamseteCommand ramseteCommand = new RamseteCommand( + exampleTrajectory, + m_robotDrive::getPose, + new RamseteController(kRamseteB, kRamseteZeta), + ksVolts, + kvVoltSecondsPerMeter, + kaVoltSecondsSquaredPerMeter, + kDriveKinematics, + m_robotDrive.getLeftEncoder()::getRate, + m_robotDrive.getRightEncoder()::getRate, + new PIDController(kPDriveVel, 0, 0), + new PIDController(kPDriveVel, 0, 0), + // RamseteCommand passes volts to the callback, so we have to rescale here + (left, right) -> m_robotDrive.tankDrive(left / 12., right / 12.), + m_robotDrive + ); + + // Run path following command, then stop at the end. + return ramseteCommand.andThen(() -> m_robotDrive.tankDrive(0, 0)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java new file mode 100644 index 0000000000..053fad97c5 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java @@ -0,0 +1,188 @@ +/*----------------------------------------------------------------------------*/ +/* 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.ramsetecommand.subsystems; + +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +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.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kGyroReversed; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderPorts; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderReversed; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor1Port; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor2Port; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderPorts; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderReversed; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightMotor1Port; +import static edu.wpi.first.wpilibj.examples.ramsetecommand.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); + + // The gyro sensor + private final Gyro m_gyro = new ADXRS450_Gyro(); + + // Odometry class for tracking robot pose + DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + } + + @Override + public void periodic() { + // Update the odometry in the periodic block + m_odometry.update(new Rotation2d(getHeading()), + new DifferentialDriveWheelSpeeds( + m_leftEncoder.getRate(), + m_rightEncoder.getRate() + )); + } + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void resetOdometry(Pose2d pose) { + m_odometry.resetPosition(pose); + } + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public void arcadeDrive(double fwd, double rot) { + m_drive.arcadeDrive(fwd, rot); + } + + /** + * Drives the robot using tank controls. Does not square inputs to enable composition with + * external controllers. + * + * @param left the commanded left output + * @param right the commanded right output + */ + public void tankDrive(double left, double right) { + m_drive.tankDrive(left, right, false); + } + + /** + * 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.; + } + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + public Encoder getLeftEncoder() { + return m_leftEncoder; + } + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + public Encoder getRightEncoder() { + return m_rightEncoder; + } + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + public void setMaxOutput(double maxOutput) { + m_drive.setMaxOutput(maxOutput); + } + + /** + * Zeroes the heading of the robot. + */ + public void zeroHeading() { + m_gyro.reset(); + } + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + public double getHeading() { + return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.); + } + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + public double getTurnRate() { + return m_gyro.getRate() * (kGyroReversed ? -1. : 1.); + } +} diff --git a/wpiutil/src/main/native/include/units/units.h b/wpiutil/src/main/native/include/units/units.h index 6b1db1d935..4e4bdbd914 100644 --- a/wpiutil/src/main/native/include/units/units.h +++ b/wpiutil/src/main/native/include/units/units.h @@ -4853,4 +4853,5 @@ using namespace time; using namespace velocity; using namespace acceleration; using namespace angle; +using namespace voltage; } // namespace units