mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
Update to match new WPILib organization
This commit is contained in:
Gold856
@@ -24,9 +24,9 @@
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
#include <frc/simulation/BatterySim.h>
|
||||
#include <frc/simulation/RoboRioSim.h>
|
||||
#include <photon/PhotonUtils.h>
|
||||
#include <wpi/simulation/BatterySim.hpp>
|
||||
#include <wpi/simulation/RoboRioSim.hpp>
|
||||
|
||||
void Robot::RobotInit() {}
|
||||
|
||||
@@ -48,7 +48,7 @@ void Robot::AutonomousPeriodic() {}
|
||||
void Robot::AutonomousExit() {}
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}};
|
||||
wpi::math::Pose2d pose{1_m, 1_m, wpi::math::Rotation2d{}};
|
||||
drivetrain.ResetPose(pose, true);
|
||||
}
|
||||
|
||||
@@ -62,8 +62,8 @@ void Robot::TeleopPeriodic() {
|
||||
-1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed;
|
||||
|
||||
bool targetVisible = false;
|
||||
units::degree_t targetYaw = 0.0_deg;
|
||||
units::meter_t targetRange = 0.0_m;
|
||||
wpi::units::degree_t targetYaw = 0.0_deg;
|
||||
wpi::units::meter_t targetRange = 0.0_m;
|
||||
auto results = camera.GetAllUnreadResults();
|
||||
if (results.size() > 0) {
|
||||
// Camera processed a new frame since last
|
||||
@@ -74,12 +74,12 @@ void Robot::TeleopPeriodic() {
|
||||
for (auto& target : result.GetTargets()) {
|
||||
if (target.GetFiducialId() == 7) {
|
||||
// Found Tag 7, record its information
|
||||
targetYaw = units::degree_t{target.GetYaw()};
|
||||
targetYaw = wpi::units::degree_t{target.GetYaw()};
|
||||
targetRange = photon::PhotonUtils::CalculateDistanceToTarget(
|
||||
0.5_m, // Measured with a tape measure, or in CAD
|
||||
1.435_m, // From 2024 game manual for ID 7
|
||||
-30.0_deg, // Measured with a protractor, or in CAD
|
||||
units::degree_t{target.GetPitch()});
|
||||
wpi::units::degree_t{target.GetPitch()});
|
||||
targetVisible = true;
|
||||
}
|
||||
}
|
||||
@@ -114,19 +114,19 @@ void Robot::SimulationPeriodic() {
|
||||
drivetrain.SimulationPeriodic();
|
||||
vision.SimPeriodic(drivetrain.GetSimPose());
|
||||
|
||||
frc::Field2d& debugField = vision.GetSimDebugField();
|
||||
wpi::Field2d& debugField = vision.GetSimDebugField();
|
||||
debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose());
|
||||
debugField.GetObject("EstimatedRobotModules")
|
||||
->SetPoses(drivetrain.GetModulePoses());
|
||||
|
||||
units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
|
||||
units::volt_t loadedBattVolts =
|
||||
frc::sim::BatterySim::Calculate({totalCurrent});
|
||||
wpi::units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
|
||||
wpi::units::volt_t loadedBattVolts =
|
||||
wpi::sim::BatterySim::Calculate({totalCurrent});
|
||||
// Using max(0.1, voltage) here isn't a *physically correct* solution,
|
||||
// but it avoids problems with battery voltage measuring 0.
|
||||
frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts));
|
||||
wpi::sim::RoboRioSim::SetVInVoltage(wpi::units::math::max(0.1_V, loadedBattVolts));
|
||||
}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() { return frc::StartRobot<Robot>(); }
|
||||
int main() { return wpi::StartRobot<Robot>(); }
|
||||
#endif
|
||||
|
||||
@@ -26,18 +26,19 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <wpi/opmode/TimedRobot.hpp>
|
||||
#include <wpi/smartdashboard/SmartDashboard.hpp>
|
||||
|
||||
SwerveDrive::SwerveDrive()
|
||||
: poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
|
||||
frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
|
||||
// gyroSim(gyro),
|
||||
swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kDriveGearRatio,
|
||||
constants::Swerve::kWheelDiameter / 2,
|
||||
constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kSteerGearRatio, kinematics) {}
|
||||
swerveDriveSim(
|
||||
constants::Swerve::kDriveFF, wpi::math::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kDriveGearRatio,
|
||||
constants::Swerve::kWheelDiameter / 2, constants::Swerve::kSteerFF,
|
||||
wpi::math::DCMotor::Falcon500(1), constants::Swerve::kSteerGearRatio,
|
||||
kinematics) {}
|
||||
|
||||
void SwerveDrive::Periodic() {
|
||||
for (auto& currentModule : swerveMods) {
|
||||
@@ -47,27 +48,30 @@ void SwerveDrive::Periodic() {
|
||||
poseEstimator.Update(GetGyroYaw(), GetModulePositions());
|
||||
}
|
||||
|
||||
void SwerveDrive::Drive(units::meters_per_second_t vx,
|
||||
units::meters_per_second_t vy,
|
||||
units::radians_per_second_t omega) {
|
||||
frc::ChassisSpeeds newChassisSpeeds =
|
||||
frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading());
|
||||
void SwerveDrive::Drive(wpi::units::meters_per_second_t vx,
|
||||
wpi::units::meters_per_second_t vy,
|
||||
wpi::units::radians_per_second_t omega) {
|
||||
wpi::math::ChassisSpeeds newChassisSpeeds =
|
||||
wpi::math::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega,
|
||||
GetHeading());
|
||||
SetChassisSpeeds(newChassisSpeeds, true, false);
|
||||
}
|
||||
|
||||
void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds,
|
||||
bool openLoop, bool steerInPlace) {
|
||||
void SwerveDrive::SetChassisSpeeds(
|
||||
const wpi::math::ChassisSpeeds& newChassisSpeeds, bool openLoop,
|
||||
bool steerInPlace) {
|
||||
SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true,
|
||||
steerInPlace);
|
||||
this->targetChassisSpeeds = newChassisSpeeds;
|
||||
}
|
||||
|
||||
void SwerveDrive::SetModuleStates(
|
||||
const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace) {
|
||||
std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
|
||||
frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
|
||||
static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&desaturatedStates),
|
||||
const std::array<wpi::math::SwerveModuleState, 4>& desiredStates,
|
||||
bool openLoop, bool steerInPlace) {
|
||||
std::array<wpi::math::SwerveModuleState, 4> desaturatedStates = desiredStates;
|
||||
wpi::math::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
|
||||
static_cast<wpi::util::array<wpi::math::SwerveModuleState, 4>*>(
|
||||
&desaturatedStates),
|
||||
constants::Swerve::kMaxLinearSpeed);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
|
||||
@@ -76,7 +80,7 @@ void SwerveDrive::SetModuleStates(
|
||||
|
||||
void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); }
|
||||
|
||||
void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
|
||||
void SwerveDrive::ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose) {
|
||||
if (resetSimPose) {
|
||||
swerveDriveSim.Reset(pose, false);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
@@ -89,20 +93,25 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
|
||||
poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetPose() const {
|
||||
wpi::math::Pose2d SwerveDrive::GetPose() const {
|
||||
return poseEstimator.GetEstimatedPosition();
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
|
||||
wpi::math::Rotation2d SwerveDrive::GetHeading() const {
|
||||
return GetPose().Rotation();
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
|
||||
wpi::math::Rotation2d SwerveDrive::GetGyroYaw() const {
|
||||
return gyro.GetRotation2d();
|
||||
}
|
||||
|
||||
frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
|
||||
wpi::math::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
|
||||
return kinematics.ToChassisSpeeds(GetModuleStates());
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
std::array<frc::SwerveModuleState, 4> moduleStates;
|
||||
std::array<wpi::math::SwerveModuleState, 4> SwerveDrive::GetModuleStates()
|
||||
const {
|
||||
std::array<wpi::math::SwerveModuleState, 4> moduleStates;
|
||||
moduleStates[0] = swerveMods[0].GetState();
|
||||
moduleStates[1] = swerveMods[1].GetState();
|
||||
moduleStates[2] = swerveMods[2].GetState();
|
||||
@@ -110,9 +119,9 @@ std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
return moduleStates;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
std::array<wpi::math::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
const {
|
||||
std::array<frc::SwerveModulePosition, 4> modulePositions;
|
||||
std::array<wpi::math::SwerveModulePosition, 4> modulePositions;
|
||||
modulePositions[0] = swerveMods[0].GetPosition();
|
||||
modulePositions[1] = swerveMods[1].GetPosition();
|
||||
modulePositions[2] = swerveMods[2].GetPosition();
|
||||
@@ -120,11 +129,11 @@ std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
return modulePositions;
|
||||
}
|
||||
|
||||
std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<frc::Pose2d, 4> modulePoses;
|
||||
std::array<wpi::math::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<wpi::math::Pose2d, 4> modulePoses;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
const SwerveModule& module = swerveMods[i];
|
||||
modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
|
||||
modulePoses[i] = GetPose().TransformBy(wpi::math::Transform2d{
|
||||
module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
|
||||
}
|
||||
return modulePoses;
|
||||
@@ -132,24 +141,24 @@ std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
|
||||
void SwerveDrive::Log() {
|
||||
std::string table = "Drive/";
|
||||
frc::Pose2d pose = GetPose();
|
||||
frc::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Heading",
|
||||
wpi::math::Pose2d pose = GetPose();
|
||||
wpi::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Heading",
|
||||
pose.Rotation().Degrees().to<double>());
|
||||
frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
|
||||
frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::math::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
|
||||
wpi::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Omega Degrees",
|
||||
chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VX",
|
||||
chassisSpeeds.omega.convert<wpi::units::degrees_per_second>().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Target VX",
|
||||
targetChassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VY",
|
||||
wpi::SmartDashboard::PutNumber(table + "Target VY",
|
||||
targetChassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Target Omega Degrees",
|
||||
targetChassisSpeeds.omega.convert<units::degrees_per_second>()
|
||||
targetChassisSpeeds.omega.convert<wpi::units::degrees_per_second>()
|
||||
.to<double>());
|
||||
|
||||
for (auto& module : swerveMods) {
|
||||
@@ -158,8 +167,8 @@ void SwerveDrive::Log() {
|
||||
}
|
||||
|
||||
void SwerveDrive::SimulationPeriodic() {
|
||||
std::array<units::volt_t, 4> driveInputs;
|
||||
std::array<units::volt_t, 4> steerInputs;
|
||||
std::array<wpi::units::volt_t, 4> driveInputs;
|
||||
std::array<wpi::units::volt_t, 4> steerInputs;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
driveInputs[i] = swerveMods[i].GetDriveVoltage();
|
||||
steerInputs[i] = swerveMods[i].GetSteerVoltage();
|
||||
@@ -167,26 +176,26 @@ void SwerveDrive::SimulationPeriodic() {
|
||||
swerveDriveSim.SetDriveInputs(driveInputs);
|
||||
swerveDriveSim.SetSteerInputs(steerInputs);
|
||||
|
||||
swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
|
||||
swerveDriveSim.Update(wpi::TimedRobot::kDefaultPeriod);
|
||||
|
||||
auto driveStates = swerveDriveSim.GetDriveStates();
|
||||
auto steerStates = swerveDriveSim.GetSteerStates();
|
||||
totalCurrentDraw = 0_A;
|
||||
std::array<units::ampere_t, 4> driveCurrents =
|
||||
std::array<wpi::units::ampere_t, 4> driveCurrents =
|
||||
swerveDriveSim.GetDriveCurrentDraw();
|
||||
for (const auto& current : driveCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
std::array<units::ampere_t, 4> steerCurrents =
|
||||
std::array<wpi::units::ampere_t, 4> steerCurrents =
|
||||
swerveDriveSim.GetSteerCurrentDraw();
|
||||
for (const auto& current : steerCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
units::meter_t drivePos{driveStates[i](0, 0)};
|
||||
units::meters_per_second_t driveRate{driveStates[i](1, 0)};
|
||||
units::radian_t steerPos{steerStates[i](0, 0)};
|
||||
units::radians_per_second_t steerRate{steerStates[i](1, 0)};
|
||||
wpi::units::meter_t drivePos{driveStates[i](0, 0)};
|
||||
wpi::units::meters_per_second_t driveRate{driveStates[i](1, 0)};
|
||||
wpi::units::radian_t steerPos{steerStates[i](0, 0)};
|
||||
wpi::units::radians_per_second_t steerRate{steerStates[i](1, 0)};
|
||||
swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
|
||||
steerPos, steerRate, steerCurrents[i]);
|
||||
}
|
||||
@@ -194,6 +203,8 @@ void SwerveDrive::SimulationPeriodic() {
|
||||
// gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
|
||||
wpi::math::Pose2d SwerveDrive::GetSimPose() const {
|
||||
return swerveDriveSim.GetPose();
|
||||
}
|
||||
|
||||
units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }
|
||||
wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }
|
||||
|
||||
@@ -24,8 +24,8 @@
|
||||
|
||||
#include "subsystems/SwerveDriveSim.h"
|
||||
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/system/Discretization.h>
|
||||
#include <wpi/math/system/Discretization.hpp>
|
||||
#include <wpi/system/RobotController.hpp>
|
||||
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
@@ -33,14 +33,14 @@ int sgn(T val) {
|
||||
}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::meters>& driveFF,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::radians>& steerFF,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: SwerveDriveSim(
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
wpi::math::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-driveFF.GetKv().to<double>() / driveFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
@@ -49,7 +49,7 @@ SwerveDriveSim::SwerveDriveSim(
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
|
||||
driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius,
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
wpi::math::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-steerFF.GetKv().to<double>() / steerFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
@@ -60,12 +60,12 @@ SwerveDriveSim::SwerveDriveSim(
|
||||
steerFF.GetKs(), steerMotor, steerGearing, kinematics) {}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: drivePlant(drivePlant),
|
||||
driveKs(driveKs),
|
||||
driveMotor(driveMotor),
|
||||
@@ -78,19 +78,19 @@ SwerveDriveSim::SwerveDriveSim(
|
||||
kinematics(kinematics) {}
|
||||
|
||||
void SwerveDriveSim::SetDriveInputs(
|
||||
const std::array<units::volt_t, numModules>& inputs) {
|
||||
units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
|
||||
const std::array<wpi::units::volt_t, numModules>& inputs) {
|
||||
wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage();
|
||||
for (int i = 0; i < driveInputs.size(); i++) {
|
||||
units::volt_t input = inputs[i];
|
||||
wpi::units::volt_t input = inputs[i];
|
||||
driveInputs[i] = std::clamp(input, -battVoltage, battVoltage);
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::SetSteerInputs(
|
||||
const std::array<units::volt_t, numModules>& inputs) {
|
||||
units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
|
||||
const std::array<wpi::units::volt_t, numModules>& inputs) {
|
||||
wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage();
|
||||
for (int i = 0; i < steerInputs.size(); i++) {
|
||||
units::volt_t input = inputs[i];
|
||||
wpi::units::volt_t input = inputs[i];
|
||||
steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
|
||||
}
|
||||
}
|
||||
@@ -98,8 +98,8 @@ void SwerveDriveSim::SetSteerInputs(
|
||||
Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
|
||||
units::volt_t kS) {
|
||||
const Eigen::Matrix<double, 2, 1>& x, wpi::units::volt_t input,
|
||||
wpi::units::volt_t kS) {
|
||||
auto Ax = discA * x;
|
||||
double nextStateVel = Ax(1, 0);
|
||||
double inputToStop = nextStateVel / -discB(1, 0);
|
||||
@@ -129,18 +129,18 @@ Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Update(units::second_t dt) {
|
||||
void SwerveDriveSim::Update(wpi::units::second_t dt) {
|
||||
Eigen::Matrix<double, 2, 2> driveDiscA;
|
||||
Eigen::Matrix<double, 2, 1> driveDiscB;
|
||||
frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
|
||||
&driveDiscB);
|
||||
wpi::math::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
|
||||
&driveDiscB);
|
||||
|
||||
Eigen::Matrix<double, 2, 2> steerDiscA;
|
||||
Eigen::Matrix<double, 2, 1> steerDiscB;
|
||||
frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
|
||||
&steerDiscB);
|
||||
wpi::math::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
|
||||
&steerDiscB);
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> moduleDeltas;
|
||||
std::array<wpi::math::SwerveModulePosition, 4> moduleDeltas;
|
||||
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
double prevDriveStatePos = driveStates[i](0, 0);
|
||||
@@ -150,17 +150,17 @@ void SwerveDriveSim::Update(units::second_t dt) {
|
||||
steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i],
|
||||
steerInputs[i], steerKs);
|
||||
double currentSteerStatePos = steerStates[i](0, 0);
|
||||
moduleDeltas[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{currentDriveStatePos - prevDriveStatePos},
|
||||
frc::Rotation2d{units::radian_t{currentSteerStatePos}}};
|
||||
moduleDeltas[i] = wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{currentDriveStatePos - prevDriveStatePos},
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{currentSteerStatePos}}};
|
||||
}
|
||||
|
||||
frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
|
||||
wpi::math::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
|
||||
pose = pose.Exp(twist);
|
||||
omega = twist.dtheta / dt;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
|
||||
void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose, bool preserveMotion) {
|
||||
this->pose = pose;
|
||||
if (!preserveMotion) {
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
@@ -171,7 +171,7 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
@@ -182,40 +182,40 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
wpi::math::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
std::array<wpi::math::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetModulePositions() const {
|
||||
std::array<frc::SwerveModulePosition, numModules> positions;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules> positions;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
positions[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{driveStates[i](0, 0)},
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
|
||||
positions[i] = wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{driveStates[i](0, 0)},
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
|
||||
units::radian_t steerStdDev) {
|
||||
std::array<frc::SwerveModulePosition, numModules> positions;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(wpi::units::meter_t driveStdDev,
|
||||
wpi::units::radian_t steerStdDev) {
|
||||
std::array<wpi::math::SwerveModulePosition, numModules> positions;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
positions[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{driveStates[i](0, 0)} +
|
||||
positions[i] = wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{driveStates[i](0, 0)} +
|
||||
randDist(generator) * driveStdDev,
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
|
||||
randDist(generator) * steerStdDev}};
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)} +
|
||||
randDist(generator) * steerStdDev}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, numModules>
|
||||
std::array<wpi::math::SwerveModuleState, numModules>
|
||||
SwerveDriveSim::GetModuleStates() {
|
||||
std::array<frc::SwerveModuleState, numModules> states;
|
||||
std::array<wpi::math::SwerveModuleState, numModules> states;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
states[i] = frc::SwerveModuleState{
|
||||
units::meters_per_second_t{driveStates[i](1, 0)},
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
|
||||
states[i] = wpi::math::SwerveModuleState{
|
||||
wpi::units::meters_per_second_t{driveStates[i](1, 0)},
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}};
|
||||
}
|
||||
return states;
|
||||
}
|
||||
@@ -230,12 +230,12 @@ SwerveDriveSim::GetSteerStates() const {
|
||||
return steerStates;
|
||||
}
|
||||
|
||||
units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
|
||||
wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
|
||||
|
||||
units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
const frc::DCMotor& motor, units::radians_per_second_t velocity,
|
||||
units::volt_t inputVolts, units::volt_t batteryVolts) const {
|
||||
units::volt_t effVolts = inputVolts - velocity / motor.Kv;
|
||||
wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
const wpi::math::DCMotor& motor, wpi::units::radians_per_second_t velocity,
|
||||
wpi::units::volt_t inputVolts, wpi::units::volt_t batteryVolts) const {
|
||||
wpi::units::volt_t effVolts = inputVolts - velocity / motor.Kv;
|
||||
if (inputVolts >= 0_V) {
|
||||
effVolts = std::clamp(effVolts, 0_V, inputVolts);
|
||||
} else {
|
||||
@@ -245,36 +245,36 @@ units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
return retVal;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
std::array<wpi::units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> currents;
|
||||
std::array<wpi::units::ampere_t, numModules> currents;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
units::radians_per_second_t speed =
|
||||
units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
|
||||
wpi::units::radians_per_second_t speed =
|
||||
wpi::units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
|
||||
driveWheelRadius.to<double>();
|
||||
currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
|
||||
frc::RobotController::GetBatteryVoltage());
|
||||
wpi::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
std::array<wpi::units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> currents;
|
||||
std::array<wpi::units::ampere_t, numModules> currents;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
units::radians_per_second_t speed =
|
||||
units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
|
||||
wpi::units::radians_per_second_t speed =
|
||||
wpi::units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
|
||||
// TODO: If uncommented we get huge current values.. Not sure how to fix
|
||||
// atm. :(
|
||||
currents[i] = 20_A;
|
||||
// currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i],
|
||||
// frc::RobotController::GetBatteryVoltage());
|
||||
// wpi::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
|
||||
units::ampere_t total{0};
|
||||
wpi::units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
|
||||
wpi::units::ampere_t total{0};
|
||||
for (const auto& val : GetDriveCurrentDraw()) {
|
||||
total += val;
|
||||
}
|
||||
|
||||
@@ -26,17 +26,17 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <frc/MathUtil.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <wpi/math/util/MathUtil.hpp>
|
||||
#include <wpi/smartdashboard/SmartDashboard.hpp>
|
||||
#include <wpi/system/RobotController.hpp>
|
||||
|
||||
SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
|
||||
: moduleConstants(consts),
|
||||
driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
|
||||
driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
|
||||
driveMotor(wpi::PWMSparkMax{moduleConstants.driveMotorId}),
|
||||
driveEncoder(wpi::Encoder{moduleConstants.driveEncoderA,
|
||||
moduleConstants.driveEncoderB}),
|
||||
steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
|
||||
steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
|
||||
steerMotor(wpi::PWMSparkMax{moduleConstants.steerMotorId}),
|
||||
steerEncoder(wpi::Encoder{moduleConstants.steerEncoderA,
|
||||
moduleConstants.steerEncoderB}),
|
||||
driveEncoderSim(driveEncoder),
|
||||
steerEncoderSim(steerEncoder) {
|
||||
@@ -48,55 +48,55 @@ SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
|
||||
}
|
||||
|
||||
void SwerveModule::Periodic() {
|
||||
units::volt_t steerPID = units::volt_t{
|
||||
wpi::units::volt_t steerPID = wpi::units::volt_t{
|
||||
steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
|
||||
desiredState.angle.Radians().to<double>())};
|
||||
steerMotor.SetVoltage(steerPID);
|
||||
|
||||
units::volt_t driveFF =
|
||||
wpi::units::volt_t driveFF =
|
||||
constants::Swerve::kDriveFF.Calculate(desiredState.speed);
|
||||
units::volt_t drivePID{0};
|
||||
wpi::units::volt_t drivePID{0};
|
||||
if (!openLoop) {
|
||||
drivePID = units::volt_t{drivePIDController.Calculate(
|
||||
drivePID = wpi::units::volt_t{drivePIDController.Calculate(
|
||||
driveEncoder.GetRate(), desiredState.speed.to<double>())};
|
||||
}
|
||||
driveMotor.SetVoltage(driveFF + drivePID);
|
||||
}
|
||||
|
||||
void SwerveModule::SetDesiredState(frc::SwerveModuleState newState,
|
||||
void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState newState,
|
||||
bool shouldBeOpenLoop, bool steerInPlace) {
|
||||
frc::Rotation2d currentRotation = GetAbsoluteHeading();
|
||||
wpi::math::Rotation2d currentRotation = GetAbsoluteHeading();
|
||||
newState.Optimize(currentRotation);
|
||||
desiredState = newState;
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
|
||||
return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
|
||||
wpi::math::Rotation2d SwerveModule::GetAbsoluteHeading() const {
|
||||
return wpi::math::Rotation2d{wpi::units::radian_t{steerEncoder.GetDistance()}};
|
||||
}
|
||||
|
||||
frc::SwerveModuleState SwerveModule::GetState() const {
|
||||
return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
|
||||
GetAbsoluteHeading()};
|
||||
wpi::math::SwerveModuleState SwerveModule::GetState() const {
|
||||
return wpi::math::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
frc::SwerveModulePosition SwerveModule::GetPosition() const {
|
||||
return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
|
||||
GetAbsoluteHeading()};
|
||||
wpi::math::SwerveModulePosition SwerveModule::GetPosition() const {
|
||||
return wpi::math::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetDriveVoltage() const {
|
||||
return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
wpi::units::volt_t SwerveModule::GetDriveVoltage() const {
|
||||
return driveMotor.Get() * wpi::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetSteerVoltage() const {
|
||||
return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
wpi::units::volt_t SwerveModule::GetSteerVoltage() const {
|
||||
return steerMotor.Get() * wpi::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetDriveCurrentSim() const {
|
||||
wpi::units::ampere_t SwerveModule::GetDriveCurrentSim() const {
|
||||
return driveCurrentSim;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetSteerCurrentSim() const {
|
||||
wpi::units::ampere_t SwerveModule::GetSteerCurrentSim() const {
|
||||
return steerCurrentSim;
|
||||
}
|
||||
|
||||
@@ -105,37 +105,37 @@ constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
|
||||
}
|
||||
|
||||
void SwerveModule::Log() {
|
||||
frc::SwerveModuleState state = GetState();
|
||||
wpi::math::SwerveModuleState state = GetState();
|
||||
|
||||
std::string table =
|
||||
"Module " + std::to_string(moduleConstants.moduleNum) + "/";
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Degrees",
|
||||
frc::AngleModulus(state.angle.Radians())
|
||||
.convert<units::degrees>()
|
||||
wpi::SmartDashboard::PutNumber(table + "Steer Degrees",
|
||||
wpi::math::AngleModulus(state.angle.Radians())
|
||||
.convert<wpi::units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Steer Target Degrees",
|
||||
units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<units::degrees>()
|
||||
wpi::units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<wpi::units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Feet",
|
||||
state.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
state.speed.convert<wpi::units::feet_per_second>().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Target Feet",
|
||||
desiredState.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
desiredState.speed.convert<wpi::units::feet_per_second>().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
driveCurrentSim.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Current",
|
||||
wpi::SmartDashboard::PutNumber(table + "Steer Current",
|
||||
steerCurrentSim.to<double>());
|
||||
}
|
||||
|
||||
void SwerveModule::SimulationUpdate(
|
||||
units::meter_t driveEncoderDist,
|
||||
units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent,
|
||||
units::radian_t steerEncoderDist,
|
||||
units::radians_per_second_t steerEncoderRate,
|
||||
units::ampere_t steerCurrent) {
|
||||
wpi::units::meter_t driveEncoderDist,
|
||||
wpi::units::meters_per_second_t driveEncoderRate, wpi::units::ampere_t driveCurrent,
|
||||
wpi::units::radian_t steerEncoderDist,
|
||||
wpi::units::radians_per_second_t steerEncoderRate,
|
||||
wpi::units::ampere_t steerCurrent) {
|
||||
driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
|
||||
driveEncoderSim.SetRate(driveEncoderRate.to<double>());
|
||||
driveCurrentSim = driveCurrent;
|
||||
|
||||
@@ -26,25 +26,26 @@
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/geometry/Transform3d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angular_acceleration.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/length.h>
|
||||
#include <units/velocity.h>
|
||||
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
|
||||
#include <wpi/apriltag/AprilTagFields.hpp>
|
||||
#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
|
||||
#include <wpi/math/geometry/Transform3d.hpp>
|
||||
#include <wpi/math/geometry/Translation2d.hpp>
|
||||
#include <wpi/units/acceleration.hpp>
|
||||
#include <wpi/units/angular_acceleration.hpp>
|
||||
#include <wpi/units/angular_velocity.hpp>
|
||||
#include <wpi/units/length.hpp>
|
||||
#include <wpi/units/velocity.hpp>
|
||||
|
||||
namespace constants {
|
||||
namespace Vision {
|
||||
inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
|
||||
inline const frc::Transform3d kRobotToCam{
|
||||
frc::Translation3d{0.5_m, 0.0_m, 0.5_m},
|
||||
frc::Rotation3d{0_rad, -30_deg, 0_rad}};
|
||||
inline const frc::AprilTagFieldLayout kTagLayout{
|
||||
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::kDefaultField)};
|
||||
inline const wpi::math::Transform3d kRobotToCam{
|
||||
wpi::math::Translation3d{0.5_m, 0.0_m, 0.5_m},
|
||||
wpi::math::Rotation3d{0_rad, -30_deg, 0_rad}};
|
||||
inline const wpi::apriltag::AprilTagFieldLayout kTagLayout{
|
||||
wpi::apriltag::AprilTagFieldLayout::LoadField(
|
||||
wpi::apriltag::AprilTagField::kDefaultField)};
|
||||
|
||||
inline const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
|
||||
inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
|
||||
@@ -52,23 +53,23 @@ inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
|
||||
namespace Swerve {
|
||||
using namespace units;
|
||||
|
||||
inline constexpr units::meter_t kTrackWidth{18.5_in};
|
||||
inline constexpr units::meter_t kTrackLength{18.5_in};
|
||||
inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
|
||||
inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
|
||||
inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
|
||||
inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
|
||||
inline constexpr units::meter_t kWheelDiameter{4_in};
|
||||
inline constexpr units::meter_t kWheelCircumference{kWheelDiameter *
|
||||
inline constexpr wpi::units::meter_t kTrackWidth{18.5_in};
|
||||
inline constexpr wpi::units::meter_t kTrackLength{18.5_in};
|
||||
inline constexpr wpi::units::meter_t kRobotWidth{25_in + 3.25_in * 2};
|
||||
inline constexpr wpi::units::meter_t kRobotLength{25_in + 3.25_in * 2};
|
||||
inline constexpr wpi::units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
|
||||
inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
|
||||
inline constexpr wpi::units::meter_t kWheelDiameter{4_in};
|
||||
inline constexpr wpi::units::meter_t kWheelCircumference{kWheelDiameter *
|
||||
std::numbers::pi};
|
||||
|
||||
inline constexpr double kDriveGearRatio = 6.75;
|
||||
inline constexpr double kSteerGearRatio = 12.8;
|
||||
|
||||
inline constexpr units::meter_t kDriveDistPerPulse =
|
||||
inline constexpr wpi::units::meter_t kDriveDistPerPulse =
|
||||
kWheelCircumference / 1024.0 / kDriveGearRatio;
|
||||
inline constexpr units::radian_t kSteerRadPerPulse =
|
||||
units::radian_t{2 * std::numbers::pi} / 1024.0;
|
||||
inline constexpr wpi::units::radian_t kSteerRadPerPulse =
|
||||
wpi::units::radian_t{2 * std::numbers::pi} / 1024.0;
|
||||
|
||||
inline constexpr double kDriveKP = 1.0;
|
||||
inline constexpr double kDriveKI = 0.0;
|
||||
@@ -80,10 +81,10 @@ inline constexpr double kSteerKD = 0.25;
|
||||
|
||||
using namespace units;
|
||||
|
||||
inline const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
|
||||
inline const wpi::math::SimpleMotorFeedforward<wpi::units::meters> kDriveFF{
|
||||
0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
|
||||
|
||||
inline const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
|
||||
inline const wpi::math::SimpleMotorFeedforward<wpi::units::radians> kSteerFF{
|
||||
0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
|
||||
|
||||
struct ModuleConstants {
|
||||
@@ -96,12 +97,12 @@ struct ModuleConstants {
|
||||
const int steerEncoderA;
|
||||
const int steerEncoderB;
|
||||
const double angleOffset;
|
||||
const frc::Translation2d centerOffset;
|
||||
const wpi::math::Translation2d centerOffset;
|
||||
|
||||
ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA,
|
||||
int driveEncoderB, int steerMotorId, int steerEncoderA,
|
||||
int steerEncoderB, double angleOffset, units::meter_t xOffset,
|
||||
units::meter_t yOffset)
|
||||
int steerEncoderB, double angleOffset, wpi::units::meter_t xOffset,
|
||||
wpi::units::meter_t yOffset)
|
||||
: moduleNum(moduleNum),
|
||||
driveMotorId(driveMotorId),
|
||||
driveEncoderA(driveEncoderA),
|
||||
@@ -110,7 +111,7 @@ struct ModuleConstants {
|
||||
steerEncoderA(steerEncoderA),
|
||||
steerEncoderB(steerEncoderB),
|
||||
angleOffset(angleOffset),
|
||||
centerOffset(frc::Translation2d{xOffset, yOffset}) {}
|
||||
centerOffset(wpi::math::Translation2d{xOffset, yOffset}) {}
|
||||
};
|
||||
|
||||
inline const ModuleConstants FL_CONSTANTS{
|
||||
|
||||
@@ -24,15 +24,15 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <photon/PhotonCamera.h>
|
||||
#include <wpi/driverstation/XboxController.hpp>
|
||||
#include <wpi/opmode/TimedRobot.hpp>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "VisionSim.h"
|
||||
#include "subsystems/SwerveDrive.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
class Robot : public wpi::TimedRobot {
|
||||
public:
|
||||
void RobotInit() override;
|
||||
void RobotPeriodic() override;
|
||||
@@ -54,7 +54,7 @@ class Robot : public frc::TimedRobot {
|
||||
photon::PhotonCamera camera{constants::Vision::kCameraName};
|
||||
SwerveDrive drivetrain{};
|
||||
VisionSim vision{&camera};
|
||||
frc::XboxController controller{0};
|
||||
wpi::XboxController controller{0};
|
||||
static constexpr auto VISION_TURN_kP = 0.01;
|
||||
static constexpr auto VISION_DES_ANGLE = 0.0_deg;
|
||||
static constexpr auto VISION_STRAFE_kP = 0.5;
|
||||
|
||||
@@ -27,28 +27,28 @@
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <photon/PhotonCamera.h>
|
||||
#include <photon/PhotonPoseEstimator.h>
|
||||
#include <photon/estimation/VisionEstimation.h>
|
||||
#include <photon/simulation/VisionSystemSim.h>
|
||||
#include <photon/simulation/VisionTargetSim.h>
|
||||
#include <photon/targeting/PhotonPipelineResult.h>
|
||||
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
|
||||
#include <wpi/apriltag/AprilTagFields.hpp>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class VisionSim {
|
||||
public:
|
||||
explicit VisionSim(photon::PhotonCamera* camera) {
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
if (wpi::RobotBase::IsSimulation()) {
|
||||
visionSim = std::make_unique<photon::VisionSystemSim>("main");
|
||||
|
||||
visionSim->AddAprilTags(constants::Vision::kTagLayout);
|
||||
|
||||
cameraProp = std::make_unique<photon::SimCameraProperties>();
|
||||
|
||||
cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg});
|
||||
cameraProp->SetCalibration(320, 240, wpi::math::Rotation2d{90_deg});
|
||||
cameraProp->SetCalibError(.35, .10);
|
||||
cameraProp->SetFPS(70_Hz);
|
||||
cameraProp->SetAvgLatency(30_ms);
|
||||
@@ -64,17 +64,17 @@ class VisionSim {
|
||||
|
||||
photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
|
||||
|
||||
void SimPeriodic(frc::Pose2d robotSimPose) {
|
||||
void SimPeriodic(wpi::math::Pose2d robotSimPose) {
|
||||
visionSim->Update(robotSimPose);
|
||||
}
|
||||
|
||||
void ResetSimPose(frc::Pose2d pose) {
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
void ResetSimPose(wpi::math::Pose2d pose) {
|
||||
if (wpi::RobotBase::IsSimulation()) {
|
||||
visionSim->ResetRobotPose(pose);
|
||||
}
|
||||
}
|
||||
|
||||
frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
|
||||
wpi::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
|
||||
|
||||
private:
|
||||
std::unique_ptr<photon::VisionSystemSim> visionSim;
|
||||
|
||||
@@ -24,9 +24,9 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/OnboardIMU.h>
|
||||
#include <frc/estimator/SwerveDrivePoseEstimator.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <wpi/hardware/imu/OnboardIMU.hpp>
|
||||
#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
||||
|
||||
#include "SwerveDriveSim.h"
|
||||
#include "SwerveModule.h"
|
||||
@@ -35,26 +35,26 @@ class SwerveDrive {
|
||||
public:
|
||||
SwerveDrive();
|
||||
void Periodic();
|
||||
void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy,
|
||||
units::radians_per_second_t omega);
|
||||
void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds,
|
||||
void Drive(wpi::units::meters_per_second_t vx, wpi::units::meters_per_second_t vy,
|
||||
wpi::units::radians_per_second_t omega);
|
||||
void SetChassisSpeeds(const wpi::math::ChassisSpeeds& targetChassisSpeeds,
|
||||
bool openLoop, bool steerInPlace);
|
||||
void SetModuleStates(
|
||||
const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace);
|
||||
const std::array<wpi::math::SwerveModuleState, 4>& desiredStates,
|
||||
bool openLoop, bool steerInPlace);
|
||||
void Stop();
|
||||
void ResetPose(const frc::Pose2d& pose, bool resetSimPose);
|
||||
frc::Pose2d GetPose() const;
|
||||
frc::Rotation2d GetHeading() const;
|
||||
frc::Rotation2d GetGyroYaw() const;
|
||||
frc::ChassisSpeeds GetChassisSpeeds() const;
|
||||
std::array<frc::SwerveModuleState, 4> GetModuleStates() const;
|
||||
std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
|
||||
std::array<frc::Pose2d, 4> GetModulePoses() const;
|
||||
void ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose);
|
||||
wpi::math::Pose2d GetPose() const;
|
||||
wpi::math::Rotation2d GetHeading() const;
|
||||
wpi::math::Rotation2d GetGyroYaw() const;
|
||||
wpi::math::ChassisSpeeds GetChassisSpeeds() const;
|
||||
std::array<wpi::math::SwerveModuleState, 4> GetModuleStates() const;
|
||||
std::array<wpi::math::SwerveModulePosition, 4> GetModulePositions() const;
|
||||
std::array<wpi::math::Pose2d, 4> GetModulePoses() const;
|
||||
void Log();
|
||||
void SimulationPeriodic();
|
||||
frc::Pose2d GetSimPose() const;
|
||||
units::ampere_t GetCurrentDraw() const;
|
||||
wpi::math::Pose2d GetSimPose() const;
|
||||
wpi::units::ampere_t GetCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::array<SwerveModule, 4> swerveMods{
|
||||
@@ -62,18 +62,18 @@ class SwerveDrive {
|
||||
SwerveModule{constants::Swerve::FR_CONSTANTS},
|
||||
SwerveModule{constants::Swerve::BL_CONSTANTS},
|
||||
SwerveModule{constants::Swerve::BR_CONSTANTS}};
|
||||
frc::SwerveDriveKinematics<4> kinematics{
|
||||
wpi::math::SwerveDriveKinematics<4> kinematics{
|
||||
swerveMods[0].GetModuleConstants().centerOffset,
|
||||
swerveMods[1].GetModuleConstants().centerOffset,
|
||||
swerveMods[2].GetModuleConstants().centerOffset,
|
||||
swerveMods[3].GetModuleConstants().centerOffset,
|
||||
};
|
||||
frc::OnboardIMU gyro{MountOrientation::kFlat};
|
||||
frc::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||
frc::ChassisSpeeds targetChassisSpeeds{};
|
||||
wpi::OnboardIMU gyro{MountOrientation::kFlat};
|
||||
wpi::math::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||
wpi::math::ChassisSpeeds targetChassisSpeeds{};
|
||||
|
||||
// TODO(Jade) onboard imu doesn't have sim yet
|
||||
// frc::sim::ADXRS450_GyroSim gyroSim;
|
||||
// wpi::sim::ADXRS450_GyroSim gyroSim;
|
||||
SwerveDriveSim swerveDriveSim;
|
||||
units::ampere_t totalCurrentDraw{0};
|
||||
wpi::units::ampere_t totalCurrentDraw{0};
|
||||
};
|
||||
|
||||
@@ -26,77 +26,80 @@
|
||||
|
||||
#include <random>
|
||||
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/system/LinearSystem.h>
|
||||
#include <frc/system/plant/DCMotor.h>
|
||||
#include <units/voltage.h>
|
||||
#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
||||
#include <wpi/math/system/LinearSystem.hpp>
|
||||
#include <wpi/math/system/plant/DCMotor.hpp>
|
||||
#include <wpi/units/voltage.hpp>
|
||||
|
||||
static constexpr int numModules{4};
|
||||
|
||||
class SwerveDriveSim {
|
||||
public:
|
||||
SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics);
|
||||
SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant,
|
||||
units::volt_t driveKs, const frc::DCMotor& driveMotor,
|
||||
double driveGearing, units::meter_t driveWheelRadius,
|
||||
const frc::LinearSystem<2, 1, 2>& steerPlant,
|
||||
units::volt_t steerKs, const frc::DCMotor& steerMotor,
|
||||
double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics);
|
||||
void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
SwerveDriveSim(
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::meters>& driveFF,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::radians>& steerFF,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics);
|
||||
SwerveDriveSim(
|
||||
const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics);
|
||||
void SetDriveInputs(const std::array<wpi::units::volt_t, numModules>& inputs);
|
||||
void SetSteerInputs(const std::array<wpi::units::volt_t, numModules>& inputs);
|
||||
static Eigen::Matrix<double, 2, 1> CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
|
||||
units::volt_t kS);
|
||||
void Update(units::second_t dt);
|
||||
void Reset(const frc::Pose2d& pose, bool preserveMotion);
|
||||
void Reset(const frc::Pose2d& pose,
|
||||
const Eigen::Matrix<double, 2, 1>& x, wpi::units::volt_t input,
|
||||
wpi::units::volt_t kS);
|
||||
void Update(wpi::units::second_t dt);
|
||||
void Reset(const wpi::math::Pose2d& pose, bool preserveMotion);
|
||||
void Reset(const wpi::math::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleSteerStates);
|
||||
frc::Pose2d GetPose() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
|
||||
units::meter_t driveStdDev, units::radian_t steerStdDev);
|
||||
std::array<frc::SwerveModuleState, numModules> GetModuleStates();
|
||||
wpi::math::Pose2d GetPose() const;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules> GetModulePositions()
|
||||
const;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules>
|
||||
GetNoisyModulePositions(wpi::units::meter_t driveStdDev,
|
||||
wpi::units::radian_t steerStdDev);
|
||||
std::array<wpi::math::SwerveModuleState, numModules> GetModuleStates();
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetSteerStates() const;
|
||||
units::radians_per_second_t GetOmega() const;
|
||||
units::ampere_t GetCurrentDraw(const frc::DCMotor& motor,
|
||||
units::radians_per_second_t velocity,
|
||||
units::volt_t inputVolts,
|
||||
units::volt_t batteryVolts) const;
|
||||
std::array<units::ampere_t, numModules> GetDriveCurrentDraw() const;
|
||||
std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
|
||||
units::ampere_t GetTotalCurrentDraw() const;
|
||||
wpi::units::radians_per_second_t GetOmega() const;
|
||||
wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor,
|
||||
wpi::units::radians_per_second_t velocity,
|
||||
wpi::units::volt_t inputVolts,
|
||||
wpi::units::volt_t batteryVolts) const;
|
||||
std::array<wpi::units::ampere_t, numModules> GetDriveCurrentDraw() const;
|
||||
std::array<wpi::units::ampere_t, numModules> GetSteerCurrentDraw() const;
|
||||
wpi::units::ampere_t GetTotalCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::random_device rd{};
|
||||
std::mt19937 generator{rd()};
|
||||
std::normal_distribution<double> randDist{0.0, 1.0};
|
||||
const frc::LinearSystem<2, 1, 2> drivePlant;
|
||||
const units::volt_t driveKs;
|
||||
const frc::DCMotor driveMotor;
|
||||
const wpi::math::LinearSystem<2, 1, 2> drivePlant;
|
||||
const wpi::units::volt_t driveKs;
|
||||
const wpi::math::DCMotor driveMotor;
|
||||
const double driveGearing;
|
||||
const units::meter_t driveWheelRadius;
|
||||
const frc::LinearSystem<2, 1, 2> steerPlant;
|
||||
const units::volt_t steerKs;
|
||||
const frc::DCMotor steerMotor;
|
||||
const wpi::units::meter_t driveWheelRadius;
|
||||
const wpi::math::LinearSystem<2, 1, 2> steerPlant;
|
||||
const wpi::units::volt_t steerKs;
|
||||
const wpi::math::DCMotor steerMotor;
|
||||
const double steerGearing;
|
||||
const frc::SwerveDriveKinematics<numModules> kinematics;
|
||||
std::array<units::volt_t, numModules> driveInputs{};
|
||||
const wpi::math::SwerveDriveKinematics<numModules> kinematics;
|
||||
std::array<wpi::units::volt_t, numModules> driveInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
|
||||
std::array<units::volt_t, numModules> steerInputs{};
|
||||
std::array<wpi::units::volt_t, numModules> steerInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
|
||||
frc::Pose2d pose{frc::Pose2d{}};
|
||||
units::radians_per_second_t omega{0};
|
||||
wpi::math::Pose2d pose{wpi::math::Pose2d{}};
|
||||
wpi::units::radians_per_second_t omega{0};
|
||||
};
|
||||
|
||||
@@ -24,13 +24,13 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/kinematics/SwerveModulePosition.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc/simulation/EncoderSim.h>
|
||||
#include <units/current.h>
|
||||
#include <wpi/hardware/motor/PWMSparkMax.hpp>
|
||||
#include <wpi/hardware/rotation/Encoder.hpp>
|
||||
#include <wpi/math/controller/PIDController.hpp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveModulePosition.hpppp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveModuleState.hpppp>
|
||||
#include <wpi/simulation/EncoderSim.hpp>
|
||||
#include <wpi/units/current.hpp>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
@@ -38,44 +38,44 @@ class SwerveModule {
|
||||
public:
|
||||
explicit SwerveModule(const constants::Swerve::ModuleConstants& consts);
|
||||
void Periodic();
|
||||
void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop,
|
||||
bool steerInPlace);
|
||||
frc::Rotation2d GetAbsoluteHeading() const;
|
||||
frc::SwerveModuleState GetState() const;
|
||||
frc::SwerveModulePosition GetPosition() const;
|
||||
units::volt_t GetDriveVoltage() const;
|
||||
units::volt_t GetSteerVoltage() const;
|
||||
units::ampere_t GetDriveCurrentSim() const;
|
||||
units::ampere_t GetSteerCurrentSim() const;
|
||||
void SetDesiredState(wpi::math::SwerveModuleState newState,
|
||||
bool shouldBeOpenLoop, bool steerInPlace);
|
||||
wpi::math::Rotation2d GetAbsoluteHeading() const;
|
||||
wpi::math::SwerveModuleState GetState() const;
|
||||
wpi::math::SwerveModulePosition GetPosition() const;
|
||||
wpi::units::volt_t GetDriveVoltage() const;
|
||||
wpi::units::volt_t GetSteerVoltage() const;
|
||||
wpi::units::ampere_t GetDriveCurrentSim() const;
|
||||
wpi::units::ampere_t GetSteerCurrentSim() const;
|
||||
constants::Swerve::ModuleConstants GetModuleConstants() const;
|
||||
void Log();
|
||||
void SimulationUpdate(units::meter_t driveEncoderDist,
|
||||
units::meters_per_second_t driveEncoderRate,
|
||||
units::ampere_t driveCurrent,
|
||||
units::radian_t steerEncoderDist,
|
||||
units::radians_per_second_t steerEncoderRate,
|
||||
units::ampere_t steerCurrent);
|
||||
void SimulationUpdate(wpi::units::meter_t driveEncoderDist,
|
||||
wpi::units::meters_per_second_t driveEncoderRate,
|
||||
wpi::units::ampere_t driveCurrent,
|
||||
wpi::units::radian_t steerEncoderDist,
|
||||
wpi::units::radians_per_second_t steerEncoderRate,
|
||||
wpi::units::ampere_t steerCurrent);
|
||||
|
||||
private:
|
||||
const constants::Swerve::ModuleConstants moduleConstants;
|
||||
|
||||
frc::PWMSparkMax driveMotor;
|
||||
frc::Encoder driveEncoder;
|
||||
frc::PWMSparkMax steerMotor;
|
||||
frc::Encoder steerEncoder;
|
||||
wpi::PWMSparkMax driveMotor;
|
||||
wpi::Encoder driveEncoder;
|
||||
wpi::PWMSparkMax steerMotor;
|
||||
wpi::Encoder steerEncoder;
|
||||
|
||||
frc::SwerveModuleState desiredState{};
|
||||
wpi::math::SwerveModuleState desiredState{};
|
||||
bool openLoop{false};
|
||||
|
||||
frc::PIDController drivePIDController{constants::Swerve::kDriveKP,
|
||||
constants::Swerve::kDriveKI,
|
||||
constants::Swerve::kDriveKD};
|
||||
frc::PIDController steerPIDController{constants::Swerve::kSteerKP,
|
||||
constants::Swerve::kSteerKI,
|
||||
constants::Swerve::kSteerKD};
|
||||
wpi::math::PIDController drivePIDController{constants::Swerve::kDriveKP,
|
||||
constants::Swerve::kDriveKI,
|
||||
constants::Swerve::kDriveKD};
|
||||
wpi::math::PIDController steerPIDController{constants::Swerve::kSteerKP,
|
||||
constants::Swerve::kSteerKI,
|
||||
constants::Swerve::kSteerKD};
|
||||
|
||||
frc::sim::EncoderSim driveEncoderSim;
|
||||
units::ampere_t driveCurrentSim{0};
|
||||
frc::sim::EncoderSim steerEncoderSim;
|
||||
units::ampere_t steerCurrentSim{0};
|
||||
wpi::sim::EncoderSim driveEncoderSim;
|
||||
wpi::units::ampere_t driveCurrentSim{0};
|
||||
wpi::sim::EncoderSim steerEncoderSim;
|
||||
wpi::units::ampere_t steerCurrentSim{0};
|
||||
};
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <hal/HAL.h>
|
||||
#include <wpi/hal/HAL.h>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
|
||||
@@ -24,8 +24,8 @@
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
#include <frc/simulation/BatterySim.h>
|
||||
#include <frc/simulation/RoboRioSim.h>
|
||||
#include <wpi/simulation/BatterySim.hpp>
|
||||
#include <wpi/simulation/RoboRioSim.hpp>
|
||||
|
||||
void Robot::RobotInit() {}
|
||||
|
||||
@@ -47,7 +47,7 @@ void Robot::AutonomousPeriodic() {}
|
||||
void Robot::AutonomousExit() {}
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}};
|
||||
wpi::math::Pose2d pose{1_m, 1_m, wpi::math::Rotation2d{}};
|
||||
drivetrain.ResetPose(pose, true);
|
||||
}
|
||||
|
||||
@@ -105,19 +105,19 @@ void Robot::SimulationPeriodic() {
|
||||
drivetrain.SimulationPeriodic();
|
||||
vision.SimPeriodic(drivetrain.GetSimPose());
|
||||
|
||||
frc::Field2d& debugField = vision.GetSimDebugField();
|
||||
wpi::Field2d& debugField = vision.GetSimDebugField();
|
||||
debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose());
|
||||
debugField.GetObject("EstimatedRobotModules")
|
||||
->SetPoses(drivetrain.GetModulePoses());
|
||||
|
||||
units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
|
||||
units::volt_t loadedBattVolts =
|
||||
frc::sim::BatterySim::Calculate({totalCurrent});
|
||||
wpi::units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
|
||||
wpi::units::volt_t loadedBattVolts =
|
||||
wpi::sim::BatterySim::Calculate({totalCurrent});
|
||||
// Using max(0.1, voltage) here isn't a *physically correct* solution,
|
||||
// but it avoids problems with battery voltage measuring 0.
|
||||
frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts));
|
||||
wpi::sim::RoboRioSim::SetVInVoltage(wpi::units::math::max(0.1_V, loadedBattVolts));
|
||||
}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() { return frc::StartRobot<Robot>(); }
|
||||
int main() { return wpi::StartRobot<Robot>(); }
|
||||
#endif
|
||||
|
||||
@@ -26,18 +26,19 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <wpi/opmode/TimedRobot.hpp>
|
||||
#include <wpi/smartdashboard/SmartDashboard.hpp>
|
||||
|
||||
SwerveDrive::SwerveDrive()
|
||||
: poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
|
||||
frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
|
||||
gyroSim(gyro),
|
||||
swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kDriveGearRatio,
|
||||
constants::Swerve::kWheelDiameter / 2,
|
||||
constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kSteerGearRatio, kinematics) {}
|
||||
swerveDriveSim(
|
||||
constants::Swerve::kDriveFF, wpi::math::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kDriveGearRatio,
|
||||
constants::Swerve::kWheelDiameter / 2, constants::Swerve::kSteerFF,
|
||||
wpi::math::DCMotor::Falcon500(1), constants::Swerve::kSteerGearRatio,
|
||||
kinematics) {}
|
||||
|
||||
void SwerveDrive::Periodic() {
|
||||
for (auto& currentModule : swerveMods) {
|
||||
@@ -47,27 +48,30 @@ void SwerveDrive::Periodic() {
|
||||
poseEstimator.Update(GetGyroYaw(), GetModulePositions());
|
||||
}
|
||||
|
||||
void SwerveDrive::Drive(units::meters_per_second_t vx,
|
||||
units::meters_per_second_t vy,
|
||||
units::radians_per_second_t omega) {
|
||||
frc::ChassisSpeeds newChassisSpeeds =
|
||||
frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading());
|
||||
void SwerveDrive::Drive(wpi::units::meters_per_second_t vx,
|
||||
wpi::units::meters_per_second_t vy,
|
||||
wpi::units::radians_per_second_t omega) {
|
||||
wpi::math::ChassisSpeeds newChassisSpeeds =
|
||||
wpi::math::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega,
|
||||
GetHeading());
|
||||
SetChassisSpeeds(newChassisSpeeds, true, false);
|
||||
}
|
||||
|
||||
void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds,
|
||||
bool openLoop, bool steerInPlace) {
|
||||
void SwerveDrive::SetChassisSpeeds(
|
||||
const wpi::math::ChassisSpeeds& newChassisSpeeds, bool openLoop,
|
||||
bool steerInPlace) {
|
||||
SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true,
|
||||
steerInPlace);
|
||||
this->targetChassisSpeeds = newChassisSpeeds;
|
||||
}
|
||||
|
||||
void SwerveDrive::SetModuleStates(
|
||||
const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace) {
|
||||
std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
|
||||
frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
|
||||
static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&desaturatedStates),
|
||||
const std::array<wpi::math::SwerveModuleState, 4>& desiredStates,
|
||||
bool openLoop, bool steerInPlace) {
|
||||
std::array<wpi::math::SwerveModuleState, 4> desaturatedStates = desiredStates;
|
||||
wpi::math::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
|
||||
static_cast<wpi::util::array<wpi::math::SwerveModuleState, 4>*>(
|
||||
&desaturatedStates),
|
||||
constants::Swerve::kMaxLinearSpeed);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
|
||||
@@ -76,7 +80,7 @@ void SwerveDrive::SetModuleStates(
|
||||
|
||||
void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); }
|
||||
|
||||
void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
|
||||
void SwerveDrive::ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose) {
|
||||
if (resetSimPose) {
|
||||
swerveDriveSim.Reset(pose, false);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
@@ -89,20 +93,25 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
|
||||
poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetPose() const {
|
||||
wpi::math::Pose2d SwerveDrive::GetPose() const {
|
||||
return poseEstimator.GetEstimatedPosition();
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
|
||||
wpi::math::Rotation2d SwerveDrive::GetHeading() const {
|
||||
return GetPose().Rotation();
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
|
||||
wpi::math::Rotation2d SwerveDrive::GetGyroYaw() const {
|
||||
return gyro.GetRotation2d();
|
||||
}
|
||||
|
||||
frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
|
||||
wpi::math::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
|
||||
return kinematics.ToChassisSpeeds(GetModuleStates());
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
std::array<frc::SwerveModuleState, 4> moduleStates;
|
||||
std::array<wpi::math::SwerveModuleState, 4> SwerveDrive::GetModuleStates()
|
||||
const {
|
||||
std::array<wpi::math::SwerveModuleState, 4> moduleStates;
|
||||
moduleStates[0] = swerveMods[0].GetState();
|
||||
moduleStates[1] = swerveMods[1].GetState();
|
||||
moduleStates[2] = swerveMods[2].GetState();
|
||||
@@ -110,9 +119,9 @@ std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
return moduleStates;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
std::array<wpi::math::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
const {
|
||||
std::array<frc::SwerveModulePosition, 4> modulePositions;
|
||||
std::array<wpi::math::SwerveModulePosition, 4> modulePositions;
|
||||
modulePositions[0] = swerveMods[0].GetPosition();
|
||||
modulePositions[1] = swerveMods[1].GetPosition();
|
||||
modulePositions[2] = swerveMods[2].GetPosition();
|
||||
@@ -120,11 +129,11 @@ std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
return modulePositions;
|
||||
}
|
||||
|
||||
std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<frc::Pose2d, 4> modulePoses;
|
||||
std::array<wpi::math::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<wpi::math::Pose2d, 4> modulePoses;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
const SwerveModule& module = swerveMods[i];
|
||||
modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
|
||||
modulePoses[i] = GetPose().TransformBy(wpi::math::Transform2d{
|
||||
module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
|
||||
}
|
||||
return modulePoses;
|
||||
@@ -132,24 +141,24 @@ std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
|
||||
void SwerveDrive::Log() {
|
||||
std::string table = "Drive/";
|
||||
frc::Pose2d pose = GetPose();
|
||||
frc::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Heading",
|
||||
wpi::math::Pose2d pose = GetPose();
|
||||
wpi::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Heading",
|
||||
pose.Rotation().Degrees().to<double>());
|
||||
frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
|
||||
frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::math::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
|
||||
wpi::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Omega Degrees",
|
||||
chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VX",
|
||||
chassisSpeeds.omega.convert<wpi::units::degrees_per_second>().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Target VX",
|
||||
targetChassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VY",
|
||||
wpi::SmartDashboard::PutNumber(table + "Target VY",
|
||||
targetChassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Target Omega Degrees",
|
||||
targetChassisSpeeds.omega.convert<units::degrees_per_second>()
|
||||
targetChassisSpeeds.omega.convert<wpi::units::degrees_per_second>()
|
||||
.to<double>());
|
||||
|
||||
for (auto& module : swerveMods) {
|
||||
@@ -158,8 +167,8 @@ void SwerveDrive::Log() {
|
||||
}
|
||||
|
||||
void SwerveDrive::SimulationPeriodic() {
|
||||
std::array<units::volt_t, 4> driveInputs;
|
||||
std::array<units::volt_t, 4> steerInputs;
|
||||
std::array<wpi::units::volt_t, 4> driveInputs;
|
||||
std::array<wpi::units::volt_t, 4> steerInputs;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
driveInputs[i] = swerveMods[i].GetDriveVoltage();
|
||||
steerInputs[i] = swerveMods[i].GetSteerVoltage();
|
||||
@@ -167,26 +176,26 @@ void SwerveDrive::SimulationPeriodic() {
|
||||
swerveDriveSim.SetDriveInputs(driveInputs);
|
||||
swerveDriveSim.SetSteerInputs(steerInputs);
|
||||
|
||||
swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
|
||||
swerveDriveSim.Update(wpi::TimedRobot::kDefaultPeriod);
|
||||
|
||||
auto driveStates = swerveDriveSim.GetDriveStates();
|
||||
auto steerStates = swerveDriveSim.GetSteerStates();
|
||||
totalCurrentDraw = 0_A;
|
||||
std::array<units::ampere_t, 4> driveCurrents =
|
||||
std::array<wpi::units::ampere_t, 4> driveCurrents =
|
||||
swerveDriveSim.GetDriveCurrentDraw();
|
||||
for (const auto& current : driveCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
std::array<units::ampere_t, 4> steerCurrents =
|
||||
std::array<wpi::units::ampere_t, 4> steerCurrents =
|
||||
swerveDriveSim.GetSteerCurrentDraw();
|
||||
for (const auto& current : steerCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
units::meter_t drivePos{driveStates[i](0, 0)};
|
||||
units::meters_per_second_t driveRate{driveStates[i](1, 0)};
|
||||
units::radian_t steerPos{steerStates[i](0, 0)};
|
||||
units::radians_per_second_t steerRate{steerStates[i](1, 0)};
|
||||
wpi::units::meter_t drivePos{driveStates[i](0, 0)};
|
||||
wpi::units::meters_per_second_t driveRate{driveStates[i](1, 0)};
|
||||
wpi::units::radian_t steerPos{steerStates[i](0, 0)};
|
||||
wpi::units::radians_per_second_t steerRate{steerStates[i](1, 0)};
|
||||
swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
|
||||
steerPos, steerRate, steerCurrents[i]);
|
||||
}
|
||||
@@ -194,6 +203,8 @@ void SwerveDrive::SimulationPeriodic() {
|
||||
gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
|
||||
wpi::math::Pose2d SwerveDrive::GetSimPose() const {
|
||||
return swerveDriveSim.GetPose();
|
||||
}
|
||||
|
||||
units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }
|
||||
wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }
|
||||
|
||||
@@ -24,8 +24,8 @@
|
||||
|
||||
#include "subsystems/SwerveDriveSim.h"
|
||||
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/system/Discretization.h>
|
||||
#include <wpi/math/system/Discretization.hpp>
|
||||
#include <wpi/system/RobotController.hpp>
|
||||
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
@@ -33,14 +33,14 @@ int sgn(T val) {
|
||||
}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::meters>& driveFF,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::radians>& steerFF,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: SwerveDriveSim(
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
wpi::math::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-driveFF.GetKv().to<double>() / driveFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
@@ -49,7 +49,7 @@ SwerveDriveSim::SwerveDriveSim(
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
|
||||
driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius,
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
wpi::math::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-steerFF.GetKv().to<double>() / steerFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
@@ -60,12 +60,12 @@ SwerveDriveSim::SwerveDriveSim(
|
||||
steerFF.GetKs(), steerMotor, steerGearing, kinematics) {}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: drivePlant(drivePlant),
|
||||
driveKs(driveKs),
|
||||
driveMotor(driveMotor),
|
||||
@@ -78,19 +78,19 @@ SwerveDriveSim::SwerveDriveSim(
|
||||
kinematics(kinematics) {}
|
||||
|
||||
void SwerveDriveSim::SetDriveInputs(
|
||||
const std::array<units::volt_t, numModules>& inputs) {
|
||||
units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
|
||||
const std::array<wpi::units::volt_t, numModules>& inputs) {
|
||||
wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage();
|
||||
for (int i = 0; i < driveInputs.size(); i++) {
|
||||
units::volt_t input = inputs[i];
|
||||
wpi::units::volt_t input = inputs[i];
|
||||
driveInputs[i] = std::clamp(input, -battVoltage, battVoltage);
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::SetSteerInputs(
|
||||
const std::array<units::volt_t, numModules>& inputs) {
|
||||
units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
|
||||
const std::array<wpi::units::volt_t, numModules>& inputs) {
|
||||
wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage();
|
||||
for (int i = 0; i < steerInputs.size(); i++) {
|
||||
units::volt_t input = inputs[i];
|
||||
wpi::units::volt_t input = inputs[i];
|
||||
steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
|
||||
}
|
||||
}
|
||||
@@ -98,8 +98,8 @@ void SwerveDriveSim::SetSteerInputs(
|
||||
Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
|
||||
units::volt_t kS) {
|
||||
const Eigen::Matrix<double, 2, 1>& x, wpi::units::volt_t input,
|
||||
wpi::units::volt_t kS) {
|
||||
auto Ax = discA * x;
|
||||
double nextStateVel = Ax(1, 0);
|
||||
double inputToStop = nextStateVel / -discB(1, 0);
|
||||
@@ -129,18 +129,18 @@ Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Update(units::second_t dt) {
|
||||
void SwerveDriveSim::Update(wpi::units::second_t dt) {
|
||||
Eigen::Matrix<double, 2, 2> driveDiscA;
|
||||
Eigen::Matrix<double, 2, 1> driveDiscB;
|
||||
frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
|
||||
&driveDiscB);
|
||||
wpi::math::DiscretizeABAB<2, 1>(drivePlant.A(), drivePlant.B(), dt,
|
||||
&driveDiscA, &driveDiscB);
|
||||
|
||||
Eigen::Matrix<double, 2, 2> steerDiscA;
|
||||
Eigen::Matrix<double, 2, 1> steerDiscB;
|
||||
frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
|
||||
&steerDiscB);
|
||||
wpi::math::DiscretizeABAB<2, 1>(steerPlant.A(), steerPlant.B(), dt,
|
||||
&steerDiscA, &steerDiscB);
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> moduleDeltas;
|
||||
std::array<wpi::math::SwerveModulePosition, 4> moduleDeltas;
|
||||
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
double prevDriveStatePos = driveStates[i](0, 0);
|
||||
@@ -150,17 +150,17 @@ void SwerveDriveSim::Update(units::second_t dt) {
|
||||
steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i],
|
||||
steerInputs[i], steerKs);
|
||||
double currentSteerStatePos = steerStates[i](0, 0);
|
||||
moduleDeltas[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{currentDriveStatePos - prevDriveStatePos},
|
||||
frc::Rotation2d{units::radian_t{currentSteerStatePos}}};
|
||||
moduleDeltas[i] = wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{currentDriveStatePos - prevDriveStatePos},
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{currentSteerStatePos}}};
|
||||
}
|
||||
|
||||
frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
|
||||
wpi::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
|
||||
pose = pose.Exp(twist);
|
||||
omega = twist.dtheta / dt;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
|
||||
void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose, bool preserveMotion) {
|
||||
this->pose = pose;
|
||||
if (!preserveMotion) {
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
@@ -171,7 +171,7 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
@@ -182,40 +182,40 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
wpi::math::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
std::array<wpi::math::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetModulePositions() const {
|
||||
std::array<frc::SwerveModulePosition, numModules> positions;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules> positions;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
positions[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{driveStates[i](0, 0)},
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
|
||||
positions[i] = wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{driveStates[i](0, 0)},
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
|
||||
units::radian_t steerStdDev) {
|
||||
std::array<frc::SwerveModulePosition, numModules> positions;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(wpi::units::meter_t driveStdDev,
|
||||
wpi::units::radian_t steerStdDev) {
|
||||
std::array<wpi::math::SwerveModulePosition, numModules> positions;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
positions[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{driveStates[i](0, 0)} +
|
||||
positions[i] = wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{driveStates[i](0, 0)} +
|
||||
randDist(generator) * driveStdDev,
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
|
||||
randDist(generator) * steerStdDev}};
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)} +
|
||||
randDist(generator) * steerStdDev}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, numModules>
|
||||
std::array<wpi::math::SwerveModuleState, numModules>
|
||||
SwerveDriveSim::GetModuleStates() {
|
||||
std::array<frc::SwerveModuleState, numModules> states;
|
||||
std::array<wpi::math::SwerveModuleState, numModules> states;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
states[i] = frc::SwerveModuleState{
|
||||
units::meters_per_second_t{driveStates[i](1, 0)},
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
|
||||
states[i] = wpi::math::SwerveModuleState{
|
||||
wpi::units::meters_per_second_t{driveStates[i](1, 0)},
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}};
|
||||
}
|
||||
return states;
|
||||
}
|
||||
@@ -230,12 +230,12 @@ SwerveDriveSim::GetSteerStates() const {
|
||||
return steerStates;
|
||||
}
|
||||
|
||||
units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
|
||||
wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
|
||||
|
||||
units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
const frc::DCMotor& motor, units::radians_per_second_t velocity,
|
||||
units::volt_t inputVolts, units::volt_t batteryVolts) const {
|
||||
units::volt_t effVolts = inputVolts - velocity / motor.Kv;
|
||||
wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
const wpi::math::DCMotor& motor, wpi::units::radians_per_second_t velocity,
|
||||
wpi::units::volt_t inputVolts, wpi::units::volt_t batteryVolts) const {
|
||||
wpi::units::volt_t effVolts = inputVolts - velocity / motor.Kv;
|
||||
if (inputVolts >= 0_V) {
|
||||
effVolts = std::clamp(effVolts, 0_V, inputVolts);
|
||||
} else {
|
||||
@@ -245,36 +245,36 @@ units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
return retVal;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
std::array<wpi::units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> currents;
|
||||
std::array<wpi::units::ampere_t, numModules> currents;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
units::radians_per_second_t speed =
|
||||
units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
|
||||
wpi::units::radians_per_second_t speed =
|
||||
wpi::units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
|
||||
driveWheelRadius.to<double>();
|
||||
currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
|
||||
frc::RobotController::GetBatteryVoltage());
|
||||
wpi::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
std::array<wpi::units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> currents;
|
||||
std::array<wpi::units::ampere_t, numModules> currents;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
units::radians_per_second_t speed =
|
||||
units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
|
||||
wpi::units::radians_per_second_t speed =
|
||||
wpi::units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
|
||||
// TODO: If uncommented we get huge current values.. Not sure how to fix
|
||||
// atm. :(
|
||||
currents[i] = 20_A;
|
||||
// currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i],
|
||||
// frc::RobotController::GetBatteryVoltage());
|
||||
// wpi::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
|
||||
units::ampere_t total{0};
|
||||
wpi::units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
|
||||
wpi::units::ampere_t total{0};
|
||||
for (const auto& val : GetDriveCurrentDraw()) {
|
||||
total += val;
|
||||
}
|
||||
|
||||
@@ -26,17 +26,17 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <frc/MathUtil.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <wpi/math/util/MathUtil.hpp>
|
||||
#include <wpi/smartdashboard/SmartDashboard.hpp>
|
||||
#include <wpi/system/RobotController.hpp>
|
||||
|
||||
SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
|
||||
: moduleConstants(consts),
|
||||
driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
|
||||
driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
|
||||
driveMotor(wpi::PWMSparkMax{moduleConstants.driveMotorId}),
|
||||
driveEncoder(wpi::Encoder{moduleConstants.driveEncoderA,
|
||||
moduleConstants.driveEncoderB}),
|
||||
steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
|
||||
steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
|
||||
steerMotor(wpi::PWMSparkMax{moduleConstants.steerMotorId}),
|
||||
steerEncoder(wpi::Encoder{moduleConstants.steerEncoderA,
|
||||
moduleConstants.steerEncoderB}),
|
||||
driveEncoderSim(driveEncoder),
|
||||
steerEncoderSim(steerEncoder) {
|
||||
@@ -48,55 +48,55 @@ SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
|
||||
}
|
||||
|
||||
void SwerveModule::Periodic() {
|
||||
units::volt_t steerPID = units::volt_t{
|
||||
wpi::units::volt_t steerPID = wpi::units::volt_t{
|
||||
steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
|
||||
desiredState.angle.Radians().to<double>())};
|
||||
steerMotor.SetVoltage(steerPID);
|
||||
|
||||
units::volt_t driveFF =
|
||||
wpi::units::volt_t driveFF =
|
||||
constants::Swerve::kDriveFF.Calculate(desiredState.speed);
|
||||
units::volt_t drivePID{0};
|
||||
wpi::units::volt_t drivePID{0};
|
||||
if (!openLoop) {
|
||||
drivePID = units::volt_t{drivePIDController.Calculate(
|
||||
drivePID = wpi::units::volt_t{drivePIDController.Calculate(
|
||||
driveEncoder.GetRate(), desiredState.speed.to<double>())};
|
||||
}
|
||||
driveMotor.SetVoltage(driveFF + drivePID);
|
||||
}
|
||||
|
||||
void SwerveModule::SetDesiredState(frc::SwerveModuleState newState,
|
||||
void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState newState,
|
||||
bool shouldBeOpenLoop, bool steerInPlace) {
|
||||
frc::Rotation2d currentRotation = GetAbsoluteHeading();
|
||||
wpi::math::Rotation2d currentRotation = GetAbsoluteHeading();
|
||||
newState.Optimize(currentRotation);
|
||||
desiredState = newState;
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
|
||||
return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
|
||||
wpi::math::Rotation2d SwerveModule::GetAbsoluteHeading() const {
|
||||
return wpi::math::Rotation2d{wpi::units::radian_t{steerEncoder.GetDistance()}};
|
||||
}
|
||||
|
||||
frc::SwerveModuleState SwerveModule::GetState() const {
|
||||
return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
|
||||
GetAbsoluteHeading()};
|
||||
wpi::math::SwerveModuleState SwerveModule::GetState() const {
|
||||
return wpi::math::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
frc::SwerveModulePosition SwerveModule::GetPosition() const {
|
||||
return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
|
||||
GetAbsoluteHeading()};
|
||||
wpi::math::SwerveModulePosition SwerveModule::GetPosition() const {
|
||||
return wpi::math::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetDriveVoltage() const {
|
||||
return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
wpi::units::volt_t SwerveModule::GetDriveVoltage() const {
|
||||
return driveMotor.Get() * wpi::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetSteerVoltage() const {
|
||||
return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
wpi::units::volt_t SwerveModule::GetSteerVoltage() const {
|
||||
return steerMotor.Get() * wpi::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetDriveCurrentSim() const {
|
||||
wpi::units::ampere_t SwerveModule::GetDriveCurrentSim() const {
|
||||
return driveCurrentSim;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetSteerCurrentSim() const {
|
||||
wpi::units::ampere_t SwerveModule::GetSteerCurrentSim() const {
|
||||
return steerCurrentSim;
|
||||
}
|
||||
|
||||
@@ -105,37 +105,37 @@ constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
|
||||
}
|
||||
|
||||
void SwerveModule::Log() {
|
||||
frc::SwerveModuleState state = GetState();
|
||||
wpi::math::SwerveModuleState state = GetState();
|
||||
|
||||
std::string table =
|
||||
"Module " + std::to_string(moduleConstants.moduleNum) + "/";
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Degrees",
|
||||
frc::AngleModulus(state.angle.Radians())
|
||||
.convert<units::degrees>()
|
||||
wpi::SmartDashboard::PutNumber(table + "Steer Degrees",
|
||||
wpi::math::AngleModulus(state.angle.Radians())
|
||||
.convert<wpi::units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Steer Target Degrees",
|
||||
units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<units::degrees>()
|
||||
wpi::units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<wpi::units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Feet",
|
||||
state.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
state.speed.convert<wpi::units::feet_per_second>().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Target Feet",
|
||||
desiredState.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
desiredState.speed.convert<wpi::units::feet_per_second>().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
driveCurrentSim.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Current",
|
||||
wpi::SmartDashboard::PutNumber(table + "Steer Current",
|
||||
steerCurrentSim.to<double>());
|
||||
}
|
||||
|
||||
void SwerveModule::SimulationUpdate(
|
||||
units::meter_t driveEncoderDist,
|
||||
units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent,
|
||||
units::radian_t steerEncoderDist,
|
||||
units::radians_per_second_t steerEncoderRate,
|
||||
units::ampere_t steerCurrent) {
|
||||
wpi::units::meter_t driveEncoderDist,
|
||||
wpi::units::meters_per_second_t driveEncoderRate, wpi::units::ampere_t driveCurrent,
|
||||
wpi::units::radian_t steerEncoderDist,
|
||||
wpi::units::radians_per_second_t steerEncoderRate,
|
||||
wpi::units::ampere_t steerCurrent) {
|
||||
driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
|
||||
driveEncoderSim.SetRate(driveEncoderRate.to<double>());
|
||||
driveCurrentSim = driveCurrent;
|
||||
|
||||
@@ -26,25 +26,26 @@
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/geometry/Transform3d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angular_acceleration.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/length.h>
|
||||
#include <units/velocity.h>
|
||||
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
|
||||
#include <wpi/apriltag/AprilTagFields.hpp>
|
||||
#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
|
||||
#include <wpi/math/geometry/Transform3d.hpp>
|
||||
#include <wpi/math/geometry/Translation2d.hpp>
|
||||
#include <wpi/units/acceleration.hpp>
|
||||
#include <wpi/units/angular_acceleration.hpp>
|
||||
#include <wpi/units/angular_velocity.hpp>
|
||||
#include <wpi/units/length.hpp>
|
||||
#include <wpi/units/velocity.hpp>
|
||||
|
||||
namespace constants {
|
||||
namespace Vision {
|
||||
inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
|
||||
inline const frc::Transform3d kRobotToCam{
|
||||
frc::Translation3d{0.5_m, 0.0_m, 0.5_m},
|
||||
frc::Rotation3d{0_rad, -30_deg, 0_rad}};
|
||||
inline const frc::AprilTagFieldLayout kTagLayout{
|
||||
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::kDefaultField)};
|
||||
inline const wpi::math::Transform3d kRobotToCam{
|
||||
wpi::math::Translation3d{0.5_m, 0.0_m, 0.5_m},
|
||||
wpi::math::Rotation3d{0_rad, -30_deg, 0_rad}};
|
||||
inline const wpi::apriltag::AprilTagFieldLayout kTagLayout{
|
||||
wpi::apriltag::AprilTagFieldLayout::LoadField(
|
||||
wpi::apriltag::AprilTagField::kDefaultField)};
|
||||
|
||||
inline const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
|
||||
inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
|
||||
@@ -52,23 +53,23 @@ inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
|
||||
namespace Swerve {
|
||||
using namespace units;
|
||||
|
||||
inline constexpr units::meter_t kTrackWidth{18.5_in};
|
||||
inline constexpr units::meter_t kTrackLength{18.5_in};
|
||||
inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
|
||||
inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
|
||||
inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
|
||||
inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
|
||||
inline constexpr units::meter_t kWheelDiameter{4_in};
|
||||
inline constexpr units::meter_t kWheelCircumference{kWheelDiameter *
|
||||
inline constexpr wpi::units::meter_t kTrackWidth{18.5_in};
|
||||
inline constexpr wpi::units::meter_t kTrackLength{18.5_in};
|
||||
inline constexpr wpi::units::meter_t kRobotWidth{25_in + 3.25_in * 2};
|
||||
inline constexpr wpi::units::meter_t kRobotLength{25_in + 3.25_in * 2};
|
||||
inline constexpr wpi::units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
|
||||
inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
|
||||
inline constexpr wpi::units::meter_t kWheelDiameter{4_in};
|
||||
inline constexpr wpi::units::meter_t kWheelCircumference{kWheelDiameter *
|
||||
std::numbers::pi};
|
||||
|
||||
inline constexpr double kDriveGearRatio = 6.75;
|
||||
inline constexpr double kSteerGearRatio = 12.8;
|
||||
|
||||
inline constexpr units::meter_t kDriveDistPerPulse =
|
||||
inline constexpr wpi::units::meter_t kDriveDistPerPulse =
|
||||
kWheelCircumference / 1024.0 / kDriveGearRatio;
|
||||
inline constexpr units::radian_t kSteerRadPerPulse =
|
||||
units::radian_t{2 * std::numbers::pi} / 1024.0;
|
||||
inline constexpr wpi::units::radian_t kSteerRadPerPulse =
|
||||
wpi::units::radian_t{2 * std::numbers::pi} / 1024.0;
|
||||
|
||||
inline constexpr double kDriveKP = 1.0;
|
||||
inline constexpr double kDriveKI = 0.0;
|
||||
@@ -80,10 +81,10 @@ inline constexpr double kSteerKD = 0.25;
|
||||
|
||||
using namespace units;
|
||||
|
||||
inline const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
|
||||
inline const wpi::math::SimpleMotorFeedforward<wpi::units::meters> kDriveFF{
|
||||
0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
|
||||
|
||||
inline const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
|
||||
inline const wpi::math::SimpleMotorFeedforward<wpi::units::radians> kSteerFF{
|
||||
0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
|
||||
|
||||
struct ModuleConstants {
|
||||
@@ -96,12 +97,12 @@ struct ModuleConstants {
|
||||
const int steerEncoderA;
|
||||
const int steerEncoderB;
|
||||
const double angleOffset;
|
||||
const frc::Translation2d centerOffset;
|
||||
const wpi::math::Translation2d centerOffset;
|
||||
|
||||
ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA,
|
||||
int driveEncoderB, int steerMotorId, int steerEncoderA,
|
||||
int steerEncoderB, double angleOffset, units::meter_t xOffset,
|
||||
units::meter_t yOffset)
|
||||
int steerEncoderB, double angleOffset, wpi::units::meter_t xOffset,
|
||||
wpi::units::meter_t yOffset)
|
||||
: moduleNum(moduleNum),
|
||||
driveMotorId(driveMotorId),
|
||||
driveEncoderA(driveEncoderA),
|
||||
@@ -110,7 +111,7 @@ struct ModuleConstants {
|
||||
steerEncoderA(steerEncoderA),
|
||||
steerEncoderB(steerEncoderB),
|
||||
angleOffset(angleOffset),
|
||||
centerOffset(frc::Translation2d{xOffset, yOffset}) {}
|
||||
centerOffset(wpi::math::Translation2d{xOffset, yOffset}) {}
|
||||
};
|
||||
|
||||
inline const ModuleConstants FL_CONSTANTS{
|
||||
|
||||
@@ -24,15 +24,15 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <photon/PhotonCamera.h>
|
||||
#include <wpi/driverstation/XboxController.hpp>
|
||||
#include <wpi/opmode/TimedRobot.hpp>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "VisionSim.h"
|
||||
#include "subsystems/SwerveDrive.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
class Robot : public wpi::TimedRobot {
|
||||
public:
|
||||
void RobotInit() override;
|
||||
void RobotPeriodic() override;
|
||||
@@ -54,6 +54,6 @@ class Robot : public frc::TimedRobot {
|
||||
photon::PhotonCamera camera{constants::Vision::kCameraName};
|
||||
SwerveDrive drivetrain{};
|
||||
VisionSim vision{&camera};
|
||||
frc::XboxController controller{0};
|
||||
wpi::XboxController controller{0};
|
||||
static constexpr double VISION_TURN_kP = 0.01;
|
||||
};
|
||||
|
||||
@@ -27,28 +27,28 @@
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <photon/PhotonCamera.h>
|
||||
#include <photon/PhotonPoseEstimator.h>
|
||||
#include <photon/estimation/VisionEstimation.h>
|
||||
#include <photon/simulation/VisionSystemSim.h>
|
||||
#include <photon/simulation/VisionTargetSim.h>
|
||||
#include <photon/targeting/PhotonPipelineResult.h>
|
||||
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
|
||||
#include <wpi/apriltag/AprilTagFields.hpp>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class VisionSim {
|
||||
public:
|
||||
explicit VisionSim(photon::PhotonCamera* camera) {
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
if (wpi::RobotBase::IsSimulation()) {
|
||||
visionSim = std::make_unique<photon::VisionSystemSim>("main");
|
||||
|
||||
visionSim->AddAprilTags(constants::Vision::kTagLayout);
|
||||
|
||||
cameraProp = std::make_unique<photon::SimCameraProperties>();
|
||||
|
||||
cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg});
|
||||
cameraProp->SetCalibration(320, 240, wpi::math::Rotation2d{90_deg});
|
||||
cameraProp->SetCalibError(.35, .10);
|
||||
cameraProp->SetFPS(70_Hz);
|
||||
cameraProp->SetAvgLatency(30_ms);
|
||||
@@ -64,17 +64,17 @@ class VisionSim {
|
||||
|
||||
photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
|
||||
|
||||
void SimPeriodic(frc::Pose2d robotSimPose) {
|
||||
void SimPeriodic(wpi::math::Pose2d robotSimPose) {
|
||||
visionSim->Update(robotSimPose);
|
||||
}
|
||||
|
||||
void ResetSimPose(frc::Pose2d pose) {
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
void ResetSimPose(wpi::math::Pose2d pose) {
|
||||
if (wpi::RobotBase::IsSimulation()) {
|
||||
visionSim->ResetRobotPose(pose);
|
||||
}
|
||||
}
|
||||
|
||||
frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
|
||||
wpi::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
|
||||
|
||||
private:
|
||||
std::unique_ptr<photon::VisionSystemSim> visionSim;
|
||||
|
||||
@@ -26,9 +26,9 @@
|
||||
|
||||
#include <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/SPI.h>
|
||||
#include <frc/estimator/SwerveDrivePoseEstimator.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/simulation/ADXRS450_GyroSim.h>
|
||||
#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
||||
|
||||
#include "SwerveDriveSim.h"
|
||||
#include "SwerveModule.h"
|
||||
@@ -37,26 +37,26 @@ class SwerveDrive {
|
||||
public:
|
||||
SwerveDrive();
|
||||
void Periodic();
|
||||
void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy,
|
||||
units::radians_per_second_t omega);
|
||||
void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds,
|
||||
void Drive(wpi::units::meters_per_second_t vx, wpi::units::meters_per_second_t vy,
|
||||
wpi::units::radians_per_second_t omega);
|
||||
void SetChassisSpeeds(const wpi::math::ChassisSpeeds& targetChassisSpeeds,
|
||||
bool openLoop, bool steerInPlace);
|
||||
void SetModuleStates(
|
||||
const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace);
|
||||
const std::array<wpi::math::SwerveModuleState, 4>& desiredStates,
|
||||
bool openLoop, bool steerInPlace);
|
||||
void Stop();
|
||||
void ResetPose(const frc::Pose2d& pose, bool resetSimPose);
|
||||
frc::Pose2d GetPose() const;
|
||||
frc::Rotation2d GetHeading() const;
|
||||
frc::Rotation2d GetGyroYaw() const;
|
||||
frc::ChassisSpeeds GetChassisSpeeds() const;
|
||||
std::array<frc::SwerveModuleState, 4> GetModuleStates() const;
|
||||
std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
|
||||
std::array<frc::Pose2d, 4> GetModulePoses() const;
|
||||
void ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose);
|
||||
wpi::math::Pose2d GetPose() const;
|
||||
wpi::math::Rotation2d GetHeading() const;
|
||||
wpi::math::Rotation2d GetGyroYaw() const;
|
||||
wpi::math::ChassisSpeeds GetChassisSpeeds() const;
|
||||
std::array<wpi::math::SwerveModuleState, 4> GetModuleStates() const;
|
||||
std::array<wpi::math::SwerveModulePosition, 4> GetModulePositions() const;
|
||||
std::array<wpi::math::Pose2d, 4> GetModulePoses() const;
|
||||
void Log();
|
||||
void SimulationPeriodic();
|
||||
frc::Pose2d GetSimPose() const;
|
||||
units::ampere_t GetCurrentDraw() const;
|
||||
wpi::math::Pose2d GetSimPose() const;
|
||||
wpi::units::ampere_t GetCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::array<SwerveModule, 4> swerveMods{
|
||||
@@ -64,17 +64,17 @@ class SwerveDrive {
|
||||
SwerveModule{constants::Swerve::FR_CONSTANTS},
|
||||
SwerveModule{constants::Swerve::BL_CONSTANTS},
|
||||
SwerveModule{constants::Swerve::BR_CONSTANTS}};
|
||||
frc::SwerveDriveKinematics<4> kinematics{
|
||||
wpi::math::SwerveDriveKinematics<4> kinematics{
|
||||
swerveMods[0].GetModuleConstants().centerOffset,
|
||||
swerveMods[1].GetModuleConstants().centerOffset,
|
||||
swerveMods[2].GetModuleConstants().centerOffset,
|
||||
swerveMods[3].GetModuleConstants().centerOffset,
|
||||
};
|
||||
frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
|
||||
frc::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||
frc::ChassisSpeeds targetChassisSpeeds{};
|
||||
wpi::ADXRS450_Gyro gyro{wpi::SPI::Port::kOnboardCS0};
|
||||
wpi::math::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||
wpi::math::ChassisSpeeds targetChassisSpeeds{};
|
||||
|
||||
frc::sim::ADXRS450_GyroSim gyroSim;
|
||||
wpi::sim::ADXRS450_GyroSim gyroSim;
|
||||
SwerveDriveSim swerveDriveSim;
|
||||
units::ampere_t totalCurrentDraw{0};
|
||||
wpi::units::ampere_t totalCurrentDraw{0};
|
||||
};
|
||||
|
||||
@@ -26,77 +26,80 @@
|
||||
|
||||
#include <random>
|
||||
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/system/LinearSystem.h>
|
||||
#include <frc/system/plant/DCMotor.h>
|
||||
#include <units/voltage.h>
|
||||
#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
||||
#include <wpi/math/system/LinearSystem.hpp>
|
||||
#include <wpi/math/system/plant/DCMotor.hpp>
|
||||
#include <wpi/units/voltage.hpp>
|
||||
|
||||
static constexpr int numModules{4};
|
||||
|
||||
class SwerveDriveSim {
|
||||
public:
|
||||
SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics);
|
||||
SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant,
|
||||
units::volt_t driveKs, const frc::DCMotor& driveMotor,
|
||||
double driveGearing, units::meter_t driveWheelRadius,
|
||||
const frc::LinearSystem<2, 1, 2>& steerPlant,
|
||||
units::volt_t steerKs, const frc::DCMotor& steerMotor,
|
||||
double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics);
|
||||
void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
SwerveDriveSim(
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::meters>& driveFF,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::radians>& steerFF,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics);
|
||||
SwerveDriveSim(
|
||||
const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics);
|
||||
void SetDriveInputs(const std::array<wpi::units::volt_t, numModules>& inputs);
|
||||
void SetSteerInputs(const std::array<wpi::units::volt_t, numModules>& inputs);
|
||||
static Eigen::Matrix<double, 2, 1> CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
|
||||
units::volt_t kS);
|
||||
void Update(units::second_t dt);
|
||||
void Reset(const frc::Pose2d& pose, bool preserveMotion);
|
||||
void Reset(const frc::Pose2d& pose,
|
||||
const Eigen::Matrix<double, 2, 1>& x, wpi::units::volt_t input,
|
||||
wpi::units::volt_t kS);
|
||||
void Update(wpi::units::second_t dt);
|
||||
void Reset(const wpi::math::Pose2d& pose, bool preserveMotion);
|
||||
void Reset(const wpi::math::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleSteerStates);
|
||||
frc::Pose2d GetPose() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
|
||||
units::meter_t driveStdDev, units::radian_t steerStdDev);
|
||||
std::array<frc::SwerveModuleState, numModules> GetModuleStates();
|
||||
wpi::math::Pose2d GetPose() const;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules> GetModulePositions()
|
||||
const;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules>
|
||||
GetNoisyModulePositions(wpi::units::meter_t driveStdDev,
|
||||
wpi::units::radian_t steerStdDev);
|
||||
std::array<wpi::math::SwerveModuleState, numModules> GetModuleStates();
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetSteerStates() const;
|
||||
units::radians_per_second_t GetOmega() const;
|
||||
units::ampere_t GetCurrentDraw(const frc::DCMotor& motor,
|
||||
units::radians_per_second_t velocity,
|
||||
units::volt_t inputVolts,
|
||||
units::volt_t batteryVolts) const;
|
||||
std::array<units::ampere_t, numModules> GetDriveCurrentDraw() const;
|
||||
std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
|
||||
units::ampere_t GetTotalCurrentDraw() const;
|
||||
wpi::units::radians_per_second_t GetOmega() const;
|
||||
wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor,
|
||||
wpi::units::radians_per_second_t velocity,
|
||||
wpi::units::volt_t inputVolts,
|
||||
wpi::units::volt_t batteryVolts) const;
|
||||
std::array<wpi::units::ampere_t, numModules> GetDriveCurrentDraw() const;
|
||||
std::array<wpi::units::ampere_t, numModules> GetSteerCurrentDraw() const;
|
||||
wpi::units::ampere_t GetTotalCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::random_device rd{};
|
||||
std::mt19937 generator{rd()};
|
||||
std::normal_distribution<double> randDist{0.0, 1.0};
|
||||
const frc::LinearSystem<2, 1, 2> drivePlant;
|
||||
const units::volt_t driveKs;
|
||||
const frc::DCMotor driveMotor;
|
||||
const wpi::math::LinearSystem<2, 1, 2> drivePlant;
|
||||
const wpi::units::volt_t driveKs;
|
||||
const wpi::math::DCMotor driveMotor;
|
||||
const double driveGearing;
|
||||
const units::meter_t driveWheelRadius;
|
||||
const frc::LinearSystem<2, 1, 2> steerPlant;
|
||||
const units::volt_t steerKs;
|
||||
const frc::DCMotor steerMotor;
|
||||
const wpi::units::meter_t driveWheelRadius;
|
||||
const wpi::math::LinearSystem<2, 1, 2> steerPlant;
|
||||
const wpi::units::volt_t steerKs;
|
||||
const wpi::math::DCMotor steerMotor;
|
||||
const double steerGearing;
|
||||
const frc::SwerveDriveKinematics<numModules> kinematics;
|
||||
std::array<units::volt_t, numModules> driveInputs{};
|
||||
const wpi::math::SwerveDriveKinematics<numModules> kinematics;
|
||||
std::array<wpi::units::volt_t, numModules> driveInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
|
||||
std::array<units::volt_t, numModules> steerInputs{};
|
||||
std::array<wpi::units::volt_t, numModules> steerInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
|
||||
frc::Pose2d pose{frc::Pose2d{}};
|
||||
units::radians_per_second_t omega{0};
|
||||
wpi::math::Pose2d pose{wpi::math::Pose2d{}};
|
||||
wpi::units::radians_per_second_t omega{0};
|
||||
};
|
||||
|
||||
@@ -24,13 +24,13 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/kinematics/SwerveModulePosition.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc/simulation/EncoderSim.h>
|
||||
#include <units/current.h>
|
||||
#include <wpi/hardware/motor/PWMSparkMax.hpp>
|
||||
#include <wpi/hardware/rotation/Encoder.hpp>
|
||||
#include <wpi/math/controller/PIDController.hpp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveModulePosition.hpppp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveModuleState.hpppp>
|
||||
#include <wpi/simulation/EncoderSim.hpp>
|
||||
#include <wpi/units/current.hpp>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
@@ -38,44 +38,44 @@ class SwerveModule {
|
||||
public:
|
||||
explicit SwerveModule(const constants::Swerve::ModuleConstants& consts);
|
||||
void Periodic();
|
||||
void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop,
|
||||
bool steerInPlace);
|
||||
frc::Rotation2d GetAbsoluteHeading() const;
|
||||
frc::SwerveModuleState GetState() const;
|
||||
frc::SwerveModulePosition GetPosition() const;
|
||||
units::volt_t GetDriveVoltage() const;
|
||||
units::volt_t GetSteerVoltage() const;
|
||||
units::ampere_t GetDriveCurrentSim() const;
|
||||
units::ampere_t GetSteerCurrentSim() const;
|
||||
void SetDesiredState(wpi::math::SwerveModuleState newState,
|
||||
bool shouldBeOpenLoop, bool steerInPlace);
|
||||
wpi::math::Rotation2d GetAbsoluteHeading() const;
|
||||
wpi::math::SwerveModuleState GetState() const;
|
||||
wpi::math::SwerveModulePosition GetPosition() const;
|
||||
wpi::units::volt_t GetDriveVoltage() const;
|
||||
wpi::units::volt_t GetSteerVoltage() const;
|
||||
wpi::units::ampere_t GetDriveCurrentSim() const;
|
||||
wpi::units::ampere_t GetSteerCurrentSim() const;
|
||||
constants::Swerve::ModuleConstants GetModuleConstants() const;
|
||||
void Log();
|
||||
void SimulationUpdate(units::meter_t driveEncoderDist,
|
||||
units::meters_per_second_t driveEncoderRate,
|
||||
units::ampere_t driveCurrent,
|
||||
units::radian_t steerEncoderDist,
|
||||
units::radians_per_second_t steerEncoderRate,
|
||||
units::ampere_t steerCurrent);
|
||||
void SimulationUpdate(wpi::units::meter_t driveEncoderDist,
|
||||
wpi::units::meters_per_second_t driveEncoderRate,
|
||||
wpi::units::ampere_t driveCurrent,
|
||||
wpi::units::radian_t steerEncoderDist,
|
||||
wpi::units::radians_per_second_t steerEncoderRate,
|
||||
wpi::units::ampere_t steerCurrent);
|
||||
|
||||
private:
|
||||
const constants::Swerve::ModuleConstants moduleConstants;
|
||||
|
||||
frc::PWMSparkMax driveMotor;
|
||||
frc::Encoder driveEncoder;
|
||||
frc::PWMSparkMax steerMotor;
|
||||
frc::Encoder steerEncoder;
|
||||
wpi::PWMSparkMax driveMotor;
|
||||
wpi::Encoder driveEncoder;
|
||||
wpi::PWMSparkMax steerMotor;
|
||||
wpi::Encoder steerEncoder;
|
||||
|
||||
frc::SwerveModuleState desiredState{};
|
||||
wpi::math::SwerveModuleState desiredState{};
|
||||
bool openLoop{false};
|
||||
|
||||
frc::PIDController drivePIDController{constants::Swerve::kDriveKP,
|
||||
constants::Swerve::kDriveKI,
|
||||
constants::Swerve::kDriveKD};
|
||||
frc::PIDController steerPIDController{constants::Swerve::kSteerKP,
|
||||
constants::Swerve::kSteerKI,
|
||||
constants::Swerve::kSteerKD};
|
||||
wpi::math::PIDController drivePIDController{constants::Swerve::kDriveKP,
|
||||
constants::Swerve::kDriveKI,
|
||||
constants::Swerve::kDriveKD};
|
||||
wpi::math::PIDController steerPIDController{constants::Swerve::kSteerKP,
|
||||
constants::Swerve::kSteerKI,
|
||||
constants::Swerve::kSteerKD};
|
||||
|
||||
frc::sim::EncoderSim driveEncoderSim;
|
||||
units::ampere_t driveCurrentSim{0};
|
||||
frc::sim::EncoderSim steerEncoderSim;
|
||||
units::ampere_t steerCurrentSim{0};
|
||||
wpi::sim::EncoderSim driveEncoderSim;
|
||||
wpi::units::ampere_t driveCurrentSim{0};
|
||||
wpi::sim::EncoderSim steerEncoderSim;
|
||||
wpi::units::ampere_t steerCurrentSim{0};
|
||||
};
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <hal/HAL.h>
|
||||
#include <wpi/hal/HAL.h>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
|
||||
@@ -24,8 +24,8 @@
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
#include <frc/simulation/BatterySim.h>
|
||||
#include <frc/simulation/RoboRioSim.h>
|
||||
#include <wpi/simulation/BatterySim.hpp>
|
||||
#include <wpi/simulation/RoboRioSim.hpp>
|
||||
|
||||
void Robot::RobotInit() {}
|
||||
|
||||
@@ -50,7 +50,7 @@ void Robot::AutonomousPeriodic() {}
|
||||
void Robot::AutonomousExit() {}
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}};
|
||||
wpi::math::Pose2d pose{1_m, 1_m, wpi::math::Rotation2d{}};
|
||||
drivetrain.ResetPose(pose, true);
|
||||
}
|
||||
|
||||
@@ -68,7 +68,7 @@ void Robot::TeleopPeriodic() {
|
||||
|
||||
// Calculate whether the gamepiece launcher runs based on our global pose
|
||||
// estimate.
|
||||
frc::Pose2d curPose = drivetrain.GetPose();
|
||||
wpi::math::Pose2d curPose = drivetrain.GetPose();
|
||||
bool shouldRun = (curPose.Y() > 2.0_m &&
|
||||
curPose.X() < 4.0_m); // Close enough to blue speaker
|
||||
launcher.setRunning(shouldRun);
|
||||
@@ -87,20 +87,20 @@ void Robot::SimulationPeriodic() {
|
||||
drivetrain.SimulationPeriodic();
|
||||
vision.SimPeriodic(drivetrain.GetSimPose());
|
||||
|
||||
frc::Field2d& debugField = vision.GetSimDebugField();
|
||||
wpi::Field2d& debugField = vision.GetSimDebugField();
|
||||
debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose());
|
||||
debugField.GetObject("EstimatedRobotModules")
|
||||
->SetPoses(drivetrain.GetModulePoses());
|
||||
|
||||
units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
|
||||
units::volt_t loadedBattVolts =
|
||||
frc::sim::BatterySim::Calculate({totalCurrent});
|
||||
wpi::units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
|
||||
wpi::units::volt_t loadedBattVolts =
|
||||
wpi::sim::BatterySim::Calculate({totalCurrent});
|
||||
|
||||
// Using max(0.1, voltage) here isn't a *physically correct* solution,
|
||||
// but it avoids problems with battery voltage measuring 0.
|
||||
frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts));
|
||||
wpi::sim::RoboRioSim::SetVInVoltage(wpi::units::math::max(0.1_V, loadedBattVolts));
|
||||
}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() { return frc::StartRobot<Robot>(); }
|
||||
int main() { return wpi::StartRobot<Robot>(); }
|
||||
#endif
|
||||
|
||||
@@ -25,7 +25,7 @@
|
||||
#include "subsystems/GamepieceLauncher.h" // Include the header file
|
||||
|
||||
GamepieceLauncher::GamepieceLauncher() {
|
||||
motor = new frc::PWMSparkMax(8); // Create the motor object for PWM port 8
|
||||
motor = new wpi::PWMSparkMax(8); // Create the motor object for PWM port 8
|
||||
simulationInit();
|
||||
}
|
||||
|
||||
@@ -36,23 +36,23 @@ void GamepieceLauncher::setRunning(bool shouldRun) {
|
||||
void GamepieceLauncher::periodic() {
|
||||
// Calculate the maximum RPM
|
||||
double maxRPM =
|
||||
units::radians_per_second_t(frc::DCMotor::Falcon500(1).freeSpeed)
|
||||
wpi::units::radians_per_second_t(wpi::math::DCMotor::Falcon500(1).freeSpeed)
|
||||
.to<double>() *
|
||||
60 / (2 * std::numbers::pi);
|
||||
curMotorCmd = curDesSpd / maxRPM;
|
||||
curMotorCmd = std::clamp(curMotorCmd, 0.0, 1.0);
|
||||
motor->Set(curMotorCmd);
|
||||
|
||||
frc::SmartDashboard::PutNumber("GPLauncher Des Spd (RPM)", curDesSpd);
|
||||
wpi::SmartDashboard::PutNumber("GPLauncher Des Spd (RPM)", curDesSpd);
|
||||
}
|
||||
|
||||
void GamepieceLauncher::simulationPeriodic() {
|
||||
launcherSim.SetInputVoltage(curMotorCmd *
|
||||
frc::RobotController::GetBatteryVoltage());
|
||||
wpi::RobotController::GetBatteryVoltage());
|
||||
launcherSim.Update(0.02_s);
|
||||
auto spd = launcherSim.GetAngularVelocity().to<double>() * 60 /
|
||||
(2 * std::numbers::pi);
|
||||
frc::SmartDashboard::PutNumber("GPLauncher Act Spd (RPM)", spd);
|
||||
wpi::SmartDashboard::PutNumber("GPLauncher Act Spd (RPM)", spd);
|
||||
}
|
||||
|
||||
void GamepieceLauncher::simulationInit() {}
|
||||
|
||||
@@ -26,18 +26,19 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <wpi/opmode/TimedRobot.hpp>
|
||||
#include <wpi/smartdashboard/SmartDashboard.hpp>
|
||||
|
||||
SwerveDrive::SwerveDrive()
|
||||
: poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
|
||||
frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
|
||||
gyroSim(gyro),
|
||||
swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kDriveGearRatio,
|
||||
constants::Swerve::kWheelDiameter / 2,
|
||||
constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kSteerGearRatio, kinematics) {}
|
||||
swerveDriveSim(
|
||||
constants::Swerve::kDriveFF, wpi::math::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kDriveGearRatio,
|
||||
constants::Swerve::kWheelDiameter / 2, constants::Swerve::kSteerFF,
|
||||
wpi::math::DCMotor::Falcon500(1), constants::Swerve::kSteerGearRatio,
|
||||
kinematics) {}
|
||||
|
||||
void SwerveDrive::Periodic() {
|
||||
for (auto& currentModule : swerveMods) {
|
||||
@@ -47,27 +48,30 @@ void SwerveDrive::Periodic() {
|
||||
poseEstimator.Update(GetGyroYaw(), GetModulePositions());
|
||||
}
|
||||
|
||||
void SwerveDrive::Drive(units::meters_per_second_t vx,
|
||||
units::meters_per_second_t vy,
|
||||
units::radians_per_second_t omega) {
|
||||
frc::ChassisSpeeds newChassisSpeeds =
|
||||
frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading());
|
||||
void SwerveDrive::Drive(wpi::units::meters_per_second_t vx,
|
||||
wpi::units::meters_per_second_t vy,
|
||||
wpi::units::radians_per_second_t omega) {
|
||||
wpi::math::ChassisSpeeds newChassisSpeeds =
|
||||
wpi::math::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega,
|
||||
GetHeading());
|
||||
SetChassisSpeeds(newChassisSpeeds, true, false);
|
||||
}
|
||||
|
||||
void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds,
|
||||
bool openLoop, bool steerInPlace) {
|
||||
void SwerveDrive::SetChassisSpeeds(
|
||||
const wpi::math::ChassisSpeeds& newChassisSpeeds, bool openLoop,
|
||||
bool steerInPlace) {
|
||||
SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true,
|
||||
steerInPlace);
|
||||
this->targetChassisSpeeds = newChassisSpeeds;
|
||||
}
|
||||
|
||||
void SwerveDrive::SetModuleStates(
|
||||
const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace) {
|
||||
std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
|
||||
frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
|
||||
static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&desaturatedStates),
|
||||
const std::array<wpi::math::SwerveModuleState, 4>& desiredStates,
|
||||
bool openLoop, bool steerInPlace) {
|
||||
std::array<wpi::math::SwerveModuleState, 4> desaturatedStates = desiredStates;
|
||||
wpi::math::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
|
||||
static_cast<wpi::util::array<wpi::math::SwerveModuleState, 4>*>(
|
||||
&desaturatedStates),
|
||||
constants::Swerve::kMaxLinearSpeed);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
|
||||
@@ -76,19 +80,19 @@ void SwerveDrive::SetModuleStates(
|
||||
|
||||
void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); }
|
||||
|
||||
void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
|
||||
units::second_t timestamp) {
|
||||
void SwerveDrive::AddVisionMeasurement(
|
||||
const wpi::math::Pose2d& visionMeasurement, wpi::units::second_t timestamp) {
|
||||
poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp);
|
||||
}
|
||||
|
||||
void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
|
||||
units::second_t timestamp,
|
||||
const Eigen::Vector3d& stdDevs) {
|
||||
wpi::array<double, 3> newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)};
|
||||
void SwerveDrive::AddVisionMeasurement(
|
||||
const wpi::math::Pose2d& visionMeasurement, wpi::units::second_t timestamp,
|
||||
const Eigen::Vector3d& stdDevs) {
|
||||
wpi::util::array<double, 3> newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)};
|
||||
poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs);
|
||||
}
|
||||
|
||||
void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
|
||||
void SwerveDrive::ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose) {
|
||||
if (resetSimPose) {
|
||||
swerveDriveSim.Reset(pose, false);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
@@ -101,20 +105,25 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
|
||||
poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetPose() const {
|
||||
wpi::math::Pose2d SwerveDrive::GetPose() const {
|
||||
return poseEstimator.GetEstimatedPosition();
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
|
||||
wpi::math::Rotation2d SwerveDrive::GetHeading() const {
|
||||
return GetPose().Rotation();
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
|
||||
wpi::math::Rotation2d SwerveDrive::GetGyroYaw() const {
|
||||
return gyro.GetRotation2d();
|
||||
}
|
||||
|
||||
frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
|
||||
wpi::math::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
|
||||
return kinematics.ToChassisSpeeds(GetModuleStates());
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
std::array<frc::SwerveModuleState, 4> moduleStates;
|
||||
std::array<wpi::math::SwerveModuleState, 4> SwerveDrive::GetModuleStates()
|
||||
const {
|
||||
std::array<wpi::math::SwerveModuleState, 4> moduleStates;
|
||||
moduleStates[0] = swerveMods[0].GetState();
|
||||
moduleStates[1] = swerveMods[1].GetState();
|
||||
moduleStates[2] = swerveMods[2].GetState();
|
||||
@@ -122,9 +131,9 @@ std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
return moduleStates;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
std::array<wpi::math::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
const {
|
||||
std::array<frc::SwerveModulePosition, 4> modulePositions;
|
||||
std::array<wpi::math::SwerveModulePosition, 4> modulePositions;
|
||||
modulePositions[0] = swerveMods[0].GetPosition();
|
||||
modulePositions[1] = swerveMods[1].GetPosition();
|
||||
modulePositions[2] = swerveMods[2].GetPosition();
|
||||
@@ -132,11 +141,11 @@ std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
return modulePositions;
|
||||
}
|
||||
|
||||
std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<frc::Pose2d, 4> modulePoses;
|
||||
std::array<wpi::math::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<wpi::math::Pose2d, 4> modulePoses;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
const SwerveModule& module = swerveMods[i];
|
||||
modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
|
||||
modulePoses[i] = GetPose().TransformBy(wpi::math::Transform2d{
|
||||
module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
|
||||
}
|
||||
return modulePoses;
|
||||
@@ -144,24 +153,24 @@ std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
|
||||
void SwerveDrive::Log() {
|
||||
std::string table = "Drive/";
|
||||
frc::Pose2d pose = GetPose();
|
||||
frc::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Heading",
|
||||
wpi::math::Pose2d pose = GetPose();
|
||||
wpi::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Heading",
|
||||
pose.Rotation().Degrees().to<double>());
|
||||
frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
|
||||
frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::math::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
|
||||
wpi::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Omega Degrees",
|
||||
chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VX",
|
||||
chassisSpeeds.omega.convert<wpi::units::degrees_per_second>().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Target VX",
|
||||
targetChassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VY",
|
||||
wpi::SmartDashboard::PutNumber(table + "Target VY",
|
||||
targetChassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Target Omega Degrees",
|
||||
targetChassisSpeeds.omega.convert<units::degrees_per_second>()
|
||||
targetChassisSpeeds.omega.convert<wpi::units::degrees_per_second>()
|
||||
.to<double>());
|
||||
|
||||
for (auto& module : swerveMods) {
|
||||
@@ -170,8 +179,8 @@ void SwerveDrive::Log() {
|
||||
}
|
||||
|
||||
void SwerveDrive::SimulationPeriodic() {
|
||||
std::array<units::volt_t, 4> driveInputs;
|
||||
std::array<units::volt_t, 4> steerInputs;
|
||||
std::array<wpi::units::volt_t, 4> driveInputs;
|
||||
std::array<wpi::units::volt_t, 4> steerInputs;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
driveInputs[i] = swerveMods[i].GetDriveVoltage();
|
||||
steerInputs[i] = swerveMods[i].GetSteerVoltage();
|
||||
@@ -179,26 +188,26 @@ void SwerveDrive::SimulationPeriodic() {
|
||||
swerveDriveSim.SetDriveInputs(driveInputs);
|
||||
swerveDriveSim.SetSteerInputs(steerInputs);
|
||||
|
||||
swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
|
||||
swerveDriveSim.Update(wpi::TimedRobot::kDefaultPeriod);
|
||||
|
||||
auto driveStates = swerveDriveSim.GetDriveStates();
|
||||
auto steerStates = swerveDriveSim.GetSteerStates();
|
||||
totalCurrentDraw = 0_A;
|
||||
std::array<units::ampere_t, 4> driveCurrents =
|
||||
std::array<wpi::units::ampere_t, 4> driveCurrents =
|
||||
swerveDriveSim.GetDriveCurrentDraw();
|
||||
for (const auto& current : driveCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
std::array<units::ampere_t, 4> steerCurrents =
|
||||
std::array<wpi::units::ampere_t, 4> steerCurrents =
|
||||
swerveDriveSim.GetSteerCurrentDraw();
|
||||
for (const auto& current : steerCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
units::meter_t drivePos{driveStates[i](0, 0)};
|
||||
units::meters_per_second_t driveRate{driveStates[i](1, 0)};
|
||||
units::radian_t steerPos{steerStates[i](0, 0)};
|
||||
units::radians_per_second_t steerRate{steerStates[i](1, 0)};
|
||||
wpi::units::meter_t drivePos{driveStates[i](0, 0)};
|
||||
wpi::units::meters_per_second_t driveRate{driveStates[i](1, 0)};
|
||||
wpi::units::radian_t steerPos{steerStates[i](0, 0)};
|
||||
wpi::units::radians_per_second_t steerRate{steerStates[i](1, 0)};
|
||||
swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
|
||||
steerPos, steerRate, steerCurrents[i]);
|
||||
}
|
||||
@@ -206,6 +215,8 @@ void SwerveDrive::SimulationPeriodic() {
|
||||
gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
|
||||
wpi::math::Pose2d SwerveDrive::GetSimPose() const {
|
||||
return swerveDriveSim.GetPose();
|
||||
}
|
||||
|
||||
units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }
|
||||
wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }
|
||||
|
||||
@@ -24,8 +24,8 @@
|
||||
|
||||
#include "subsystems/SwerveDriveSim.h"
|
||||
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/system/Discretization.h>
|
||||
#include <wpi/math/system/Discretization.hpp>
|
||||
#include <wpi/system/RobotController.hpp>
|
||||
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
@@ -33,14 +33,14 @@ int sgn(T val) {
|
||||
}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::meters>& driveFF,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::radians>& steerFF,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: SwerveDriveSim(
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
wpi::math::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-driveFF.GetKv().to<double>() / driveFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
@@ -49,7 +49,7 @@ SwerveDriveSim::SwerveDriveSim(
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
|
||||
driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius,
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
wpi::math::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-steerFF.GetKv().to<double>() / steerFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
@@ -60,12 +60,12 @@ SwerveDriveSim::SwerveDriveSim(
|
||||
steerFF.GetKs(), steerMotor, steerGearing, kinematics) {}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: drivePlant(drivePlant),
|
||||
driveKs(driveKs),
|
||||
driveMotor(driveMotor),
|
||||
@@ -78,19 +78,19 @@ SwerveDriveSim::SwerveDriveSim(
|
||||
kinematics(kinematics) {}
|
||||
|
||||
void SwerveDriveSim::SetDriveInputs(
|
||||
const std::array<units::volt_t, numModules>& inputs) {
|
||||
units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
|
||||
const std::array<wpi::units::volt_t, numModules>& inputs) {
|
||||
wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage();
|
||||
for (int i = 0; i < driveInputs.size(); i++) {
|
||||
units::volt_t input = inputs[i];
|
||||
wpi::units::volt_t input = inputs[i];
|
||||
driveInputs[i] = std::clamp(input, -battVoltage, battVoltage);
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::SetSteerInputs(
|
||||
const std::array<units::volt_t, numModules>& inputs) {
|
||||
units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
|
||||
const std::array<wpi::units::volt_t, numModules>& inputs) {
|
||||
wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage();
|
||||
for (int i = 0; i < steerInputs.size(); i++) {
|
||||
units::volt_t input = inputs[i];
|
||||
wpi::units::volt_t input = inputs[i];
|
||||
steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
|
||||
}
|
||||
}
|
||||
@@ -98,8 +98,8 @@ void SwerveDriveSim::SetSteerInputs(
|
||||
Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
|
||||
units::volt_t kS) {
|
||||
const Eigen::Matrix<double, 2, 1>& x, wpi::units::volt_t input,
|
||||
wpi::units::volt_t kS) {
|
||||
auto Ax = discA * x;
|
||||
double nextStateVel = Ax(1, 0);
|
||||
double inputToStop = nextStateVel / -discB(1, 0);
|
||||
@@ -129,18 +129,18 @@ Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Update(units::second_t dt) {
|
||||
void SwerveDriveSim::Update(wpi::units::second_t dt) {
|
||||
Eigen::Matrix<double, 2, 2> driveDiscA;
|
||||
Eigen::Matrix<double, 2, 1> driveDiscB;
|
||||
frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
|
||||
&driveDiscB);
|
||||
wpi::math::DiscretizeABAB<2, 1>(drivePlant.A(), drivePlant.B(), dt,
|
||||
&driveDiscA, &driveDiscB);
|
||||
|
||||
Eigen::Matrix<double, 2, 2> steerDiscA;
|
||||
Eigen::Matrix<double, 2, 1> steerDiscB;
|
||||
frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
|
||||
&steerDiscB);
|
||||
wpi::math::DiscretizeABAB<2, 1>(steerPlant.A(), steerPlant.B(), dt,
|
||||
&steerDiscA, &steerDiscB);
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> moduleDeltas;
|
||||
std::array<wpi::math::SwerveModulePosition, 4> moduleDeltas;
|
||||
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
double prevDriveStatePos = driveStates[i](0, 0);
|
||||
@@ -150,17 +150,17 @@ void SwerveDriveSim::Update(units::second_t dt) {
|
||||
steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i],
|
||||
steerInputs[i], steerKs);
|
||||
double currentSteerStatePos = steerStates[i](0, 0);
|
||||
moduleDeltas[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{currentDriveStatePos - prevDriveStatePos},
|
||||
frc::Rotation2d{units::radian_t{currentSteerStatePos}}};
|
||||
moduleDeltas[i] = wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{currentDriveStatePos - prevDriveStatePos},
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{currentSteerStatePos}}};
|
||||
}
|
||||
|
||||
frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
|
||||
wpi::math::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
|
||||
pose = pose.Exp(twist);
|
||||
omega = twist.dtheta / dt;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
|
||||
void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose, bool preserveMotion) {
|
||||
this->pose = pose;
|
||||
if (!preserveMotion) {
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
@@ -171,7 +171,7 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
@@ -182,40 +182,40 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
wpi::math::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
std::array<wpi::math::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetModulePositions() const {
|
||||
std::array<frc::SwerveModulePosition, numModules> positions;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules> positions;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
positions[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{driveStates[i](0, 0)},
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
|
||||
positions[i] = wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{driveStates[i](0, 0)},
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
|
||||
units::radian_t steerStdDev) {
|
||||
std::array<frc::SwerveModulePosition, numModules> positions;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(wpi::units::meter_t driveStdDev,
|
||||
wpi::units::radian_t steerStdDev) {
|
||||
std::array<wpi::math::SwerveModulePosition, numModules> positions;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
positions[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{driveStates[i](0, 0)} +
|
||||
positions[i] = wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{driveStates[i](0, 0)} +
|
||||
randDist(generator) * driveStdDev,
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
|
||||
randDist(generator) * steerStdDev}};
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)} +
|
||||
randDist(generator) * steerStdDev}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, numModules>
|
||||
std::array<wpi::math::SwerveModuleState, numModules>
|
||||
SwerveDriveSim::GetModuleStates() {
|
||||
std::array<frc::SwerveModuleState, numModules> states;
|
||||
std::array<wpi::math::SwerveModuleState, numModules> states;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
states[i] = frc::SwerveModuleState{
|
||||
units::meters_per_second_t{driveStates[i](1, 0)},
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
|
||||
states[i] = wpi::math::SwerveModuleState{
|
||||
wpi::units::meters_per_second_t{driveStates[i](1, 0)},
|
||||
wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}};
|
||||
}
|
||||
return states;
|
||||
}
|
||||
@@ -230,12 +230,12 @@ SwerveDriveSim::GetSteerStates() const {
|
||||
return steerStates;
|
||||
}
|
||||
|
||||
units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
|
||||
wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
|
||||
|
||||
units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
const frc::DCMotor& motor, units::radians_per_second_t velocity,
|
||||
units::volt_t inputVolts, units::volt_t batteryVolts) const {
|
||||
units::volt_t effVolts = inputVolts - velocity / motor.Kv;
|
||||
wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
const wpi::math::DCMotor& motor, wpi::units::radians_per_second_t velocity,
|
||||
wpi::units::volt_t inputVolts, wpi::units::volt_t batteryVolts) const {
|
||||
wpi::units::volt_t effVolts = inputVolts - velocity / motor.Kv;
|
||||
if (inputVolts >= 0_V) {
|
||||
effVolts = std::clamp(effVolts, 0_V, inputVolts);
|
||||
} else {
|
||||
@@ -245,36 +245,36 @@ units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
return retVal;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
std::array<wpi::units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> currents;
|
||||
std::array<wpi::units::ampere_t, numModules> currents;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
units::radians_per_second_t speed =
|
||||
units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
|
||||
wpi::units::radians_per_second_t speed =
|
||||
wpi::units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
|
||||
driveWheelRadius.to<double>();
|
||||
currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
|
||||
frc::RobotController::GetBatteryVoltage());
|
||||
wpi::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
std::array<wpi::units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> currents;
|
||||
std::array<wpi::units::ampere_t, numModules> currents;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
units::radians_per_second_t speed =
|
||||
units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
|
||||
wpi::units::radians_per_second_t speed =
|
||||
wpi::units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
|
||||
// TODO: If uncommented we get huge current values.. Not sure how to fix
|
||||
// atm. :(
|
||||
currents[i] = 20_A;
|
||||
// currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i],
|
||||
// frc::RobotController::GetBatteryVoltage());
|
||||
// wpi::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
|
||||
units::ampere_t total{0};
|
||||
wpi::units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
|
||||
wpi::units::ampere_t total{0};
|
||||
for (const auto& val : GetDriveCurrentDraw()) {
|
||||
total += val;
|
||||
}
|
||||
|
||||
@@ -26,17 +26,17 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <frc/MathUtil.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <wpi/math/util/MathUtil.hpp>
|
||||
#include <wpi/smartdashboard/SmartDashboard.hpp>
|
||||
#include <wpi/system/RobotController.hpp>
|
||||
|
||||
SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
|
||||
: moduleConstants(consts),
|
||||
driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
|
||||
driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
|
||||
driveMotor(wpi::PWMSparkMax{moduleConstants.driveMotorId}),
|
||||
driveEncoder(wpi::Encoder{moduleConstants.driveEncoderA,
|
||||
moduleConstants.driveEncoderB}),
|
||||
steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
|
||||
steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
|
||||
steerMotor(wpi::PWMSparkMax{moduleConstants.steerMotorId}),
|
||||
steerEncoder(wpi::Encoder{moduleConstants.steerEncoderA,
|
||||
moduleConstants.steerEncoderB}),
|
||||
driveEncoderSim(driveEncoder),
|
||||
steerEncoderSim(steerEncoder) {
|
||||
@@ -48,55 +48,55 @@ SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
|
||||
}
|
||||
|
||||
void SwerveModule::Periodic() {
|
||||
units::volt_t steerPID = units::volt_t{
|
||||
wpi::units::volt_t steerPID = wpi::units::volt_t{
|
||||
steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
|
||||
desiredState.angle.Radians().to<double>())};
|
||||
steerMotor.SetVoltage(steerPID);
|
||||
|
||||
units::volt_t driveFF =
|
||||
wpi::units::volt_t driveFF =
|
||||
constants::Swerve::kDriveFF.Calculate(desiredState.speed);
|
||||
units::volt_t drivePID{0};
|
||||
wpi::units::volt_t drivePID{0};
|
||||
if (!openLoop) {
|
||||
drivePID = units::volt_t{drivePIDController.Calculate(
|
||||
drivePID = wpi::units::volt_t{drivePIDController.Calculate(
|
||||
driveEncoder.GetRate(), desiredState.speed.to<double>())};
|
||||
}
|
||||
driveMotor.SetVoltage(driveFF + drivePID);
|
||||
}
|
||||
|
||||
void SwerveModule::SetDesiredState(frc::SwerveModuleState newState,
|
||||
void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState newState,
|
||||
bool shouldBeOpenLoop, bool steerInPlace) {
|
||||
frc::Rotation2d currentRotation = GetAbsoluteHeading();
|
||||
wpi::math::Rotation2d currentRotation = GetAbsoluteHeading();
|
||||
newState.Optimize(currentRotation);
|
||||
desiredState = newState;
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
|
||||
return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
|
||||
wpi::math::Rotation2d SwerveModule::GetAbsoluteHeading() const {
|
||||
return wpi::math::Rotation2d{wpi::units::radian_t{steerEncoder.GetDistance()}};
|
||||
}
|
||||
|
||||
frc::SwerveModuleState SwerveModule::GetState() const {
|
||||
return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
|
||||
GetAbsoluteHeading()};
|
||||
wpi::math::SwerveModuleState SwerveModule::GetState() const {
|
||||
return wpi::math::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
frc::SwerveModulePosition SwerveModule::GetPosition() const {
|
||||
return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
|
||||
GetAbsoluteHeading()};
|
||||
wpi::math::SwerveModulePosition SwerveModule::GetPosition() const {
|
||||
return wpi::math::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetDriveVoltage() const {
|
||||
return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
wpi::units::volt_t SwerveModule::GetDriveVoltage() const {
|
||||
return driveMotor.Get() * wpi::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetSteerVoltage() const {
|
||||
return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
wpi::units::volt_t SwerveModule::GetSteerVoltage() const {
|
||||
return steerMotor.Get() * wpi::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetDriveCurrentSim() const {
|
||||
wpi::units::ampere_t SwerveModule::GetDriveCurrentSim() const {
|
||||
return driveCurrentSim;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetSteerCurrentSim() const {
|
||||
wpi::units::ampere_t SwerveModule::GetSteerCurrentSim() const {
|
||||
return steerCurrentSim;
|
||||
}
|
||||
|
||||
@@ -105,37 +105,37 @@ constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
|
||||
}
|
||||
|
||||
void SwerveModule::Log() {
|
||||
frc::SwerveModuleState state = GetState();
|
||||
wpi::math::SwerveModuleState state = GetState();
|
||||
|
||||
std::string table =
|
||||
"Module " + std::to_string(moduleConstants.moduleNum) + "/";
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Degrees",
|
||||
frc::AngleModulus(state.angle.Radians())
|
||||
.convert<units::degrees>()
|
||||
wpi::SmartDashboard::PutNumber(table + "Steer Degrees",
|
||||
wpi::math::AngleModulus(state.angle.Radians())
|
||||
.convert<wpi::units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Steer Target Degrees",
|
||||
units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<units::degrees>()
|
||||
wpi::units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<wpi::units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Feet",
|
||||
state.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
state.speed.convert<wpi::units::feet_per_second>().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Target Feet",
|
||||
desiredState.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
desiredState.speed.convert<wpi::units::feet_per_second>().to<double>());
|
||||
wpi::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
driveCurrentSim.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Current",
|
||||
wpi::SmartDashboard::PutNumber(table + "Steer Current",
|
||||
steerCurrentSim.to<double>());
|
||||
}
|
||||
|
||||
void SwerveModule::SimulationUpdate(
|
||||
units::meter_t driveEncoderDist,
|
||||
units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent,
|
||||
units::radian_t steerEncoderDist,
|
||||
units::radians_per_second_t steerEncoderRate,
|
||||
units::ampere_t steerCurrent) {
|
||||
wpi::units::meter_t driveEncoderDist,
|
||||
wpi::units::meters_per_second_t driveEncoderRate, wpi::units::ampere_t driveCurrent,
|
||||
wpi::units::radian_t steerEncoderDist,
|
||||
wpi::units::radians_per_second_t steerEncoderRate,
|
||||
wpi::units::ampere_t steerCurrent) {
|
||||
driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
|
||||
driveEncoderSim.SetRate(driveEncoderRate.to<double>());
|
||||
driveCurrentSim = driveCurrent;
|
||||
|
||||
@@ -26,25 +26,26 @@
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/geometry/Transform3d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angular_acceleration.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/length.h>
|
||||
#include <units/velocity.h>
|
||||
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
|
||||
#include <wpi/apriltag/AprilTagFields.hpp>
|
||||
#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
|
||||
#include <wpi/math/geometry/Transform3d.hpp>
|
||||
#include <wpi/math/geometry/Translation2d.hpp>
|
||||
#include <wpi/units/acceleration.hpp>
|
||||
#include <wpi/units/angular_acceleration.hpp>
|
||||
#include <wpi/units/angular_velocity.hpp>
|
||||
#include <wpi/units/length.hpp>
|
||||
#include <wpi/units/velocity.hpp>
|
||||
|
||||
namespace constants {
|
||||
namespace Vision {
|
||||
inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
|
||||
inline const frc::Transform3d kRobotToCam{
|
||||
frc::Translation3d{0.5_m, 0.0_m, 0.5_m},
|
||||
frc::Rotation3d{0_rad, -30_deg, 0_rad}};
|
||||
inline const frc::AprilTagFieldLayout kTagLayout{
|
||||
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::kDefaultField)};
|
||||
inline const wpi::math::Transform3d kRobotToCam{
|
||||
wpi::math::Translation3d{0.5_m, 0.0_m, 0.5_m},
|
||||
wpi::math::Rotation3d{0_rad, -30_deg, 0_rad}};
|
||||
inline const wpi::apriltag::AprilTagFieldLayout kTagLayout{
|
||||
wpi::apriltag::AprilTagFieldLayout::LoadField(
|
||||
wpi::apriltag::AprilTagField::kDefaultField)};
|
||||
|
||||
inline const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
|
||||
inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
|
||||
@@ -52,23 +53,23 @@ inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
|
||||
namespace Swerve {
|
||||
using namespace units;
|
||||
|
||||
inline constexpr units::meter_t kTrackWidth{18.5_in};
|
||||
inline constexpr units::meter_t kTrackLength{18.5_in};
|
||||
inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
|
||||
inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
|
||||
inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
|
||||
inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
|
||||
inline constexpr units::meter_t kWheelDiameter{4_in};
|
||||
inline constexpr units::meter_t kWheelCircumference{kWheelDiameter *
|
||||
inline constexpr wpi::units::meter_t kTrackWidth{18.5_in};
|
||||
inline constexpr wpi::units::meter_t kTrackLength{18.5_in};
|
||||
inline constexpr wpi::units::meter_t kRobotWidth{25_in + 3.25_in * 2};
|
||||
inline constexpr wpi::units::meter_t kRobotLength{25_in + 3.25_in * 2};
|
||||
inline constexpr wpi::units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
|
||||
inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
|
||||
inline constexpr wpi::units::meter_t kWheelDiameter{4_in};
|
||||
inline constexpr wpi::units::meter_t kWheelCircumference{kWheelDiameter *
|
||||
std::numbers::pi};
|
||||
|
||||
inline constexpr double kDriveGearRatio = 6.75;
|
||||
inline constexpr double kSteerGearRatio = 12.8;
|
||||
|
||||
inline constexpr units::meter_t kDriveDistPerPulse =
|
||||
inline constexpr wpi::units::meter_t kDriveDistPerPulse =
|
||||
kWheelCircumference / 1024.0 / kDriveGearRatio;
|
||||
inline constexpr units::radian_t kSteerRadPerPulse =
|
||||
units::radian_t{2 * std::numbers::pi} / 1024.0;
|
||||
inline constexpr wpi::units::radian_t kSteerRadPerPulse =
|
||||
wpi::units::radian_t{2 * std::numbers::pi} / 1024.0;
|
||||
|
||||
inline constexpr double kDriveKP = 1.0;
|
||||
inline constexpr double kDriveKI = 0.0;
|
||||
@@ -80,10 +81,10 @@ inline constexpr double kSteerKD = 0.25;
|
||||
|
||||
using namespace units;
|
||||
|
||||
inline const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
|
||||
inline const wpi::math::SimpleMotorFeedforward<wpi::units::meters> kDriveFF{
|
||||
0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
|
||||
|
||||
inline const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
|
||||
inline const wpi::math::SimpleMotorFeedforward<wpi::units::radians> kSteerFF{
|
||||
0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
|
||||
|
||||
struct ModuleConstants {
|
||||
@@ -96,12 +97,12 @@ struct ModuleConstants {
|
||||
const int steerEncoderA;
|
||||
const int steerEncoderB;
|
||||
const double angleOffset;
|
||||
const frc::Translation2d centerOffset;
|
||||
const wpi::math::Translation2d centerOffset;
|
||||
|
||||
ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA,
|
||||
int driveEncoderB, int steerMotorId, int steerEncoderA,
|
||||
int steerEncoderB, double angleOffset, units::meter_t xOffset,
|
||||
units::meter_t yOffset)
|
||||
int steerEncoderB, double angleOffset, wpi::units::meter_t xOffset,
|
||||
wpi::units::meter_t yOffset)
|
||||
: moduleNum(moduleNum),
|
||||
driveMotorId(driveMotorId),
|
||||
driveEncoderA(driveEncoderA),
|
||||
@@ -110,7 +111,7 @@ struct ModuleConstants {
|
||||
steerEncoderA(steerEncoderA),
|
||||
steerEncoderB(steerEncoderB),
|
||||
angleOffset(angleOffset),
|
||||
centerOffset(frc::Translation2d{xOffset, yOffset}) {}
|
||||
centerOffset(wpi::math::Translation2d{xOffset, yOffset}) {}
|
||||
};
|
||||
|
||||
inline const ModuleConstants FL_CONSTANTS{
|
||||
|
||||
@@ -24,14 +24,14 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <wpi/driverstation/XboxController.hpp>
|
||||
#include <wpi/opmode/TimedRobot.hpp>
|
||||
|
||||
#include "Vision.h"
|
||||
#include "subsystems/GamepieceLauncher.h"
|
||||
#include "subsystems/SwerveDrive.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
class Robot : public wpi::TimedRobot {
|
||||
public:
|
||||
void RobotInit() override;
|
||||
void RobotPeriodic() override;
|
||||
@@ -51,10 +51,10 @@ class Robot : public frc::TimedRobot {
|
||||
|
||||
private:
|
||||
SwerveDrive drivetrain{};
|
||||
Vision vision{[=, this](frc::Pose2d pose, units::second_t timestamp,
|
||||
Vision vision{[=, this](wpi::math::Pose2d pose, wpi::units::second_t timestamp,
|
||||
Eigen::Matrix<double, 3, 1> stddevs) {
|
||||
drivetrain.AddVisionMeasurement(pose, timestamp, stddevs);
|
||||
}};
|
||||
GamepieceLauncher launcher{};
|
||||
frc::XboxController controller{0};
|
||||
wpi::XboxController controller{0};
|
||||
};
|
||||
|
||||
@@ -28,14 +28,14 @@
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <photon/PhotonCamera.h>
|
||||
#include <photon/PhotonPoseEstimator.h>
|
||||
#include <photon/estimation/VisionEstimation.h>
|
||||
#include <photon/simulation/VisionSystemSim.h>
|
||||
#include <photon/simulation/VisionTargetSim.h>
|
||||
#include <photon/targeting/PhotonPipelineResult.h>
|
||||
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
|
||||
#include <wpi/apriltag/AprilTagFields.hpp>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
@@ -45,18 +45,18 @@ class Vision {
|
||||
* @param estConsumer Lamba that will accept a pose estimate and pass it to
|
||||
* your desired SwerveDrivePoseEstimator.
|
||||
*/
|
||||
Vision(std::function<void(frc::Pose2d, units::second_t,
|
||||
Vision(std::function<void(wpi::math::Pose2d, wpi::units::second_t,
|
||||
Eigen::Matrix<double, 3, 1>)>
|
||||
estConsumer)
|
||||
: estConsumer{estConsumer} {
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
if (wpi::RobotBase::IsSimulation()) {
|
||||
visionSim = std::make_unique<photon::VisionSystemSim>("main");
|
||||
|
||||
visionSim->AddAprilTags(constants::Vision::kTagLayout);
|
||||
|
||||
cameraProp = std::make_unique<photon::SimCameraProperties>();
|
||||
|
||||
cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg});
|
||||
cameraProp->SetCalibration(960, 720, wpi::math::Rotation2d{90_deg});
|
||||
cameraProp->SetCalibError(.35, .10);
|
||||
cameraProp->SetFPS(15_Hz);
|
||||
cameraProp->SetAvgLatency(50_ms);
|
||||
@@ -83,7 +83,7 @@ class Vision {
|
||||
m_latestResult = result;
|
||||
|
||||
// In sim only, add our vision estimate to the sim debug field
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
if (wpi::RobotBase::IsSimulation()) {
|
||||
if (visionEst) {
|
||||
GetSimDebugField()
|
||||
.GetObject("VisionEstimation")
|
||||
@@ -100,12 +100,13 @@ class Vision {
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, 3, 1> GetEstimationStdDevs(frc::Pose2d estimatedPose) {
|
||||
Eigen::Matrix<double, 3, 1> GetEstimationStdDevs(
|
||||
wpi::math::Pose2d estimatedPose) {
|
||||
Eigen::Matrix<double, 3, 1> estStdDevs =
|
||||
constants::Vision::kSingleTagStdDevs;
|
||||
auto targets = GetLatestResult().GetTargets();
|
||||
int numTags = 0;
|
||||
units::meter_t avgDist = 0_m;
|
||||
wpi::units::meter_t avgDist = 0_m;
|
||||
for (const auto& tgt : targets) {
|
||||
auto tagPose =
|
||||
photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId());
|
||||
@@ -133,17 +134,17 @@ class Vision {
|
||||
return estStdDevs;
|
||||
}
|
||||
|
||||
void SimPeriodic(frc::Pose2d robotSimPose) {
|
||||
void SimPeriodic(wpi::math::Pose2d robotSimPose) {
|
||||
visionSim->Update(robotSimPose);
|
||||
}
|
||||
|
||||
void ResetSimPose(frc::Pose2d pose) {
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
void ResetSimPose(wpi::math::Pose2d pose) {
|
||||
if (wpi::RobotBase::IsSimulation()) {
|
||||
visionSim->ResetRobotPose(pose);
|
||||
}
|
||||
}
|
||||
|
||||
frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
|
||||
wpi::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
|
||||
|
||||
private:
|
||||
photon::PhotonPoseEstimator photonEstimator{constants::Vision::kTagLayout,
|
||||
@@ -155,6 +156,7 @@ class Vision {
|
||||
|
||||
// The most recent result, cached for calculating std devs
|
||||
photon::PhotonPipelineResult m_latestResult;
|
||||
std::function<void(frc::Pose2d, units::second_t, Eigen::Matrix<double, 3, 1>)>
|
||||
std::function<void(wpi::math::Pose2d, wpi::units::second_t,
|
||||
Eigen::Matrix<double, 3, 1>)>
|
||||
estConsumer;
|
||||
};
|
||||
|
||||
@@ -27,14 +27,14 @@
|
||||
#include <cmath>
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc/simulation/FlywheelSim.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc/system/plant/DCMotor.h>
|
||||
#include <frc/system/plant/LinearSystemId.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/moment_of_inertia.h>
|
||||
#include <wpi/hardware/motor/PWMSparkMax.hpp>
|
||||
#include <wpi/math/system/plant/DCMotor.hpp>
|
||||
#include <wpi/math/system/plant/LinearSystemId.hpp>
|
||||
#include <wpi/simulation/FlywheelSim.hpp>
|
||||
#include <wpi/smartdashboard/SmartDashboard.hpp>
|
||||
#include <wpi/system/RobotController.hpp>
|
||||
#include <wpi/units/angle.hpp>
|
||||
#include <wpi/units/moment_of_inertia.hpp>
|
||||
|
||||
class GamepieceLauncher {
|
||||
public:
|
||||
@@ -44,20 +44,21 @@ class GamepieceLauncher {
|
||||
void simulationPeriodic(); // Method to handle simulation updates
|
||||
|
||||
private:
|
||||
frc::PWMSparkMax* motor; // Motor controller
|
||||
wpi::PWMSparkMax* motor; // Motor controller
|
||||
|
||||
const double LAUNCH_SPEED_RPM = 2500;
|
||||
double curDesSpd = 0.0;
|
||||
double curMotorCmd = 0.0;
|
||||
|
||||
static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia =
|
||||
static constexpr wpi::units::kilogram_square_meter_t kFlywheelMomentOfInertia =
|
||||
0.5 * 1.5_lb * 4_in * 4_in;
|
||||
|
||||
frc::DCMotor m_gearbox = frc::DCMotor::Falcon500(1);
|
||||
frc::LinearSystem<1, 1, 1> m_plant{frc::LinearSystemId::FlywheelSystem(
|
||||
m_gearbox, kFlywheelMomentOfInertia, 1.0)};
|
||||
wpi::math::DCMotor m_gearbox = wpi::math::DCMotor::Falcon500(1);
|
||||
wpi::math::LinearSystem<1, 1, 1> m_plant{
|
||||
wpi::math::LinearSystemId::FlywheelSystem(m_gearbox,
|
||||
kFlywheelMomentOfInertia, 1.0)};
|
||||
|
||||
frc::sim::FlywheelSim launcherSim{m_plant, m_gearbox};
|
||||
wpi::sim::FlywheelSim launcherSim{m_plant, m_gearbox};
|
||||
|
||||
void simulationInit(); // Method to initialize simulation components
|
||||
};
|
||||
|
||||
@@ -26,9 +26,9 @@
|
||||
|
||||
#include <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/SPI.h>
|
||||
#include <frc/estimator/SwerveDrivePoseEstimator.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/simulation/ADXRS450_GyroSim.h>
|
||||
#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
||||
|
||||
#include "SwerveDriveSim.h"
|
||||
#include "SwerveModule.h"
|
||||
@@ -37,31 +37,31 @@ class SwerveDrive {
|
||||
public:
|
||||
SwerveDrive();
|
||||
void Periodic();
|
||||
void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy,
|
||||
units::radians_per_second_t omega);
|
||||
void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds,
|
||||
void Drive(wpi::units::meters_per_second_t vx, wpi::units::meters_per_second_t vy,
|
||||
wpi::units::radians_per_second_t omega);
|
||||
void SetChassisSpeeds(const wpi::math::ChassisSpeeds& targetChassisSpeeds,
|
||||
bool openLoop, bool steerInPlace);
|
||||
void SetModuleStates(
|
||||
const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace);
|
||||
const std::array<wpi::math::SwerveModuleState, 4>& desiredStates,
|
||||
bool openLoop, bool steerInPlace);
|
||||
void Stop();
|
||||
void AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
|
||||
units::second_t timestamp);
|
||||
void AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
|
||||
units::second_t timestamp,
|
||||
void AddVisionMeasurement(const wpi::math::Pose2d& visionMeasurement,
|
||||
wpi::units::second_t timestamp);
|
||||
void AddVisionMeasurement(const wpi::math::Pose2d& visionMeasurement,
|
||||
wpi::units::second_t timestamp,
|
||||
const Eigen::Vector3d& stdDevs);
|
||||
void ResetPose(const frc::Pose2d& pose, bool resetSimPose);
|
||||
frc::Pose2d GetPose() const;
|
||||
frc::Rotation2d GetHeading() const;
|
||||
frc::Rotation2d GetGyroYaw() const;
|
||||
frc::ChassisSpeeds GetChassisSpeeds() const;
|
||||
std::array<frc::SwerveModuleState, 4> GetModuleStates() const;
|
||||
std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
|
||||
std::array<frc::Pose2d, 4> GetModulePoses() const;
|
||||
void ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose);
|
||||
wpi::math::Pose2d GetPose() const;
|
||||
wpi::math::Rotation2d GetHeading() const;
|
||||
wpi::math::Rotation2d GetGyroYaw() const;
|
||||
wpi::math::ChassisSpeeds GetChassisSpeeds() const;
|
||||
std::array<wpi::math::SwerveModuleState, 4> GetModuleStates() const;
|
||||
std::array<wpi::math::SwerveModulePosition, 4> GetModulePositions() const;
|
||||
std::array<wpi::math::Pose2d, 4> GetModulePoses() const;
|
||||
void Log();
|
||||
void SimulationPeriodic();
|
||||
frc::Pose2d GetSimPose() const;
|
||||
units::ampere_t GetCurrentDraw() const;
|
||||
wpi::math::Pose2d GetSimPose() const;
|
||||
wpi::units::ampere_t GetCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::array<SwerveModule, 4> swerveMods{
|
||||
@@ -69,17 +69,17 @@ class SwerveDrive {
|
||||
SwerveModule{constants::Swerve::FR_CONSTANTS},
|
||||
SwerveModule{constants::Swerve::BL_CONSTANTS},
|
||||
SwerveModule{constants::Swerve::BR_CONSTANTS}};
|
||||
frc::SwerveDriveKinematics<4> kinematics{
|
||||
wpi::math::SwerveDriveKinematics<4> kinematics{
|
||||
swerveMods[0].GetModuleConstants().centerOffset,
|
||||
swerveMods[1].GetModuleConstants().centerOffset,
|
||||
swerveMods[2].GetModuleConstants().centerOffset,
|
||||
swerveMods[3].GetModuleConstants().centerOffset,
|
||||
};
|
||||
frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
|
||||
frc::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||
frc::ChassisSpeeds targetChassisSpeeds{};
|
||||
wpi::ADXRS450_Gyro gyro{wpi::SPI::Port::kOnboardCS0};
|
||||
wpi::math::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||
wpi::math::ChassisSpeeds targetChassisSpeeds{};
|
||||
|
||||
frc::sim::ADXRS450_GyroSim gyroSim;
|
||||
wpi::sim::ADXRS450_GyroSim gyroSim;
|
||||
SwerveDriveSim swerveDriveSim;
|
||||
units::ampere_t totalCurrentDraw{0};
|
||||
wpi::units::ampere_t totalCurrentDraw{0};
|
||||
};
|
||||
|
||||
@@ -26,77 +26,80 @@
|
||||
|
||||
#include <random>
|
||||
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/system/LinearSystem.h>
|
||||
#include <frc/system/plant/DCMotor.h>
|
||||
#include <units/voltage.h>
|
||||
#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
||||
#include <wpi/math/system/LinearSystem.hpp>
|
||||
#include <wpi/math/system/plant/DCMotor.hpp>
|
||||
#include <wpi/units/voltage.hpp>
|
||||
|
||||
static constexpr int numModules{4};
|
||||
|
||||
class SwerveDriveSim {
|
||||
public:
|
||||
SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics);
|
||||
SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant,
|
||||
units::volt_t driveKs, const frc::DCMotor& driveMotor,
|
||||
double driveGearing, units::meter_t driveWheelRadius,
|
||||
const frc::LinearSystem<2, 1, 2>& steerPlant,
|
||||
units::volt_t steerKs, const frc::DCMotor& steerMotor,
|
||||
double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics);
|
||||
void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
SwerveDriveSim(
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::meters>& driveFF,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::SimpleMotorFeedforward<wpi::units::radians>& steerFF,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics);
|
||||
SwerveDriveSim(
|
||||
const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs,
|
||||
const wpi::math::DCMotor& driveMotor, double driveGearing,
|
||||
wpi::units::meter_t driveWheelRadius,
|
||||
const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs,
|
||||
const wpi::math::DCMotor& steerMotor, double steerGearing,
|
||||
const wpi::math::SwerveDriveKinematics<numModules>& kinematics);
|
||||
void SetDriveInputs(const std::array<wpi::units::volt_t, numModules>& inputs);
|
||||
void SetSteerInputs(const std::array<wpi::units::volt_t, numModules>& inputs);
|
||||
static Eigen::Matrix<double, 2, 1> CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
|
||||
units::volt_t kS);
|
||||
void Update(units::second_t dt);
|
||||
void Reset(const frc::Pose2d& pose, bool preserveMotion);
|
||||
void Reset(const frc::Pose2d& pose,
|
||||
const Eigen::Matrix<double, 2, 1>& x, wpi::units::volt_t input,
|
||||
wpi::units::volt_t kS);
|
||||
void Update(wpi::units::second_t dt);
|
||||
void Reset(const wpi::math::Pose2d& pose, bool preserveMotion);
|
||||
void Reset(const wpi::math::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleSteerStates);
|
||||
frc::Pose2d GetPose() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
|
||||
units::meter_t driveStdDev, units::radian_t steerStdDev);
|
||||
std::array<frc::SwerveModuleState, numModules> GetModuleStates();
|
||||
wpi::math::Pose2d GetPose() const;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules> GetModulePositions()
|
||||
const;
|
||||
std::array<wpi::math::SwerveModulePosition, numModules>
|
||||
GetNoisyModulePositions(wpi::units::meter_t driveStdDev,
|
||||
wpi::units::radian_t steerStdDev);
|
||||
std::array<wpi::math::SwerveModuleState, numModules> GetModuleStates();
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetSteerStates() const;
|
||||
units::radians_per_second_t GetOmega() const;
|
||||
units::ampere_t GetCurrentDraw(const frc::DCMotor& motor,
|
||||
units::radians_per_second_t velocity,
|
||||
units::volt_t inputVolts,
|
||||
units::volt_t batteryVolts) const;
|
||||
std::array<units::ampere_t, numModules> GetDriveCurrentDraw() const;
|
||||
std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
|
||||
units::ampere_t GetTotalCurrentDraw() const;
|
||||
wpi::units::radians_per_second_t GetOmega() const;
|
||||
wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor,
|
||||
wpi::units::radians_per_second_t velocity,
|
||||
wpi::units::volt_t inputVolts,
|
||||
wpi::units::volt_t batteryVolts) const;
|
||||
std::array<wpi::units::ampere_t, numModules> GetDriveCurrentDraw() const;
|
||||
std::array<wpi::units::ampere_t, numModules> GetSteerCurrentDraw() const;
|
||||
wpi::units::ampere_t GetTotalCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::random_device rd{};
|
||||
std::mt19937 generator{rd()};
|
||||
std::normal_distribution<double> randDist{0.0, 1.0};
|
||||
const frc::LinearSystem<2, 1, 2> drivePlant;
|
||||
const units::volt_t driveKs;
|
||||
const frc::DCMotor driveMotor;
|
||||
const wpi::math::LinearSystem<2, 1, 2> drivePlant;
|
||||
const wpi::units::volt_t driveKs;
|
||||
const wpi::math::DCMotor driveMotor;
|
||||
const double driveGearing;
|
||||
const units::meter_t driveWheelRadius;
|
||||
const frc::LinearSystem<2, 1, 2> steerPlant;
|
||||
const units::volt_t steerKs;
|
||||
const frc::DCMotor steerMotor;
|
||||
const wpi::units::meter_t driveWheelRadius;
|
||||
const wpi::math::LinearSystem<2, 1, 2> steerPlant;
|
||||
const wpi::units::volt_t steerKs;
|
||||
const wpi::math::DCMotor steerMotor;
|
||||
const double steerGearing;
|
||||
const frc::SwerveDriveKinematics<numModules> kinematics;
|
||||
std::array<units::volt_t, numModules> driveInputs{};
|
||||
const wpi::math::SwerveDriveKinematics<numModules> kinematics;
|
||||
std::array<wpi::units::volt_t, numModules> driveInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
|
||||
std::array<units::volt_t, numModules> steerInputs{};
|
||||
std::array<wpi::units::volt_t, numModules> steerInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
|
||||
frc::Pose2d pose{frc::Pose2d{}};
|
||||
units::radians_per_second_t omega{0};
|
||||
wpi::math::Pose2d pose{wpi::math::Pose2d{}};
|
||||
wpi::units::radians_per_second_t omega{0};
|
||||
};
|
||||
|
||||
@@ -24,13 +24,13 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/kinematics/SwerveModulePosition.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc/simulation/EncoderSim.h>
|
||||
#include <units/current.h>
|
||||
#include <wpi/hardware/motor/PWMSparkMax.hpp>
|
||||
#include <wpi/hardware/rotation/Encoder.hpp>
|
||||
#include <wpi/math/controller/PIDController.hpp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveModulePosition.hpppp>
|
||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveModuleState.hpppp>
|
||||
#include <wpi/simulation/EncoderSim.hpp>
|
||||
#include <wpi/units/current.hpp>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
@@ -38,44 +38,44 @@ class SwerveModule {
|
||||
public:
|
||||
explicit SwerveModule(const constants::Swerve::ModuleConstants& consts);
|
||||
void Periodic();
|
||||
void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop,
|
||||
bool steerInPlace);
|
||||
frc::Rotation2d GetAbsoluteHeading() const;
|
||||
frc::SwerveModuleState GetState() const;
|
||||
frc::SwerveModulePosition GetPosition() const;
|
||||
units::volt_t GetDriveVoltage() const;
|
||||
units::volt_t GetSteerVoltage() const;
|
||||
units::ampere_t GetDriveCurrentSim() const;
|
||||
units::ampere_t GetSteerCurrentSim() const;
|
||||
void SetDesiredState(wpi::math::SwerveModuleState newState,
|
||||
bool shouldBeOpenLoop, bool steerInPlace);
|
||||
wpi::math::Rotation2d GetAbsoluteHeading() const;
|
||||
wpi::math::SwerveModuleState GetState() const;
|
||||
wpi::math::SwerveModulePosition GetPosition() const;
|
||||
wpi::units::volt_t GetDriveVoltage() const;
|
||||
wpi::units::volt_t GetSteerVoltage() const;
|
||||
wpi::units::ampere_t GetDriveCurrentSim() const;
|
||||
wpi::units::ampere_t GetSteerCurrentSim() const;
|
||||
constants::Swerve::ModuleConstants GetModuleConstants() const;
|
||||
void Log();
|
||||
void SimulationUpdate(units::meter_t driveEncoderDist,
|
||||
units::meters_per_second_t driveEncoderRate,
|
||||
units::ampere_t driveCurrent,
|
||||
units::radian_t steerEncoderDist,
|
||||
units::radians_per_second_t steerEncoderRate,
|
||||
units::ampere_t steerCurrent);
|
||||
void SimulationUpdate(wpi::units::meter_t driveEncoderDist,
|
||||
wpi::units::meters_per_second_t driveEncoderRate,
|
||||
wpi::units::ampere_t driveCurrent,
|
||||
wpi::units::radian_t steerEncoderDist,
|
||||
wpi::units::radians_per_second_t steerEncoderRate,
|
||||
wpi::units::ampere_t steerCurrent);
|
||||
|
||||
private:
|
||||
const constants::Swerve::ModuleConstants moduleConstants;
|
||||
|
||||
frc::PWMSparkMax driveMotor;
|
||||
frc::Encoder driveEncoder;
|
||||
frc::PWMSparkMax steerMotor;
|
||||
frc::Encoder steerEncoder;
|
||||
wpi::PWMSparkMax driveMotor;
|
||||
wpi::Encoder driveEncoder;
|
||||
wpi::PWMSparkMax steerMotor;
|
||||
wpi::Encoder steerEncoder;
|
||||
|
||||
frc::SwerveModuleState desiredState{};
|
||||
wpi::math::SwerveModuleState desiredState{};
|
||||
bool openLoop{false};
|
||||
|
||||
frc::PIDController drivePIDController{constants::Swerve::kDriveKP,
|
||||
constants::Swerve::kDriveKI,
|
||||
constants::Swerve::kDriveKD};
|
||||
frc::PIDController steerPIDController{constants::Swerve::kSteerKP,
|
||||
constants::Swerve::kSteerKI,
|
||||
constants::Swerve::kSteerKD};
|
||||
wpi::math::PIDController drivePIDController{constants::Swerve::kDriveKP,
|
||||
constants::Swerve::kDriveKI,
|
||||
constants::Swerve::kDriveKD};
|
||||
wpi::math::PIDController steerPIDController{constants::Swerve::kSteerKP,
|
||||
constants::Swerve::kSteerKI,
|
||||
constants::Swerve::kSteerKD};
|
||||
|
||||
frc::sim::EncoderSim driveEncoderSim;
|
||||
units::ampere_t driveCurrentSim{0};
|
||||
frc::sim::EncoderSim steerEncoderSim;
|
||||
units::ampere_t steerCurrentSim{0};
|
||||
wpi::sim::EncoderSim driveEncoderSim;
|
||||
wpi::units::ampere_t driveCurrentSim{0};
|
||||
wpi::sim::EncoderSim steerEncoderSim;
|
||||
wpi::units::ampere_t steerCurrentSim{0};
|
||||
};
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <hal/HAL.h>
|
||||
#include <wpi/hal/HAL.h>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
|
||||
Reference in New Issue
Block a user