From 70de8ea245c99b28e2157929f29d035535314d62 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Tue, 17 Sep 2024 07:37:04 -0400 Subject: [PATCH] Add feedforward --- .../cpp/subsystems/Drive.cpp | 8 +++++++- .../RapidReactCommandBot/include/Constants.h | 8 ++++++++ .../include/subsystems/Drive.h | 4 ++++ .../rapidreactcommandbot/Constants.java | 8 ++++++++ .../rapidreactcommandbot/subsystems/Drive.java | 18 +++++++++++++++++- 5 files changed, 44 insertions(+), 2 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp index 67eee460675..082402fdc74 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp @@ -6,6 +6,7 @@ #include +#include #include Drive::Drive() { @@ -57,7 +58,12 @@ frc2::CommandPtr Drive::TurnToAngleCommand(units::degree_t angle) { [this, angle] { m_drive.ArcadeDrive( 0, m_controller.Calculate(m_gyro.GetRotation2d().Radians(), - angle)); + angle) + + // Divide feedforward voltage by battery voltage to + // normalize it to [-1, 1] + m_feedforward.Calculate( + m_controller.GetSetpoint().velocity) / + frc::RobotController::GetBatteryVoltage()); }) .Until([this] { return m_controller.AtGoal(); }) .FinallyDo([this] { m_drive.ArcadeDrive(0, 0); }); diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h index 6fb6e865fcc..a266a9c95a3 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h @@ -38,6 +38,14 @@ 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; + +// 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 SysId tool provides a convenient +// method for obtaining these values for your robot. +inline constexpr auto ks = 1_V; +inline constexpr auto kv = 0.8_V * 1_s / 1_rad; +inline constexpr auto ka = 0.15_V * 1_s * 1_s / 1_rad; } // namespace DriveConstants namespace IntakeConstants { diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h index 4b99dabae1d..6a9c32b9782 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h @@ -9,10 +9,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include "Constants.h" @@ -73,4 +75,6 @@ class Drive : public frc2::SubsystemBase { DriveConstants::kTurnI, DriveConstants::kTurnD, {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}}; + frc::SimpleMotorFeedforward m_feedforward{ + DriveConstants::ks, DriveConstants::kv, DriveConstants::ka}; }; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java index 57839f325e9..1178e3f8672 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java @@ -41,6 +41,14 @@ public static final class DriveConstants { public static final double kMaxTurnRateDegPerS = 100; public static final double kMaxTurnAccelerationDegPerSSquared = 300; + + // 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 SysId tool provides a convenient method for obtaining these values for your robot. + public static final double ksVolts = 1; + public static final double kvVoltSecondsPerDegree = 0.8; + public static final double kaVoltSecondsSquaredPerDegree = 0.15; } public static final class ShooterConstants { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java index 08d34aa673a..f205c71f97f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java @@ -4,13 +4,18 @@ package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; @@ -56,6 +61,11 @@ public class Drive extends SubsystemBase { new TrapezoidProfile.Constraints( DriveConstants.kMaxTurnRateDegPerS, DriveConstants.kMaxTurnAccelerationDegPerSSquared)); + private final SimpleMotorFeedforward m_feedforward = + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerDegree, + DriveConstants.kaVoltSecondsSquaredPerDegree); /** Creates a new Drive subsystem. */ public Drive() { @@ -130,7 +140,13 @@ public Command turnToAngleCommand(double angleDeg) { () -> m_controller.reset(m_gyro.getRotation2d().getDegrees()), () -> m_drive.arcadeDrive( - 0, m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg))) + 0, + m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg) + // Divide feedforward voltage by battery voltage to normalize it to [-1, 1] + + m_feedforward + .calculate(RadiansPerSecond.of(m_controller.getSetpoint().velocity)) + .in(Volts) + / RobotController.getBatteryVoltage())) .until(m_controller::atGoal) .finallyDo(() -> m_drive.arcadeDrive(0, 0)); }