Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[examples] Add ProfiledPID command to RapidReactCommandBot (#7030)

Browse Source
This commit is contained in:
Gold856
2024-10-11 11:43:24 -04:00
committed by GitHub
parent dcf5f55a30
commit 28cb7cf757
7 changed files with 140 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h
View File

@@ -29,6 +29,23 @@ inline constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
((kWheelDiameter * std::numbers::pi) / kEncoderCPR).value();
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These values MUST be determined either experimentally or theoretically for
// *your* robot's drive. The SysId tool provides a convenient method for
// obtaining feedback and feedforward values for your robot.
inline constexpr double kTurnP = 1;
inline constexpr double kTurnI = 0;
inline constexpr double kTurnD = 0;
inline constexpr auto kTurnTolerance = 5_deg;
inline constexpr auto kTurnRateTolerance = 10_deg_per_s;
inline constexpr auto kMaxTurnRate = 100_deg_per_s;
inline constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
inline constexpr auto ks = 1_V;
inline constexpr auto kv = 0.8_V * 1_s / 1_deg;
inline constexpr auto ka = 0.15_V * 1_s * 1_s / 1_deg;
} // namespace DriveConstants
namespace IntakeConstants {

23
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h
View File

@@ -6,11 +6,15 @@
#include <functional>
#include <frc/ADXRS450_Gyro.h>
#include <frc/Encoder.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <frc2/command/CommandPtr.h>
#include <frc2/command/SubsystemBase.h>
#include <units/angle.h>
#include <units/length.h>
#include "Constants.h"
@@ -38,6 +42,15 @@ class Drive : public frc2::SubsystemBase {
[[nodiscard]]
frc2::CommandPtr DriveDistanceCommand(units::meter_t distance, double speed);
/**
* Returns a command that turns to robot to the specified angle using a motion
* profile and PID controller.
*
* @param angle The angle to turn to
*/
[[nodiscard]]
frc2::CommandPtr TurnToAngleCommand(units::degree_t angle);
private:
frc::PWMSparkMax m_leftLeader{DriveConstants::kLeftMotor1Port};
frc::PWMSparkMax m_leftFollower{DriveConstants::kLeftMotor2Port};
@@ -54,4 +67,14 @@ class Drive : public frc2::SubsystemBase {
frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
DriveConstants::kRightEncoderPorts[1],
DriveConstants::kRightEncoderReversed};
frc::ADXRS450_Gyro m_gyro;
frc::ProfiledPIDController<units::radians> m_controller{
DriveConstants::kTurnP,
DriveConstants::kTurnI,
DriveConstants::kTurnD,
{DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
frc::SimpleMotorFeedforward<units::radians> m_feedforward{
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
};