Skip to content

Commit

Permalink
Add feedforward
Browse files Browse the repository at this point in the history
  • Loading branch information
Gold856 committed Sep 24, 2024
1 parent 1e9e3a9 commit 70de8ea
Show file tree
Hide file tree
Showing 5 changed files with 44 additions and 2 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

#include <utility>

#include <frc/RobotController.h>
#include <frc2/command/Commands.h>

Drive::Drive() {
Expand Down Expand Up @@ -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); });
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,12 @@
#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"
Expand Down Expand Up @@ -73,4 +75,6 @@ class Drive : public frc2::SubsystemBase {
DriveConstants::kTurnI,
DriveConstants::kTurnD,
{DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
frc::SimpleMotorFeedforward<units::radians> m_feedforward{
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
};
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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));
}
Expand Down

0 comments on commit 70de8ea

Please sign in to comment.