YAMS
Yet Another Mechanism System
Loading...
Searching...
No Matches
SwerveInputStream.hpp
Go to the documentation of this file.
1// Copyright (c) 2026 Yet Another Software Suite
2// SPDX-License-Identifier: LGPL-3.0-or-later
3
4#pragma once
5
6#include <cmath>
7#include <functional>
8#include <iostream>
9#include <memory>
10#include <numbers>
11#include <optional>
12#include <stdexcept>
13#include <string>
14#include <utility>
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>
29
30#include "yams/exceptions.hpp"
33
35
36template <size_t NumModules>
38
39template <size_t NumModules = 4>
41 public:
43
44 static SwerveInputStream Of(SwerveDrive<NumModules>& drive, std::function<double()> x,
45 std::function<double()> y) {
46 return SwerveInputStream{drive, std::move(x), std::move(y)};
47 }
48
49 SwerveInputStream(SwerveDrive<NumModules>& drive, std::function<double()> x,
50 std::function<double()> y, std::function<double()> rot)
51 : SwerveInputStream{drive, std::move(x), std::move(y)} {
52 m_controllerOmega = std::move(rot);
53 }
54
55 SwerveInputStream(SwerveDrive<NumModules>& drive, std::function<double()> x,
56 std::function<double()> y, std::function<double()> headingX,
57 std::function<double()> headingY)
58 : SwerveInputStream{drive, std::move(x), std::move(y)} {
59 WithControllerHeadingAxis(std::move(headingX), std::move(headingY));
60 }
61
67 SwerveInputStream clone = *this;
68 clone.m_telemetry.settings.reset();
69 return clone;
70 }
71
85 SwerveInputStream& WithTelemetry(std::string name, TelemetryVerbosity verbosity) {
86 m_telemetry.telemetry.reset();
87 if (verbosity == TelemetryVerbosity::NONE) {
88 m_telemetry.settings.reset();
89 } else {
90 m_telemetry.settings.emplace(std::move(name), verbosity);
91 }
92 return *this;
93 }
94
100 std::optional<std::reference_wrapper<SwerveInputStreamTelemetry<NumModules>>> GetTelemetry() {
101 if (!m_telemetry.settings) {
102 return std::nullopt;
103 }
104 if (!m_telemetry.telemetry) {
105 m_telemetry.telemetry = std::make_unique<SwerveInputStreamTelemetry<NumModules>>(
106 *this, m_telemetry.settings->first, m_telemetry.settings->second);
107 }
108 return std::ref(*m_telemetry.telemetry);
109 }
110
111 std::string GetCurrentModeName() const {
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:
118 return "HEADING";
119 case SwerveInputMode::AIM:
120 return "AIM";
121 }
122 return "UNKNOWN";
123 }
124
129 std::optional<wpi::units::radian_t> GetRotationControllerMeasurement() const {
130 return m_rotationControllerMeasurement;
131 }
132
137 std::optional<wpi::units::radian_t> GetRotationControllerSetpoint() const {
138 return m_rotationControllerSetpoint;
139 }
140
141 double GetAxisDeadband() const { return m_axisDeadband.value_or(0.0); }
142 void SetAxisDeadband(double deadband) {
143 m_axisDeadband = (deadband == 0.0) ? std::nullopt : std::optional<double>{deadband};
144 }
145
146 double GetTranslationAxisScale() const { return m_translationAxisScale.value_or(1.0); }
147 void SetTranslationAxisScale(double scale) {
148 m_translationAxisScale = (scale == 0.0) ? std::nullopt : std::optional<double>{scale};
149 }
150
151 double GetOmegaAxisScale() const { return m_omegaAxisScale.value_or(1.0); }
152 void SetOmegaAxisScale(double scale) {
153 m_omegaAxisScale = (scale == 0.0) ? std::nullopt : std::optional<double>{scale};
154 }
155
156 wpi::units::meters_per_second_t GetMaximumChassisLinearVelocity() const {
157 return m_maximumChassisLinearVelocity;
158 }
159 void SetMaximumChassisLinearVelocity(wpi::units::meters_per_second_t velocity) {
160 m_maximumChassisLinearVelocity = velocity;
161 }
162
163 wpi::units::radians_per_second_t GetMaximumChassisAngularVelocity() const {
164 return m_maximumChassisAngularVelocity;
165 }
166 void SetMaximumChassisAngularVelocity(wpi::units::radians_per_second_t velocity) {
167 m_maximumChassisAngularVelocity = velocity;
168 }
169
171 return m_translationCube.has_value() && m_translationCube.value()();
172 }
173 void SetTranslationCubeEnabled(bool enabled) {
174 m_translationCube = enabled ? std::optional<std::function<bool()>>{[] { return true; }} : std::nullopt;
175 }
176
177 bool IsOmegaCubeEnabled() const { return m_omegaCube.has_value() && m_omegaCube.value()(); }
178 void SetOmegaCubeEnabled(bool enabled) {
179 m_omegaCube = enabled ? std::optional<std::function<bool()>>{[] { return true; }} : std::nullopt;
180 }
181
183 return m_allianceRelative.has_value() && m_allianceRelative.value()();
184 }
185 void SetAllianceRelativeEnabled(bool enabled) {
186 m_allianceRelative = enabled ? std::optional<std::function<bool()>>{[] { return true; }} : std::nullopt;
187 }
188
189 bool IsRobotRelativeEnabled() const { return m_robotRelative.has_value() && m_robotRelative.value()(); }
190 void SetRobotRelativeEnabled(bool enabled) {
191 m_robotRelative = enabled ? std::optional<std::function<bool()>>{[] { return true; }} : std::nullopt;
192 }
193
194 SwerveInputStream& WithMaximumLinearVelocity(wpi::units::meters_per_second_t velocity) {
195 m_maximumChassisLinearVelocity = velocity;
196 return *this;
197 }
198
199 SwerveInputStream& WithMaximumAngularVelocity(wpi::units::radians_per_second_t velocity) {
200 m_maximumChassisAngularVelocity = velocity;
201 return *this;
202 }
203
204 SwerveInputStream& WithControllerRotationAxis(std::function<double()> rot) {
205 m_controllerOmega = std::move(rot);
206 return *this;
207 }
208
209 SwerveInputStream& WithControllerHeadingAxis(std::function<double()> headingX,
210 std::function<double()> headingY) {
211 WithHeading([headingX, headingY] {
212 return wpi::units::radian_t{std::atan2(headingX(), headingY())};
213 });
214 m_controllerHeadingX = std::move(headingX);
215 m_controllerHeadingY = std::move(headingY);
216 return *this;
217 }
218
219 SwerveInputStream& WithDeadband(double deadband) {
220 m_axisDeadband = (deadband == 0.0) ? std::nullopt : std::optional<double>{deadband};
221 return *this;
222 }
223
225 SwerveInputStream& Deadband(double deadband) { return WithDeadband(deadband); }
226
228 m_translationAxisScale = (scale == 0.0) ? std::nullopt : std::optional<double>{scale};
229 return *this;
230 }
231
233 m_omegaAxisScale = (scale == 0.0) ? std::nullopt : std::optional<double>{scale};
234 return *this;
235 }
236
237 SwerveInputStream& WithHeadingControl(std::function<bool()> trigger) {
238 m_headingEnabled = std::move(trigger);
239 return *this;
240 }
241
242 SwerveInputStream& WithHeading(std::function<wpi::units::radian_t()> heading) {
243 m_headingSupplier = std::move(heading);
244 m_controllerHeadingX.reset();
245 m_controllerHeadingY.reset();
246 return *this;
247 }
248
249 SwerveInputStream& WithAim(std::function<wpi::math::Pose2d()> aimTarget,
250 std::function<bool()> trigger) {
251 m_aimTarget = std::move(aimTarget);
252 m_aimEnabled = std::move(trigger);
253 return *this;
254 }
255
268 SwerveInputStream& WithAimLookahead(wpi::units::second_t lookahead) {
269 m_aimLookahead = lookahead;
270 return *this;
271 }
272
279 wpi::math::SimpleMotorFeedforward<wpi::units::radians> feedforward) {
280 m_aimFeedforward = feedforward;
281 return *this;
282 }
283
288 SwerveInputStream& WithAimHeadingOffset(wpi::math::Rotation2d offset,
289 std::function<bool()> enabled) {
290 m_aimHeadingOffset = offset;
291 m_aimHeadingOffsetEnabled = std::move(enabled);
292 return *this;
293 }
294
295 SwerveInputStream& WithAimHeadingOffset(wpi::math::Rotation2d offset) {
296 return WithAimHeadingOffset(offset, [] { return true; });
297 }
298
303 bool IsAimLocked(wpi::units::radian_t tolerance) {
304 if (!m_aimEnabled.has_value() || !m_aimEnabled.value()() || !m_aimGoalHeading.has_value()) {
305 return false;
306 }
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()) <=
310 tolerance;
311 }
312
317 wpi::math::Translation2d GetTargetVector(const wpi::math::Pose2d& target) {
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()};
323 }
324 return target.Translation() - robot;
325 }
326
331 wpi::units::radians_per_second_t CalculateAngularVelocity(wpi::units::radian_t target) {
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})
338 .value();
339 }
340 return wpi::units::radians_per_second_t{omega};
341 }
342
343 SwerveInputStream& WithTranslationOnly(std::function<bool()> trigger) {
344 m_translationOnlyEnabled = std::move(trigger);
345 return *this;
346 }
347
348 SwerveInputStream& WithCubeRotationControllerAxis(std::function<bool()> enabled) {
349 m_omegaCube = std::move(enabled);
350 return *this;
351 }
352
356
357 SwerveInputStream& WithCubeTranslationControllerAxis(std::function<bool()> enabled) {
358 m_translationCube = std::move(enabled);
359 return *this;
360 }
361
365
366 SwerveInputStream& WithAllianceRelativeControl(std::function<bool()> enabled) {
367 m_allianceRelative = std::move(enabled);
368 return *this;
369 }
370
374
375 SwerveInputStream& WithRobotRelative(std::function<bool()> enabled) {
376 m_robotRelative = std::move(enabled);
377 return *this;
378 }
379
381 return WithRobotRelative([] { return true; });
382 }
383
385 std::function<bool()> enabled) {
386 m_translationHeadingOffset = angle;
387 m_translationHeadingOffsetEnabled = std::move(enabled);
388 return *this;
389 }
390
391 SwerveInputStream& WithTranslationHeadingOffset(wpi::math::Rotation2d angle) {
392 return WithTranslationHeadingOffset(angle, [] { return true; });
393 }
394
395 wpi::math::ChassisVelocities Get() {
396 m_rotationControllerMeasurement = std::nullopt;
397 m_rotationControllerSetpoint = std::nullopt;
398 // Starts from the drive config's maximums (see the constructor); WithMaximum*Velocity and live
399 // tuning override them.
400 double maxLinear = m_maximumChassisLinearVelocity.value();
401 wpi::units::radians_per_second_t maxAngular = m_maximumChassisAngularVelocity;
402
403 wpi::math::Translation2d scaledTranslation = ApplyTranslationScalar(
404 ApplyDeadband(m_controllerTranslationX()), ApplyDeadband(m_controllerTranslationY()));
405 scaledTranslation = ApplyTranslationCube(scaledTranslation);
406 scaledTranslation = ApplyAllianceAwareTranslation(scaledTranslation);
407
408 double vx = scaledTranslation.X().value() * maxLinear;
409 double vy = scaledTranslation.Y().value() * maxLinear;
410 double omega = 0.0;
411 wpi::math::ChassisVelocities speeds{};
412
413 SwerveInputMode newMode = FindMode();
414 if (m_currentMode != newMode) {
415 TransitionMode(newMode);
416 }
417
418 switch (newMode) {
419 case SwerveInputMode::TRANSLATION_ONLY: {
420 omega = CalculateRotation(wpi::units::radian_t{m_swerveDrive->GetGyroAngle()},
421 m_lockedHeading.value().Radians());
422 break;
423 }
424 case SwerveInputMode::ANGULAR_VELOCITY: {
425 omega = ApplyOmegaCube(ApplyRotationalScalar(ApplyDeadband(m_controllerOmega.value()()))) *
426 maxAngular.value();
427 break;
428 }
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()) {
436 omega = 0.0;
437 }
438 break;
439 }
440 case SwerveInputMode::AIM: {
441 auto currentHeading =
442 wpi::math::Rotation2d{wpi::units::radian_t{m_swerveDrive->GetGyroAngle()}};
443 auto target = currentHeading;
444 // The vector is field relative. Turn it into a heading in the gyro's frame.
445 if (auto fieldAngle = GetTargetVector(m_aimTarget.value()()).Angle()) {
446 target = fieldAngle.value() - m_swerveDrive->GetPose().Rotation() + currentHeading;
447 }
448 if (m_aimHeadingOffset.has_value() && m_aimHeadingOffsetEnabled.has_value() &&
449 m_aimHeadingOffsetEnabled.value()()) {
450 target = target + m_aimHeadingOffset.value();
451 }
452 m_aimGoalHeading = target;
453 omega = CalculateAngularVelocity(target.Radians()).value();
454 break;
455 }
456 }
457
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));
463 if (auto telemetry = GetTelemetry()) {
464 telemetry->get().UpdateTelemetry();
465 }
466 return fieldRelativeSpeeds;
467 }
468
469 wpi::math::ChassisVelocities operator()() { return Get(); }
470
471 private:
472 enum class SwerveInputMode {
473 TRANSLATION_ONLY,
474 ANGULAR_VELOCITY,
475 HEADING,
476 AIM,
477 };
478
479 SwerveInputStream(SwerveDrive<NumModules>& drive, std::function<double()> x,
480 std::function<double()> y)
481 : m_swerveDrive{&drive},
482 m_controllerTranslationX{std::move(x)},
483 m_controllerTranslationY{std::move(y)} {
484 auto& cfg = drive.GetConfig();
485 if (auto maxLinear = cfg.GetMaximumChassisLinearVelocity()) {
486 m_maximumChassisLinearVelocity = *maxLinear;
487 }
488 if (auto maxAngular = cfg.GetMaximumChassisAngularVelocity()) {
489 m_maximumChassisAngularVelocity = wpi::units::radians_per_second_t{*maxAngular};
490 }
491 }
492
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;
518 // Heading the robot is turning to in AIM, in the gyro's frame.
519 std::optional<wpi::math::Rotation2d> m_aimGoalHeading;
520 // Measurement and setpoint the rotation controller was given in the last Get(), empty when it
521 // wasn't used.
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};
527
533 struct TelemetryHolder {
534 std::optional<std::pair<std::string, TelemetryVerbosity>> settings;
535 std::unique_ptr<SwerveInputStreamTelemetry<NumModules>> telemetry;
536
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();
542 }
543 TelemetryHolder& operator=(const TelemetryHolder& other) {
544 if (this != &other) {
545 settings = other.settings;
546 telemetry.reset();
547 }
548 return *this;
549 }
550 TelemetryHolder& operator=(TelemetryHolder&& other) noexcept {
551 if (this != &other) {
552 settings = std::move(other.settings);
553 telemetry.reset();
554 other.settings.reset();
555 other.telemetry.reset();
556 }
557 return *this;
558 }
559 };
560 TelemetryHolder m_telemetry;
561
562 wpi::math::PIDController& RequireRotationPID() {
563 auto pid = m_swerveDrive->GetConfig().GetRotationPID();
564 if (!pid) {
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 "
568 "error.");
569 }
570 return pid->get();
571 }
572
573 // Run the rotation controller, keeping its measurement and setpoint for telemetry. Returns rad/s.
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());
578 }
579
580 SwerveInputMode FindMode() {
581 if (m_translationOnlyEnabled.has_value() && m_translationOnlyEnabled.value()()) {
582 return SwerveInputMode::TRANSLATION_ONLY;
583 }
584 if (m_aimEnabled.has_value() && m_aimEnabled.value()()) {
585 if (m_aimTarget.has_value()) {
586 return SwerveInputMode::AIM;
587 }
588 std::cerr << "[YAMS SwerveInputStream] AIM mode enabled but no target set. Call WithAim() first.\n";
589 }
590 if (m_headingEnabled.has_value() && m_headingEnabled.value()()) {
591 if (m_headingSupplier.has_value()) {
592 return SwerveInputMode::HEADING;
593 }
594 std::cerr << "[YAMS SwerveInputStream] HEADING mode enabled but no heading set. "
595 "Call WithHeading() or WithControllerHeadingAxis() first.\n";
596 }
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;
602 }
603 return SwerveInputMode::ANGULAR_VELOCITY;
604 }
605
606 void TransitionMode(SwerveInputMode newMode) {
607 if (m_currentMode == SwerveInputMode::AIM) {
608 m_aimGoalHeading = std::nullopt;
609 }
610 switch (m_currentMode) {
611 case SwerveInputMode::TRANSLATION_ONLY:
612 m_lockedHeading = std::nullopt;
613 break;
614 case SwerveInputMode::HEADING:
615 case SwerveInputMode::AIM:
616 m_swerveDrive->ResetRotationPID();
617 break;
618 default:
619 break;
620 }
621 switch (newMode) {
622 case SwerveInputMode::TRANSLATION_ONLY:
623 m_lockedHeading =
624 wpi::math::Rotation2d{wpi::units::radian_t{m_swerveDrive->GetGyroAngle()}};
625 m_swerveDrive->ResetRotationPID();
626 break;
627 case SwerveInputMode::HEADING:
628 case SwerveInputMode::AIM:
629 m_swerveDrive->ResetRotationPID();
630 break;
631 default:
632 break;
633 }
634 }
635
636 double ApplyDeadband(double axisValue) {
637 return m_axisDeadband.has_value() ? wpi::math::ApplyDeadband(axisValue, m_axisDeadband.value())
638 : axisValue;
639 }
640
641 double ApplyRotationalScalar(double axisValue) {
642 return m_omegaAxisScale.has_value() ? axisValue * m_omegaAxisScale.value() : axisValue;
643 }
644
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()
648 ? SwerveDriveConfig::ScaleTranslation(t, m_translationAxisScale.value())
649 : t;
650 }
651
652 wpi::math::Translation2d ApplyTranslationCube(wpi::math::Translation2d translation) {
653 if (m_translationCube.has_value() && m_translationCube.value()()) {
654 return SwerveDriveConfig::CubeTranslation(translation);
655 }
656 return translation;
657 }
658
659 double ApplyOmegaCube(double rotationAxis) {
660 if (m_omegaCube.has_value() && m_omegaCube.value()()) {
661 return rotationAxis * rotationAxis * rotationAxis;
662 }
663 return rotationAxis;
664 }
665
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()}});
670 }
671 return speeds;
672 }
673
674 wpi::math::Translation2d ApplyAllianceAwareTranslation(wpi::math::Translation2d translation) {
675 if (m_robotRelative.has_value() && m_robotRelative.value()()) {
676 return translation;
677 }
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}});
682 }
683 }
684 return translation;
685 }
686
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()},
695 speeds.omega};
696 }
697 return speeds;
698 }
699};
700
701} // namespace yams::mechanisms::swerve::utility
702
703// The telemetry is a template too, so it only needs to be complete where a stream is used.
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 SwerveInputStreamTelemetry.hpp:50
SwerveInputStream(SwerveDrive< NumModules > &drive, std::function< double()> x, std::function< double()> y, std::function< double()> headingX, std::function< double()> headingY)
Definition SwerveInputStream.hpp:55
bool IsTranslationCubeEnabled() const
Definition SwerveInputStream.hpp:170
bool IsAimLocked(wpi::units::radian_t tolerance)
Definition SwerveInputStream.hpp:303
SwerveInputStream & WithAimHeadingOffset(wpi::math::Rotation2d offset, std::function< bool()> enabled)
Definition SwerveInputStream.hpp:288
wpi::math::Translation2d GetTargetVector(const wpi::math::Pose2d &target)
Definition SwerveInputStream.hpp:317
SwerveDriveConfig::TelemetryVerbosity TelemetryVerbosity
Definition SwerveInputStream.hpp:42
SwerveInputStream & WithDeadband(double deadband)
Definition SwerveInputStream.hpp:219
void SetTranslationCubeEnabled(bool enabled)
Definition SwerveInputStream.hpp:173
SwerveInputStream & WithControllerRotationAxis(std::function< double()> rot)
Definition SwerveInputStream.hpp:204
SwerveInputStream & WithCubeRotationControllerAxis()
Definition SwerveInputStream.hpp:353
SwerveInputStream & WithAim(std::function< wpi::math::Pose2d()> aimTarget, std::function< bool()> trigger)
Definition SwerveInputStream.hpp:249
SwerveInputStream & WithMaximumAngularVelocity(wpi::units::radians_per_second_t velocity)
Definition SwerveInputStream.hpp:199
void SetAxisDeadband(double deadband)
Definition SwerveInputStream.hpp:142
SwerveInputStream Clone() const
Definition SwerveInputStream.hpp:66
void SetRobotRelativeEnabled(bool enabled)
Definition SwerveInputStream.hpp:190
SwerveInputStream & WithTranslationHeadingOffset(wpi::math::Rotation2d angle, std::function< bool()> enabled)
Definition SwerveInputStream.hpp:384
bool IsRobotRelativeEnabled() const
Definition SwerveInputStream.hpp:189
SwerveInputStream & Deadband(double deadband)
Definition SwerveInputStream.hpp:225
SwerveInputStream & WithAimHeadingOffset(wpi::math::Rotation2d offset)
Definition SwerveInputStream.hpp:295
wpi::units::radians_per_second_t CalculateAngularVelocity(wpi::units::radian_t target)
Definition SwerveInputStream.hpp:331
SwerveInputStream & WithScaleRotation(double scale)
Definition SwerveInputStream.hpp:232
void SetMaximumChassisAngularVelocity(wpi::units::radians_per_second_t velocity)
Definition SwerveInputStream.hpp:166
SwerveInputStream & WithTranslationOnly(std::function< bool()> trigger)
Definition SwerveInputStream.hpp:343
wpi::units::meters_per_second_t GetMaximumChassisLinearVelocity() const
Definition SwerveInputStream.hpp:156
wpi::math::ChassisVelocities Get()
Definition SwerveInputStream.hpp:395
std::optional< wpi::units::radian_t > GetRotationControllerMeasurement() const
Definition SwerveInputStream.hpp:129
SwerveInputStream & WithControllerHeadingAxis(std::function< double()> headingX, std::function< double()> headingY)
Definition SwerveInputStream.hpp:209
SwerveInputStream & WithRobotRelative()
Definition SwerveInputStream.hpp:380
wpi::math::ChassisVelocities operator()()
Definition SwerveInputStream.hpp:469
double GetOmegaAxisScale() const
Definition SwerveInputStream.hpp:151
SwerveInputStream & WithCubeTranslationControllerAxis()
Definition SwerveInputStream.hpp:362
void SetMaximumChassisLinearVelocity(wpi::units::meters_per_second_t velocity)
Definition SwerveInputStream.hpp:159
SwerveInputStream & WithTelemetry(std::string name, TelemetryVerbosity verbosity)
Definition SwerveInputStream.hpp:85
void SetOmegaCubeEnabled(bool enabled)
Definition SwerveInputStream.hpp:178
std::optional< std::reference_wrapper< SwerveInputStreamTelemetry< NumModules > > > GetTelemetry()
Definition SwerveInputStream.hpp:100
SwerveInputStream & WithRobotRelative(std::function< bool()> enabled)
Definition SwerveInputStream.hpp:375
std::optional< wpi::units::radian_t > GetRotationControllerSetpoint() const
Definition SwerveInputStream.hpp:137
SwerveInputStream & WithAllianceRelativeControl(std::function< bool()> enabled)
Definition SwerveInputStream.hpp:366
SwerveInputStream & WithScaleTranslation(double scale)
Definition SwerveInputStream.hpp:227
SwerveInputStream & WithCubeTranslationControllerAxis(std::function< bool()> enabled)
Definition SwerveInputStream.hpp:357
SwerveInputStream & WithAimLookahead(wpi::units::second_t lookahead)
Definition SwerveInputStream.hpp:268
SwerveInputStream(SwerveDrive< NumModules > &drive, std::function< double()> x, std::function< double()> y, std::function< double()> rot)
Definition SwerveInputStream.hpp:49
SwerveInputStream & WithAimFeedforward(wpi::math::SimpleMotorFeedforward< wpi::units::radians > feedforward)
Definition SwerveInputStream.hpp:278
SwerveInputStream & WithCubeRotationControllerAxis(std::function< bool()> enabled)
Definition SwerveInputStream.hpp:348
double GetTranslationAxisScale() const
Definition SwerveInputStream.hpp:146
SwerveInputStream & WithMaximumLinearVelocity(wpi::units::meters_per_second_t velocity)
Definition SwerveInputStream.hpp:194
void SetOmegaAxisScale(double scale)
Definition SwerveInputStream.hpp:152
SwerveInputStream & WithHeading(std::function< wpi::units::radian_t()> heading)
Definition SwerveInputStream.hpp:242
double GetAxisDeadband() const
Definition SwerveInputStream.hpp:141
void SetAllianceRelativeEnabled(bool enabled)
Definition SwerveInputStream.hpp:185
void SetTranslationAxisScale(double scale)
Definition SwerveInputStream.hpp:147
SwerveInputStream & WithTranslationHeadingOffset(wpi::math::Rotation2d angle)
Definition SwerveInputStream.hpp:391
static SwerveInputStream Of(SwerveDrive< NumModules > &drive, std::function< double()> x, std::function< double()> y)
Definition SwerveInputStream.hpp:44
SwerveInputStream & WithHeadingControl(std::function< bool()> trigger)
Definition SwerveInputStream.hpp:237
bool IsAllianceRelativeEnabled() const
Definition SwerveInputStream.hpp:182
SwerveInputStream & WithAllianceRelativeControl()
Definition SwerveInputStream.hpp:371
std::string GetCurrentModeName() const
Definition SwerveInputStream.hpp:111
wpi::units::radians_per_second_t GetMaximumChassisAngularVelocity() const
Definition SwerveInputStream.hpp:163
bool IsOmegaCubeEnabled() const
Definition SwerveInputStream.hpp:177
Definition SwerveInputStream.hpp:34
Definition FlyWheel.hpp:21
Definition SwerveDrive.hpp:872