15#include <wpi/driverstation/Alliance.hpp>
16#include <wpi/driverstation/MatchState.hpp>
17#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
18#include <wpi/math/geometry/Pose2d.hpp>
19#include <wpi/math/geometry/Rotation2d.hpp>
20#include <wpi/math/geometry/Translation2d.hpp>
21#include <wpi/math/kinematics/ChassisVelocities.hpp>
22#include <wpi/math/util/MathUtil.hpp>
23#include <wpi/units/angle.hpp>
24#include <wpi/units/angular_velocity.hpp>
25#include <wpi/units/length.hpp>
26#include <wpi/units/math.hpp>
27#include <wpi/units/time.hpp>
28#include <wpi/units/velocity.hpp>
36template <
size_t NumModules>
39template <
size_t NumModules = 4>
45 std::function<
double()> y) {
50 std::function<
double()> y, std::function<
double()> rot)
52 m_controllerOmega = std::move(rot);
56 std::function<
double()> y, std::function<
double()> headingX,
57 std::function<
double()> headingY)
68 clone.m_telemetry.settings.reset();
86 m_telemetry.telemetry.reset();
87 if (verbosity == TelemetryVerbosity::NONE) {
88 m_telemetry.settings.reset();
90 m_telemetry.settings.emplace(std::move(name), verbosity);
100 std::optional<std::reference_wrapper<SwerveInputStreamTelemetry<NumModules>>>
GetTelemetry() {
101 if (!m_telemetry.settings) {
104 if (!m_telemetry.telemetry) {
105 m_telemetry.telemetry = std::make_unique<SwerveInputStreamTelemetry<NumModules>>(
106 *
this, m_telemetry.settings->first, m_telemetry.settings->second);
108 return std::ref(*m_telemetry.telemetry);
112 switch (m_currentMode) {
113 case SwerveInputMode::TRANSLATION_ONLY:
114 return "TRANSLATION_ONLY";
115 case SwerveInputMode::ANGULAR_VELOCITY:
116 return "ANGULAR_VELOCITY";
117 case SwerveInputMode::HEADING:
119 case SwerveInputMode::AIM:
130 return m_rotationControllerMeasurement;
138 return m_rotationControllerSetpoint;
143 m_axisDeadband = (deadband == 0.0) ? std::nullopt : std::optional<double>{deadband};
148 m_translationAxisScale = (scale == 0.0) ? std::nullopt : std::optional<double>{scale};
153 m_omegaAxisScale = (scale == 0.0) ? std::nullopt : std::optional<double>{scale};
157 return m_maximumChassisLinearVelocity;
160 m_maximumChassisLinearVelocity =
velocity;
164 return m_maximumChassisAngularVelocity;
167 m_maximumChassisAngularVelocity =
velocity;
171 return m_translationCube.has_value() && m_translationCube.value()();
174 m_translationCube = enabled ? std::optional<std::function<bool()>>{[] {
return true; }} : std::nullopt;
179 m_omegaCube = enabled ? std::optional<std::function<bool()>>{[] {
return true; }} : std::nullopt;
183 return m_allianceRelative.has_value() && m_allianceRelative.value()();
186 m_allianceRelative = enabled ? std::optional<std::function<bool()>>{[] {
return true; }} : std::nullopt;
191 m_robotRelative = enabled ? std::optional<std::function<bool()>>{[] {
return true; }} : std::nullopt;
195 m_maximumChassisLinearVelocity =
velocity;
200 m_maximumChassisAngularVelocity =
velocity;
205 m_controllerOmega = std::move(rot);
210 std::function<
double()> headingY) {
212 return wpi::units::radian_t{std::atan2(headingX(), headingY())};
214 m_controllerHeadingX = std::move(headingX);
215 m_controllerHeadingY = std::move(headingY);
220 m_axisDeadband = (deadband == 0.0) ? std::nullopt : std::optional<double>{deadband};
228 m_translationAxisScale = (scale == 0.0) ? std::nullopt : std::optional<double>{scale};
233 m_omegaAxisScale = (scale == 0.0) ? std::nullopt : std::optional<double>{scale};
238 m_headingEnabled = std::move(trigger);
243 m_headingSupplier = std::move(heading);
244 m_controllerHeadingX.reset();
245 m_controllerHeadingY.reset();
250 std::function<
bool()> trigger) {
251 m_aimTarget = std::move(aimTarget);
252 m_aimEnabled = std::move(trigger);
269 m_aimLookahead = lookahead;
279 wpi::math::SimpleMotorFeedforward<wpi::units::radians> feedforward) {
280 m_aimFeedforward = feedforward;
289 std::function<
bool()> enabled) {
290 m_aimHeadingOffset = offset;
291 m_aimHeadingOffsetEnabled = std::move(enabled);
304 if (!m_aimEnabled.has_value() || !m_aimEnabled.value()() || !m_aimGoalHeading.has_value()) {
307 auto currentHeading =
308 wpi::math::Rotation2d{wpi::units::radian_t{m_swerveDrive->GetGyroAngle()}};
309 return wpi::units::math::abs((m_aimGoalHeading.value() - currentHeading).Radians()) <=
318 auto robot = m_swerveDrive->GetPose().Translation();
319 if (m_aimLookahead.has_value()) {
320 auto fieldVelocity = m_swerveDrive->GetFieldRelativeSpeed();
321 robot = robot + wpi::math::Translation2d{fieldVelocity.vx * m_aimLookahead.value(),
322 fieldVelocity.vy * m_aimLookahead.value()};
324 return target.Translation() - robot;
332 double omega = CalculateRotation(
333 wpi::math::Rotation2d{wpi::units::radian_t{m_swerveDrive->GetGyroAngle()}}.Radians(), target);
334 if (m_aimFeedforward.has_value()) {
335 omega += m_aimFeedforward
336 ->Calculate(m_swerveDrive->GetRobotRelativeSpeed().omega,
337 wpi::units::radians_per_second_t{omega})
340 return wpi::units::radians_per_second_t{omega};
344 m_translationOnlyEnabled = std::move(trigger);
349 m_omegaCube = std::move(enabled);
358 m_translationCube = std::move(enabled);
367 m_allianceRelative = std::move(enabled);
376 m_robotRelative = std::move(enabled);
385 std::function<
bool()> enabled) {
386 m_translationHeadingOffset = angle;
387 m_translationHeadingOffsetEnabled = std::move(enabled);
395 wpi::math::ChassisVelocities
Get() {
396 m_rotationControllerMeasurement = std::nullopt;
397 m_rotationControllerSetpoint = std::nullopt;
400 double maxLinear = m_maximumChassisLinearVelocity.value();
401 wpi::units::radians_per_second_t maxAngular = m_maximumChassisAngularVelocity;
403 wpi::math::Translation2d scaledTranslation = ApplyTranslationScalar(
404 ApplyDeadband(m_controllerTranslationX()), ApplyDeadband(m_controllerTranslationY()));
405 scaledTranslation = ApplyTranslationCube(scaledTranslation);
406 scaledTranslation = ApplyAllianceAwareTranslation(scaledTranslation);
408 double vx = scaledTranslation.X().value() * maxLinear;
409 double vy = scaledTranslation.Y().value() * maxLinear;
411 wpi::math::ChassisVelocities speeds{};
413 SwerveInputMode newMode = FindMode();
414 if (m_currentMode != newMode) {
415 TransitionMode(newMode);
419 case SwerveInputMode::TRANSLATION_ONLY: {
420 omega = CalculateRotation(wpi::units::radian_t{m_swerveDrive->GetGyroAngle()},
421 m_lockedHeading.value().Radians());
424 case SwerveInputMode::ANGULAR_VELOCITY: {
425 omega = ApplyOmegaCube(ApplyRotationalScalar(ApplyDeadband(m_controllerOmega.value()()))) *
429 case SwerveInputMode::HEADING: {
430 omega = CalculateRotation(wpi::units::radian_t{m_swerveDrive->GetGyroAngle()},
431 m_headingSupplier.value()());
432 if (m_controllerHeadingX.has_value() && m_controllerHeadingY.has_value() &&
433 m_axisDeadband.has_value() &&
434 std::abs(m_controllerHeadingX.value()()) + std::abs(m_controllerHeadingY.value()()) <
435 m_axisDeadband.value()) {
440 case SwerveInputMode::AIM: {
441 auto currentHeading =
442 wpi::math::Rotation2d{wpi::units::radian_t{m_swerveDrive->GetGyroAngle()}};
443 auto target = currentHeading;
445 if (
auto fieldAngle =
GetTargetVector(m_aimTarget.value()()).Angle()) {
446 target = fieldAngle.value() - m_swerveDrive->GetPose().Rotation() + currentHeading;
448 if (m_aimHeadingOffset.has_value() && m_aimHeadingOffsetEnabled.has_value() &&
449 m_aimHeadingOffsetEnabled.value()()) {
450 target = target + m_aimHeadingOffset.value();
452 m_aimGoalHeading = target;
458 m_currentMode = newMode;
459 speeds = wpi::math::ChassisVelocities{wpi::units::meters_per_second_t{vx},
460 wpi::units::meters_per_second_t{vy},
461 wpi::units::radians_per_second_t{omega}};
462 auto fieldRelativeSpeeds = ApplyTranslationHeadingOffset(ApplyRobotRelativeTranslation(speeds));
466 return fieldRelativeSpeeds;
472 enum class SwerveInputMode {
480 std::function<
double()> y)
481 : m_swerveDrive{&drive},
482 m_controllerTranslationX{std::move(x)},
483 m_controllerTranslationY{std::move(y)} {
485 if (
auto maxLinear = cfg.GetMaximumChassisLinearVelocity()) {
486 m_maximumChassisLinearVelocity = *maxLinear;
488 if (
auto maxAngular = cfg.GetMaximumChassisAngularVelocity()) {
489 m_maximumChassisAngularVelocity = wpi::units::radians_per_second_t{*maxAngular};
493 SwerveDrive<NumModules>* m_swerveDrive;
494 std::function<double()> m_controllerTranslationX;
495 std::function<double()> m_controllerTranslationY;
496 std::optional<std::function<double()>> m_controllerOmega;
497 std::optional<std::function<double()>> m_controllerHeadingX;
498 std::optional<std::function<double()>> m_controllerHeadingY;
499 std::optional<std::function<wpi::units::radian_t()>> m_headingSupplier;
500 std::optional<double> m_axisDeadband;
501 std::optional<double> m_translationAxisScale;
502 std::optional<double> m_omegaAxisScale;
503 std::optional<std::function<wpi::math::Pose2d()>> m_aimTarget;
504 std::optional<std::function<bool()>> m_headingEnabled;
505 std::optional<wpi::math::Rotation2d> m_lockedHeading;
506 std::optional<std::function<bool()>> m_aimEnabled;
507 std::optional<std::function<bool()>> m_translationOnlyEnabled;
508 std::optional<std::function<bool()>> m_translationCube;
509 std::optional<std::function<bool()>> m_omegaCube;
510 std::optional<std::function<bool()>> m_robotRelative;
511 std::optional<std::function<bool()>> m_allianceRelative;
512 std::optional<std::function<bool()>> m_translationHeadingOffsetEnabled;
513 std::optional<wpi::math::Rotation2d> m_translationHeadingOffset;
514 std::optional<wpi::units::second_t> m_aimLookahead;
515 std::optional<wpi::math::SimpleMotorFeedforward<wpi::units::radians>> m_aimFeedforward;
516 std::optional<wpi::math::Rotation2d> m_aimHeadingOffset;
517 std::optional<std::function<bool()>> m_aimHeadingOffsetEnabled;
519 std::optional<wpi::math::Rotation2d> m_aimGoalHeading;
522 std::optional<wpi::units::radian_t> m_rotationControllerMeasurement;
523 std::optional<wpi::units::radian_t> m_rotationControllerSetpoint;
524 SwerveInputMode m_currentMode{SwerveInputMode::ANGULAR_VELOCITY};
525 wpi::units::meters_per_second_t m_maximumChassisLinearVelocity{4.0};
526 wpi::units::radians_per_second_t m_maximumChassisAngularVelocity{2.0 * std::numbers::pi};
533 struct TelemetryHolder {
534 std::optional<std::pair<std::string, TelemetryVerbosity>> settings;
535 std::unique_ptr<SwerveInputStreamTelemetry<NumModules>> telemetry;
537 TelemetryHolder() =
default;
538 TelemetryHolder(
const TelemetryHolder& other) : settings{other.settings} {}
539 TelemetryHolder(TelemetryHolder&& other) noexcept : settings{std::move(other.settings)} {
540 other.settings.reset();
541 other.telemetry.reset();
543 TelemetryHolder& operator=(
const TelemetryHolder& other) {
544 if (
this != &other) {
545 settings = other.settings;
550 TelemetryHolder& operator=(TelemetryHolder&& other)
noexcept {
551 if (
this != &other) {
552 settings = std::move(other.settings);
554 other.settings.reset();
555 other.telemetry.reset();
560 TelemetryHolder m_telemetry;
562 wpi::math::PIDController& RequireRotationPID() {
563 auto pid = m_swerveDrive->GetConfig().GetRotationPID();
565 throw exceptions::SwerveDriveConfigurationException(
566 "No rotation PID controller configured, heading, aim, and translation only control are "
567 "unavailable. Use SwerveDriveConfig::WithRotationController(PIDController) to fix this "
574 double CalculateRotation(wpi::units::radian_t measurement, wpi::units::radian_t setpoint) {
575 m_rotationControllerMeasurement = measurement;
576 m_rotationControllerSetpoint = setpoint;
577 return RequireRotationPID().Calculate(measurement.value(), setpoint.value());
580 SwerveInputMode FindMode() {
581 if (m_translationOnlyEnabled.has_value() && m_translationOnlyEnabled.value()()) {
582 return SwerveInputMode::TRANSLATION_ONLY;
584 if (m_aimEnabled.has_value() && m_aimEnabled.value()()) {
585 if (m_aimTarget.has_value()) {
586 return SwerveInputMode::AIM;
588 std::cerr <<
"[YAMS SwerveInputStream] AIM mode enabled but no target set. Call WithAim() first.\n";
590 if (m_headingEnabled.has_value() && m_headingEnabled.value()()) {
591 if (m_headingSupplier.has_value()) {
592 return SwerveInputMode::HEADING;
594 std::cerr <<
"[YAMS SwerveInputStream] HEADING mode enabled but no heading set. "
595 "Call WithHeading() or WithControllerHeadingAxis() first.\n";
597 if (!m_controllerOmega.has_value()) {
598 std::cerr <<
"[YAMS SwerveInputStream] No rotation axis configured. "
599 "Call WithControllerRotationAxis() or WithControllerHeadingAxis(). "
600 "Falling back to TRANSLATION_ONLY.\n";
601 return SwerveInputMode::TRANSLATION_ONLY;
603 return SwerveInputMode::ANGULAR_VELOCITY;
606 void TransitionMode(SwerveInputMode newMode) {
607 if (m_currentMode == SwerveInputMode::AIM) {
608 m_aimGoalHeading = std::nullopt;
610 switch (m_currentMode) {
611 case SwerveInputMode::TRANSLATION_ONLY:
612 m_lockedHeading = std::nullopt;
614 case SwerveInputMode::HEADING:
615 case SwerveInputMode::AIM:
616 m_swerveDrive->ResetRotationPID();
622 case SwerveInputMode::TRANSLATION_ONLY:
624 wpi::math::Rotation2d{wpi::units::radian_t{m_swerveDrive->GetGyroAngle()}};
625 m_swerveDrive->ResetRotationPID();
627 case SwerveInputMode::HEADING:
628 case SwerveInputMode::AIM:
629 m_swerveDrive->ResetRotationPID();
636 double ApplyDeadband(
double axisValue) {
637 return m_axisDeadband.has_value() ? wpi::math::ApplyDeadband(axisValue, m_axisDeadband.value())
641 double ApplyRotationalScalar(
double axisValue) {
642 return m_omegaAxisScale.has_value() ? axisValue * m_omegaAxisScale.value() : axisValue;
645 wpi::math::Translation2d ApplyTranslationScalar(
double xAxis,
double yAxis) {
646 wpi::math::Translation2d t{wpi::units::meter_t{xAxis}, wpi::units::meter_t{yAxis}};
647 return m_translationAxisScale.has_value()
652 wpi::math::Translation2d ApplyTranslationCube(wpi::math::Translation2d translation) {
653 if (m_translationCube.has_value() && m_translationCube.value()()) {
659 double ApplyOmegaCube(
double rotationAxis) {
660 if (m_omegaCube.has_value() && m_omegaCube.value()()) {
661 return rotationAxis * rotationAxis * rotationAxis;
666 wpi::math::ChassisVelocities ApplyRobotRelativeTranslation(wpi::math::ChassisVelocities speeds) {
667 if (m_robotRelative.has_value() && m_robotRelative.value()()) {
668 return speeds.ToFieldRelative(
669 wpi::math::Rotation2d{wpi::units::radian_t{m_swerveDrive->GetGyroAngle()}});
674 wpi::math::Translation2d ApplyAllianceAwareTranslation(wpi::math::Translation2d translation) {
675 if (m_robotRelative.has_value() && m_robotRelative.value()()) {
678 if (m_allianceRelative.has_value() && m_allianceRelative.value()()) {
679 auto alliance = wpi::MatchState::GetAlliance();
680 if (alliance.has_value() && alliance.value() == wpi::Alliance::RED) {
681 return translation.RotateBy(wpi::math::Rotation2d{wpi::units::degree_t{180.0}});
687 wpi::math::ChassisVelocities ApplyTranslationHeadingOffset(wpi::math::ChassisVelocities speeds) {
688 if (m_translationHeadingOffsetEnabled.has_value() &&
689 m_translationHeadingOffsetEnabled.value()() && m_translationHeadingOffset.has_value()) {
690 wpi::math::Translation2d vec{wpi::units::meter_t{speeds.vx.value()},
691 wpi::units::meter_t{speeds.vy.value()}};
692 auto rotated = vec.RotateBy(m_translationHeadingOffset.value());
693 return wpi::math::ChassisVelocities{wpi::units::meters_per_second_t{rotated.X().value()},
694 wpi::units::meters_per_second_t{rotated.Y().value()},
static wpi::math::Translation2d CubeTranslation(wpi::math::Translation2d translation)
static wpi::math::Translation2d ScaleTranslation(wpi::math::Translation2d translation, double scalar)
motorcontrollers::SmartMotorControllerConfig::TelemetryVerbosity TelemetryVerbosity
Definition SwerveDriveConfig.hpp:40
Definition SwerveDrive.hpp:138
SwerveDriveConfig & GetConfig()
Definition SwerveDrive.hpp:730
Definition SwerveInputStream.hpp:34
Definition FlyWheel.hpp:21
Definition SwerveDrive.hpp:872