mirror of
https://github.com/Astatin3/photonvision-2025.0.0-beta-6.git
synced 2026-06-09 08:38:00 -06:00
Initial commit
This commit is contained in:
astatin3
@@ -0,0 +1,200 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "subsystems/SwerveDrive.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
SwerveDrive::SwerveDrive()
|
||||
: poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
|
||||
frc::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) {}
|
||||
|
||||
void SwerveDrive::Periodic() {
|
||||
for (auto& currentModule : swerveMods) {
|
||||
currentModule.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());
|
||||
SetChassisSpeeds(newChassisSpeeds, true, false);
|
||||
}
|
||||
|
||||
void SwerveDrive::SetChassisSpeeds(const frc::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),
|
||||
constants::Swerve::kMaxLinearSpeed);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); }
|
||||
|
||||
void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
|
||||
if (resetSimPose) {
|
||||
swerveDriveSim.Reset(pose, false);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A);
|
||||
}
|
||||
gyroSim.SetAngle(-pose.Rotation().Degrees());
|
||||
gyroSim.SetRate(0_rad_per_s);
|
||||
}
|
||||
|
||||
poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetPose() const {
|
||||
return poseEstimator.GetEstimatedPosition();
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
|
||||
|
||||
frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
|
||||
return kinematics.ToChassisSpeeds(GetModuleStates());
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
std::array<frc::SwerveModuleState, 4> moduleStates;
|
||||
moduleStates[0] = swerveMods[0].GetState();
|
||||
moduleStates[1] = swerveMods[1].GetState();
|
||||
moduleStates[2] = swerveMods[2].GetState();
|
||||
moduleStates[3] = swerveMods[3].GetState();
|
||||
return moduleStates;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
const {
|
||||
std::array<frc::SwerveModulePosition, 4> modulePositions;
|
||||
modulePositions[0] = swerveMods[0].GetPosition();
|
||||
modulePositions[1] = swerveMods[1].GetPosition();
|
||||
modulePositions[2] = swerveMods[2].GetPosition();
|
||||
modulePositions[3] = swerveMods[3].GetPosition();
|
||||
return modulePositions;
|
||||
}
|
||||
|
||||
std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<frc::Pose2d, 4> modulePoses;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
const SwerveModule& module = swerveMods[i];
|
||||
modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
|
||||
module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
|
||||
}
|
||||
return modulePoses;
|
||||
}
|
||||
|
||||
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",
|
||||
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(
|
||||
table + "Omega Degrees",
|
||||
chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VX",
|
||||
targetChassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VY",
|
||||
targetChassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Target Omega Degrees",
|
||||
targetChassisSpeeds.omega.convert<units::degrees_per_second>()
|
||||
.to<double>());
|
||||
|
||||
for (auto& module : swerveMods) {
|
||||
module.Log();
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDrive::SimulationPeriodic() {
|
||||
std::array<units::volt_t, 4> driveInputs;
|
||||
std::array<units::volt_t, 4> steerInputs;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
driveInputs[i] = swerveMods[i].GetDriveVoltage();
|
||||
steerInputs[i] = swerveMods[i].GetSteerVoltage();
|
||||
}
|
||||
swerveDriveSim.SetDriveInputs(driveInputs);
|
||||
swerveDriveSim.SetSteerInputs(steerInputs);
|
||||
|
||||
swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
|
||||
|
||||
auto driveStates = swerveDriveSim.GetDriveStates();
|
||||
auto steerStates = swerveDriveSim.GetSteerStates();
|
||||
totalCurrentDraw = 0_A;
|
||||
std::array<units::ampere_t, 4> driveCurrents =
|
||||
swerveDriveSim.GetDriveCurrentDraw();
|
||||
for (const auto& current : driveCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
std::array<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)};
|
||||
swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
|
||||
steerPos, steerRate, steerCurrents[i]);
|
||||
}
|
||||
gyroSim.SetRate(-swerveDriveSim.GetOmega());
|
||||
gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
|
||||
|
||||
units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }
|
||||
@@ -0,0 +1,287 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "subsystems/SwerveDriveSim.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/system/Discretization.h>
|
||||
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
return (T(0) < val) - (val < T(0));
|
||||
}
|
||||
|
||||
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)
|
||||
: SwerveDriveSim(
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-driveFF.GetKv().to<double>() / driveFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0,
|
||||
1.0 / driveFF.GetKa().to<double>()},
|
||||
(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>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-steerFF.GetKv().to<double>() / steerFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0,
|
||||
1.0 / steerFF.GetKa().to<double>()},
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
|
||||
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)
|
||||
: drivePlant(drivePlant),
|
||||
driveKs(driveKs),
|
||||
driveMotor(driveMotor),
|
||||
driveGearing(driveGearing),
|
||||
driveWheelRadius(driveWheelRadius),
|
||||
steerPlant(steerPlant),
|
||||
steerKs(steerKs),
|
||||
steerMotor(steerMotor),
|
||||
steerGearing(steerGearing),
|
||||
kinematics(kinematics) {}
|
||||
|
||||
void SwerveDriveSim::SetDriveInputs(
|
||||
const std::array<units::volt_t, numModules>& inputs) {
|
||||
units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
|
||||
for (int i = 0; i < driveInputs.size(); i++) {
|
||||
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();
|
||||
for (int i = 0; i < steerInputs.size(); i++) {
|
||||
units::volt_t input = inputs[i];
|
||||
steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
auto Ax = discA * x;
|
||||
double nextStateVel = Ax(1, 0);
|
||||
double inputToStop = nextStateVel / -discB(1, 0);
|
||||
double ksSystemEffect =
|
||||
std::clamp(inputToStop, -kS.to<double>(), kS.to<double>());
|
||||
|
||||
nextStateVel += discB(1, 0) * ksSystemEffect;
|
||||
inputToStop = nextStateVel / -discB(1, 0);
|
||||
double signToStop = sgn(inputToStop);
|
||||
double inputSign = sgn(input.to<double>());
|
||||
double ksInputEffect = 0;
|
||||
|
||||
if (std::abs(ksSystemEffect) < kS.to<double>()) {
|
||||
double absInput = std::abs(input.to<double>());
|
||||
ksInputEffect =
|
||||
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
|
||||
} else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
|
||||
double absInput = std::abs(input.to<double>() - inputToStop);
|
||||
ksInputEffect =
|
||||
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
|
||||
}
|
||||
|
||||
auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
|
||||
ksInputEffect};
|
||||
auto Bu = discB * sF;
|
||||
auto retVal = Ax + Bu;
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Update(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);
|
||||
|
||||
Eigen::Matrix<double, 2, 2> steerDiscA;
|
||||
Eigen::Matrix<double, 2, 1> steerDiscB;
|
||||
frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
|
||||
&steerDiscB);
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> moduleDeltas;
|
||||
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
double prevDriveStatePos = driveStates[i](0, 0);
|
||||
driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i],
|
||||
driveInputs[i], driveKs);
|
||||
double currentDriveStatePos = driveStates[i](0, 0);
|
||||
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}}};
|
||||
}
|
||||
|
||||
frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
|
||||
pose = pose.Exp(twist);
|
||||
omega = twist.dtheta / dt;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
|
||||
this->pose = pose;
|
||||
if (!preserveMotion) {
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
driveStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
|
||||
steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
|
||||
}
|
||||
omega = 0_rad_per_s;
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleSteerStates) {
|
||||
this->pose = pose;
|
||||
driveStates = moduleDriveStates;
|
||||
steerStates = moduleSteerStates;
|
||||
omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetModulePositions() const {
|
||||
std::array<frc::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)}}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
|
||||
units::radian_t steerStdDev) {
|
||||
std::array<frc::SwerveModulePosition, numModules> positions;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
positions[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{driveStates[i](0, 0)} +
|
||||
randDist(generator) * driveStdDev,
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
|
||||
randDist(generator) * steerStdDev}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, numModules>
|
||||
SwerveDriveSim::GetModuleStates() {
|
||||
std::array<frc::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)}}};
|
||||
}
|
||||
return states;
|
||||
}
|
||||
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules>
|
||||
SwerveDriveSim::GetDriveStates() const {
|
||||
return driveStates;
|
||||
}
|
||||
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules>
|
||||
SwerveDriveSim::GetSteerStates() const {
|
||||
return steerStates;
|
||||
}
|
||||
|
||||
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;
|
||||
if (inputVolts >= 0_V) {
|
||||
effVolts = std::clamp(effVolts, 0_V, inputVolts);
|
||||
} else {
|
||||
effVolts = std::clamp(effVolts, inputVolts, 0_V);
|
||||
}
|
||||
auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R);
|
||||
return retVal;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
const {
|
||||
std::array<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 /
|
||||
driveWheelRadius.to<double>();
|
||||
currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
|
||||
frc::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
const {
|
||||
std::array<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};
|
||||
// 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());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
|
||||
units::ampere_t total{0};
|
||||
for (const auto& val : GetDriveCurrentDraw()) {
|
||||
total += val;
|
||||
}
|
||||
for (const auto& val : GetSteerCurrentDraw()) {
|
||||
total += val;
|
||||
}
|
||||
return total;
|
||||
}
|
||||
@@ -0,0 +1,146 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "subsystems/SwerveModule.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
#include <frc/MathUtil.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
|
||||
: moduleConstants(consts),
|
||||
driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
|
||||
driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
|
||||
moduleConstants.driveEncoderB}),
|
||||
steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
|
||||
steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
|
||||
moduleConstants.steerEncoderB}),
|
||||
driveEncoderSim(driveEncoder),
|
||||
steerEncoderSim(steerEncoder) {
|
||||
driveEncoder.SetDistancePerPulse(
|
||||
constants::Swerve::kDriveDistPerPulse.to<double>());
|
||||
steerEncoder.SetDistancePerPulse(
|
||||
constants::Swerve::kSteerRadPerPulse.to<double>());
|
||||
steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
|
||||
}
|
||||
|
||||
void SwerveModule::Periodic() {
|
||||
units::volt_t steerPID = units::volt_t{
|
||||
steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
|
||||
desiredState.angle.Radians().to<double>())};
|
||||
steerMotor.SetVoltage(steerPID);
|
||||
|
||||
units::volt_t driveFF =
|
||||
constants::Swerve::kDriveFF.Calculate(desiredState.speed);
|
||||
units::volt_t drivePID{0};
|
||||
if (!openLoop) {
|
||||
drivePID = units::volt_t{drivePIDController.Calculate(
|
||||
driveEncoder.GetRate(), desiredState.speed.to<double>())};
|
||||
}
|
||||
driveMotor.SetVoltage(driveFF + drivePID);
|
||||
}
|
||||
|
||||
void SwerveModule::SetDesiredState(frc::SwerveModuleState newState,
|
||||
bool shouldBeOpenLoop, bool steerInPlace) {
|
||||
frc::Rotation2d currentRotation = GetAbsoluteHeading();
|
||||
newState.Optimize(currentRotation);
|
||||
desiredState = newState;
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
|
||||
return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
|
||||
}
|
||||
|
||||
frc::SwerveModuleState SwerveModule::GetState() const {
|
||||
return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
frc::SwerveModulePosition SwerveModule::GetPosition() const {
|
||||
return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetDriveVoltage() const {
|
||||
return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetSteerVoltage() const {
|
||||
return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetDriveCurrentSim() const {
|
||||
return driveCurrentSim;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetSteerCurrentSim() const {
|
||||
return steerCurrentSim;
|
||||
}
|
||||
|
||||
constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
|
||||
return moduleConstants;
|
||||
}
|
||||
|
||||
void SwerveModule::Log() {
|
||||
frc::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>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Steer Target Degrees",
|
||||
units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Feet",
|
||||
state.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Target Feet",
|
||||
desiredState.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
driveCurrentSim.to<double>());
|
||||
frc::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) {
|
||||
driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
|
||||
driveEncoderSim.SetRate(driveEncoderRate.to<double>());
|
||||
driveCurrentSim = driveCurrent;
|
||||
steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
|
||||
steerEncoderSim.SetRate(steerEncoderRate.to<double>());
|
||||
steerCurrentSim = steerCurrent;
|
||||
}
|
||||
Reference in New Issue
Block a user