YAMS
Yet Another Mechanism System
Loading...
Searching...
No Matches
SwerveDrive.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 <cassert>
7#include <cstdio>
8#include <functional>
9#include <optional>
10#include <span>
11#include <stdexcept>
12#include <string>
13#include <utility>
14#include <vector>
15#include <wpi/commands2/CommandPtr.hpp>
16#include <wpi/commands2/Commands.hpp>
17#include <wpi/framework/RobotBase.hpp>
18#include <wpi/hal/UsageReporting.hpp>
19#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
20#include <wpi/math/geometry/Pose2d.hpp>
21#include <wpi/math/geometry/Rotation2d.hpp>
22#include <wpi/math/geometry/Rotation3d.hpp>
23#include <wpi/math/geometry/Translation2d.hpp>
24#include <wpi/math/geometry/Twist2d.hpp>
25#include <wpi/math/kinematics/ChassisVelocities.hpp>
26#include <wpi/math/kinematics/SwerveDriveKinematics.hpp>
27#include <wpi/math/kinematics/SwerveModulePosition.hpp>
28#include <wpi/math/kinematics/SwerveModuleVelocity.hpp>
29#include <wpi/math/kinematics/struct/ChassisVelocitiesStruct.hpp>
30#include <wpi/nt/NetworkTableInstance.hpp>
31#include <wpi/smartdashboard/Field2d.hpp>
32#include <wpi/system/Timer.hpp>
33#include <wpi/tunables/Tunables.hpp>
34#include <wpi/units/angle.hpp>
35#include <wpi/units/force.hpp>
36#include <wpi/units/length.hpp>
37#include <wpi/units/math.hpp>
38#include <wpi/units/time.hpp>
39#include <wpi/units/velocity.hpp>
40#include <wpi/util/array.hpp>
41
42#include "yams/Rivet.hpp"
43#include "yams/exceptions.hpp"
50
52
137template <size_t NumModules = 4>
139 public:
146 : m_config{config},
147 m_kinematics{BuildKinematics(*m_config)},
148 m_poseEstimator{m_kinematics, ComputeInitialRotation(*m_config),
149 ComputeInitialPositions(*m_config), m_config->GetInitialPose()} {
150 assert(m_config->GetModules().size() == NumModules &&
151 "Module count in SwerveDriveConfig must equal NumModules template parameter.");
152 HAL_ReportUsage("RobotDrive", "YAGSL");
153
154 if (auto optimization = m_config->GetModuleStateOptimization()) {
155 for (auto* module : m_config->GetModules()) {
156 module->GetMutableConfig().WithOptimization(*optimization);
157 }
158 }
159 m_simPose = m_config->GetInitialPose();
160 // Start with the gyro reading the starting pose's heading so field relative driving matches it.
161 ResetOdometry(m_config->GetInitialPose());
162
163 SetupTelemetry();
164 Rivet::Show();
165
166 m_field2d.SetRobotPose(GetPose());
168 wpi::tunables::Publish("Mechanisms/" + GetName() + "/field", m_field2d);
169 }
170
171 // ---- Drive commands --------------------------------------------------------
172
179 wpi::cmd::CommandPtr Drive(std::function<wpi::math::ChassisVelocities()> robotRelativeSpeeds) {
180 return wpi::cmd::Run(
181 [this, robotRelativeSpeeds] {
182 SetRobotRelativeChassisSpeeds(robotRelativeSpeeds());
183 },
184 {m_config->GetSubsystem()})
185 .WithName("Drive");
186 }
187
197 wpi::cmd::CommandPtr DriveToPose(wpi::math::Pose2d pose) {
198 return wpi::cmd::StartRun(
199 [this] {
202 },
203 [this, pose] { SetRobotRelativeChassisSpeeds(DriveToPoseSetpoint(pose)); },
204 {m_config->GetSubsystem()})
205 .FinallyDo([this] { Stop(); })
206 .WithName("Drive to Pose");
207 }
208
221 wpi::cmd::CommandPtr DriveToPose(wpi::math::Pose2d pose,
222 wpi::units::meter_t translationTolerance,
223 wpi::units::degree_t rotationTolerance) {
224 return wpi::cmd::StartRun(
225 [this] {
228 },
229 [this, pose] { SetRobotRelativeChassisSpeeds(DriveToPoseSetpoint(pose)); },
230 {m_config->GetSubsystem()})
231 .Until([this, pose, translationTolerance, rotationTolerance] {
232 return IsNear(pose, translationTolerance, rotationTolerance);
233 })
234 .FinallyDo([this] { Stop(); })
235 .WithName("Drive to Pose");
236 }
237
246 bool IsNear(wpi::math::Pose2d pose, wpi::units::meter_t translationTolerance,
247 wpi::units::degree_t rotationTolerance) {
248 return GetDistanceFromPose(pose) <= translationTolerance &&
249 wpi::units::math::abs(GetAngleDifferenceFromPose(pose)) <= rotationTolerance;
250 }
251
253 void Stop() { SetRobotRelativeChassisSpeeds(wpi::math::ChassisVelocities{}); }
254
266 wpi::math::ChassisVelocities DriveToPoseSetpoint(wpi::math::Pose2d targetPose) {
267 return DriveToPoseSetpoint(targetPose, GetPose(),
268 wpi::math::Rotation2d{wpi::units::radian_t{GetGyroAngle()}});
269 }
270
282 wpi::math::ChassisVelocities DriveToPoseSetpoint(wpi::math::Pose2d targetPose,
283 wpi::math::Pose2d currentPose,
284 wpi::math::Rotation2d heading) {
285 auto rotationPIDRef = m_config->GetRotationPID();
286 if (!rotationPIDRef) {
288 "No rotation PID controller configured, cannot drive to pose. Use "
289 "SwerveDriveConfig::WithRotationController(PIDController) to fix this error.");
290 }
291 auto translationPIDRef = m_config->GetTranslationPID();
292 if (!translationPIDRef) {
294 "No translation PID controller configured, cannot drive to pose. Use "
295 "SwerveDriveConfig::WithTranslationController(PIDController) to fix this error.");
296 }
297 auto& rotationPID = rotationPIDRef->get();
298 auto& translationPID = translationPIDRef->get();
299 // Plain field-frame translation delta (not RelativeTo/operator-, which expresses the result
300 // in targetPose's rotated frame and would skew the commanded direction whenever targetPose's
301 // heading is non-zero).
302 auto toTarget = targetPose.Translation() - currentPose.Translation();
303 double distance = toTarget.Norm().value();
304 // The controller sees the distance as its measurement with a setpoint of 0, so its output is
305 // negative while there is distance left: the speed toward the target is the negative of it.
306 double speed = -translationPID.Calculate(distance, 0.0);
307 // Drive at that speed along the unit vector toward the target.
308 double vx = distance > 1e-9 ? toTarget.X().value() / distance * speed : 0.0;
309 double vy = distance > 1e-9 ? toTarget.Y().value() / distance * speed : 0.0;
310 return (wpi::math::ChassisVelocities{
311 wpi::units::meters_per_second_t{vx}, wpi::units::meters_per_second_t{vy},
312 wpi::units::radians_per_second_t{
313 rotationPID.Calculate(currentPose.Rotation().Radians().value(),
314 targetPose.Rotation().Radians().value())}})
315 .ToRobotRelative(heading);
316 }
317
318 // ---- Core drive methods ---------------------------------------------------
319
325 void SetSwerveModuleStates(wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> states) {
326 for (size_t i = 0; i < NumModules; ++i) {
327 m_desiredModuleStates[i] = m_config->GetModules()[i]->SetSwerveModuleState(states[i]);
328 }
329 }
330
337 void SetSwerveModuleStates(wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> states,
338 std::span<const wpi::units::newton_t> feedforwardForces) {
339 for (size_t i = 0; i < NumModules; ++i) {
340 m_desiredModuleStates[i] =
341 m_config->GetModules()[i]->SetSwerveModuleState(states[i], feedforwardForces[i]);
342 }
343 }
344
351 wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules>
352 GetStateFromRobotRelativeChassisSpeeds(wpi::math::ChassisVelocities robotRelativeSpeeds) {
353 robotRelativeSpeeds = m_config->OptimizeRobotRelativeChassisSpeeds(robotRelativeSpeeds);
354 auto cor = m_config->GetCenterOfRotation();
355 auto states = cor ? m_kinematics.ToSwerveModuleVelocities(robotRelativeSpeeds, *cor)
356 : m_kinematics.ToSwerveModuleVelocities(robotRelativeSpeeds);
357 // Scale the wheels down together when one is asked to go faster than it can.
358 if (auto maxModuleSpeed = m_config->GetMaximumModuleLinearVelocity()) {
359 auto maxLinear = m_config->GetMaximumChassisLinearVelocity();
360 auto maxAngular = m_config->GetMaximumChassisAngularVelocity();
361 if (maxLinear && maxAngular) {
362 states = wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelVelocities(
363 states, robotRelativeSpeeds, *maxModuleSpeed, *maxLinear,
364 wpi::units::radians_per_second_t{*maxAngular});
365 } else {
366 states = wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelVelocities(
367 states, *maxModuleSpeed);
368 }
369 }
370 return states;
371 }
372
379 wpi::math::ChassisVelocities GetRobotRelativeChassisSpeedsFromState(
380 const wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules>& states) {
381 return m_kinematics.ToChassisVelocities(states);
382 }
383
389 void SetRobotRelativeChassisSpeeds(wpi::math::ChassisVelocities robotRelativeSpeeds) {
390 m_desiredChassisSpeeds = robotRelativeSpeeds;
392 }
393
402 void SetRobotRelativeChassisSpeeds(wpi::math::ChassisVelocities robotRelativeSpeeds,
403 std::span<const wpi::units::newton_t> feedforwardForces) {
404 m_desiredChassisSpeeds = robotRelativeSpeeds;
405 auto states = GetStateFromRobotRelativeChassisSpeeds(robotRelativeSpeeds);
406 if (feedforwardForces.empty()) {
407 SetSwerveModuleStates(states);
408 return;
409 }
410 if (feedforwardForces.size() != NumModules) {
411 throw std::invalid_argument("feedforwardForces must be empty or have one entry per module (" +
412 std::to_string(NumModules) + "), in FL, FR, BL, BR order.");
413 }
414 SetSwerveModuleStates(states, feedforwardForces);
415 }
416
422 void SetFieldRelativeChassisSpeeds(wpi::math::ChassisVelocities fieldRelativeSpeeds) {
424 (fieldRelativeSpeeds)
425 .ToRobotRelative(wpi::math::Rotation2d{wpi::units::radian_t{GetGyroAngle()}}));
426 }
427
431 void LockPose() {
432 // Turn each wheel toward the robot center without driving it. SwerveModule::SetAzimuthAngle
433 // skips the velocity deadband, which would otherwise hold these zero speed states where the
434 // wheels are.
435 for (size_t i = 0; i < NumModules; ++i) {
436 auto* module = m_config->GetModules()[i];
437 m_desiredModuleStates[i] = module->SetAzimuthAngle(
438 module->GetConfig().GetLocation()->Angle().value_or(wpi::math::Rotation2d{}));
439 }
440 m_desiredChassisSpeeds = wpi::math::ChassisVelocities{};
441 }
442
450 void SetAzimuthAngles(wpi::math::Rotation2d angle) {
451 for (size_t i = 0; i < NumModules; ++i) {
452 m_desiredModuleStates[i] = m_config->GetModules()[i]->SetAzimuthAngle(angle);
453 }
454 m_desiredChassisSpeeds = wpi::math::ChassisVelocities{};
455 }
456
457 // ---- Odometry & pose -------------------------------------------------------
458
462 wpi::math::Pose2d GetPose() { return m_poseEstimator.GetEstimatedPosition(); }
463
471 void ResetOdometry(wpi::math::Pose2d pose) {
472 SetGyroAngle(pose.Rotation().Degrees());
473 m_poseEstimator.ResetPosition(wpi::math::Rotation2d{wpi::units::radian_t{GetGyroAngle()}},
474 GetModulePositions(), pose);
475 m_desiredChassisSpeeds = wpi::math::ChassisVelocities{};
476 m_desiredModuleStates = m_kinematics.ToSwerveModuleVelocities(wpi::math::ChassisVelocities{});
477 m_simPose = pose;
478 }
479
483 void ZeroGyro() {
484 // ResetOdometry also sets the gyro to read the new heading.
485 ResetOdometry(wpi::math::Pose2d{GetPose().Translation(), wpi::math::Rotation2d{}});
486 }
487
494 void AddVisionMeasurement(wpi::math::Pose2d robotPose, wpi::units::second_t timestamp) {
495 m_poseEstimator.AddVisionMeasurement(robotPose, timestamp);
496 }
497
505 void AddVisionMeasurement(wpi::math::Pose2d robotPose, wpi::units::second_t timestamp,
506 const wpi::util::array<double, 3>& stdDevs) {
507 m_poseEstimator.AddVisionMeasurement(robotPose, timestamp, stdDevs);
508 }
509
510 // ---- Telemetry & simulation -----------------------------------------------
511
518 UpdatePoseEstimator();
519 auto pose = GetPose();
520 auto currentStates = GetModuleStates();
521
522 m_swerveTelemetry->Publish(this);
523
524 for (auto* mod : m_config->GetModules()) {
525 mod->UpdateTelemetry();
526 }
527 m_telemetry.UpdateLoopTime();
528
529 m_field2d.SetRobotPose(pose);
530 std::vector<wpi::math::Pose2d> modulePoses;
531 modulePoses.reserve(NumModules);
532 for (size_t i = 0; i < NumModules; ++i) {
533 auto* mod = m_config->GetModules()[i];
534 auto location = *mod->GetConfig().GetLocation();
535 auto moduleTranslation = pose.Translation() + location.RotateBy(pose.Rotation());
536 auto moduleHeading = pose.Rotation() + currentStates[i].angle;
537 modulePoses.push_back(wpi::math::Pose2d{moduleTranslation, moduleHeading});
538 }
539 m_field2d.GetObject("modules")->SetPoses(modulePoses);
540 }
541
549 void SimIterate() {
550 if (!m_simTimer.IsRunning()) m_simTimer.Start();
551 for (auto* mod : m_config->GetModules()) {
552 mod->SimIterate();
553 }
554 auto desired = m_kinematics.ToChassisVelocities(m_desiredModuleStates);
555
556 auto dt = m_simTimer.Get();
557 wpi::math::Twist2d twist{desired.vx * dt, desired.vy * dt, desired.omega * dt};
558 m_simPose = m_simPose + twist.Exp();
559
560 auto speeds = m_kinematics.ToChassisVelocities(GetModuleStates());
561 m_simGyroAngle +=
562 wpi::units::degree_t{wpi::units::degrees_per_second_t{speeds.omega}.value() * dt.value()};
563 m_simTimer.Reset();
564 }
565
566 // ---- Queries ---------------------------------------------------------------
567
573 wpi::units::degree_t GetGyroAngle() {
574 if (wpi::RobotBase::IsSimulation()) return m_simGyroAngle;
575 return m_config->GetGyroAngle();
576 }
577
584 wpi::math::Rotation3d GetGyroRotation3d() {
585 if (wpi::RobotBase::IsSimulation()) {
586 return wpi::math::Rotation3d{wpi::units::radian_t{0}, wpi::units::radian_t{0},
587 wpi::units::radian_t{m_simGyroAngle}};
588 }
589 return m_config->GetGyroRotation3d();
590 }
591
597
602 if (m_swerveTelemetry) m_swerveTelemetry->ApplyTuningValues(this);
603 }
604
613 wpi::math::Pose2d GetSimPose() { return m_simPose; }
614
616 wpi::math::ChassisVelocities GetRobotRelativeSpeed() {
617 return m_kinematics.ToChassisVelocities(GetModuleStates());
618 }
619
621 wpi::math::ChassisVelocities GetFieldRelativeSpeed() {
622 return GetRobotRelativeSpeed().ToFieldRelative(
623 wpi::math::Rotation2d{wpi::units::radian_t{GetGyroAngle()}});
624 }
625
627 wpi::util::array<wpi::math::SwerveModulePosition, NumModules> GetModulePositions() {
628 wpi::util::array<wpi::math::SwerveModulePosition, NumModules> positions{wpi::util::empty_array};
629 for (size_t i = 0; i < NumModules; ++i) {
630 positions[i] = m_config->GetModules()[i]->GetPosition();
631 }
632 return positions;
633 }
634
636 wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> GetModuleStates() {
637 wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> states{wpi::util::empty_array};
638 for (size_t i = 0; i < NumModules; ++i) {
639 states[i] = m_config->GetModules()[i]->GetState();
640 }
641 return states;
642 }
643
650 wpi::units::meter_t GetDistanceFromPose(wpi::math::Pose2d pose) {
651 return wpi::units::meter_t{GetPose().Translation().Distance(pose.Translation())};
652 }
653
660 wpi::units::degree_t GetAngleDifferenceFromPose(wpi::math::Pose2d pose) {
661 return (GetPose() - pose).Rotation().Degrees();
662 }
663
670 std::optional<SwerveModule*> GetModule(const std::string& name) {
671 for (auto* mod : m_config->GetModules()) {
672 if (mod->GetName() == name) return mod;
673 }
674 return std::nullopt;
675 }
676
679 if (auto pid = m_config->GetRotationPID()) pid->get().Reset();
680 }
681
684 if (auto pid = m_config->GetTranslationPID()) pid->get().Reset();
685 }
686
692 void SetRotationPID(wpi::math::PIDController controller) {
693 auto currentRotationPID = m_config->GetRotationPID();
694 if (!currentRotationPID || currentRotationPID->get().GetP() != controller.GetP() ||
695 currentRotationPID->get().GetI() != controller.GetI() ||
696 currentRotationPID->get().GetD() != controller.GetD()) {
697 controller.Reset();
698 m_config->WithRotationController(std::move(controller));
699 }
700 }
701
707 void SetTranslationPID(wpi::math::PIDController controller) {
708 auto currentTranslationPID = m_config->GetTranslationPID();
709 if (!currentTranslationPID || currentTranslationPID->get().GetP() != controller.GetP() ||
710 currentTranslationPID->get().GetI() != controller.GetI() ||
711 currentTranslationPID->get().GetD() != controller.GetD()) {
712 controller.Reset();
713 m_config->WithTranslationController(std::move(controller));
714 }
715 }
716
722 void SetVisionMeasurementStdDevs(const wpi::util::array<double, 3>& stdDevs) {
723 m_poseEstimator.SetVisionMeasurementStdDevs(stdDevs);
724 }
725
727 std::string GetName() const { return m_config->GetTelemetryName(); }
728
730 SwerveDriveConfig& GetConfig() { return *m_config; }
731
733 wpi::math::SwerveDriveKinematics<NumModules>& GetKinematics() { return m_kinematics; }
734
739 wpi::Field2d& GetField2d() { return m_field2d; }
740
751 wpi::math::ChassisVelocities GetDesiredChassisSpeeds() { return m_desiredChassisSpeeds; }
752
759 wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> GetDesiredModuleStates() {
760 return m_desiredModuleStates;
761 }
762
763 private:
764 SwerveDriveConfig* m_config{nullptr};
765 wpi::math::SwerveDriveKinematics<NumModules> m_kinematics;
766 wpi::math::SwerveDrivePoseEstimator<NumModules> m_poseEstimator;
767
769 std::optional<telemetry::SwerveDriveTelemetry> m_swerveTelemetry;
770 std::optional<wpi::cmd::CommandPtr> m_driveToPoseTuningCommand;
771
772 wpi::Field2d m_field2d;
773 wpi::Timer m_simTimer;
774 wpi::units::degree_t m_simGyroAngle{0};
775 wpi::math::Pose2d m_simPose;
776
777 wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> m_desiredModuleStates{
778 wpi::util::empty_array};
779 wpi::math::ChassisVelocities m_desiredChassisSpeeds{};
780
786 void SetupTelemetry() {
788 telemetry::SwerveDriveTelemetryConfig telemetryCfg;
789 if (auto specified = m_config->GetSwerveDriveTelemetryConfig()) {
790 telemetryCfg = std::move(*specified);
791 } else {
792 telemetryCfg.WithTelemetryVerbosity(
793 m_config->GetTelemetryVerbosity().value_or(TelemetryVerbosity::HIGH));
794 }
795
796 if (auto dataLogName = telemetryCfg.GetDataLogName()) {
797 m_telemetry.SetupTelemetry(GetName(), *dataLogName);
798 } else {
799 m_telemetry.SetupTelemetry(GetName());
800 }
801
802 m_swerveTelemetry.emplace(std::move(telemetryCfg));
803 m_swerveTelemetry->SetupTelemetry(this);
804
805 // Drive to pose tuning needs both controllers, so only offer it when they are configured.
806 if (m_config->GetTranslationPID() && m_config->GetRotationPID()) {
807 auto tuningCommand = wpi::cmd::StartRun(
808 [this] {
809 std::puts(
810 "================= Starting SwerveDrive.driveToPoseTuning() =================\n");
813 },
814 [this] { m_swerveTelemetry->ApplyTuningValues(this); },
815 // Requires the drive, so the default drive command does not undo its setpoints.
816 {m_config->GetSubsystem()});
817 // Stop any tuning voltage when the command ends.
818 m_driveToPoseTuningCommand.emplace(
819 std::move(tuningCommand)
820 .FinallyDo([this] { m_swerveTelemetry->StopTuning(this); })
821 .WithName("Live Tuning"));
823 wpi::tunables::Publish("Tuning/" + GetName() + "/Live Tuning",
824 *m_driveToPoseTuningCommand->get());
825 }
826 }
827
828 void UpdatePoseEstimator() {
829 m_poseEstimator.Update(wpi::math::Rotation2d{wpi::units::radian_t{GetGyroAngle()}},
831 }
832
833 static wpi::math::SwerveDriveKinematics<NumModules> BuildKinematics(
834 const SwerveDriveConfig& config) {
835 assert(config.GetModules().size() == NumModules);
836 wpi::util::array<wpi::math::Translation2d, NumModules> locations{wpi::util::empty_array};
837 for (size_t i = 0; i < NumModules; ++i) {
838 locations[i] = *config.GetModules()[i]->GetConfig().GetLocation();
839 }
840 return wpi::math::SwerveDriveKinematics<NumModules>{locations};
841 }
842
847 void SetGyroAngle(wpi::units::degree_t heading) {
848 if (wpi::RobotBase::IsSimulation()) {
849 m_simGyroAngle = heading;
850 return;
851 }
852 m_config->WithGyroOffset(GetGyroAngle() + m_config->GetGyroOffset() - heading);
853 }
854
855 static wpi::math::Rotation2d ComputeInitialRotation(const SwerveDriveConfig& config) {
856 if (wpi::RobotBase::IsSimulation()) return wpi::math::Rotation2d{};
857 return wpi::math::Rotation2d{wpi::units::radian_t{config.GetGyroAngle()}};
858 }
859
860 static wpi::util::array<wpi::math::SwerveModulePosition, NumModules> ComputeInitialPositions(
861 const SwerveDriveConfig& config) {
862 wpi::util::array<wpi::math::SwerveModulePosition, NumModules> positions{wpi::util::empty_array};
863 for (size_t i = 0; i < NumModules; ++i) {
864 positions[i] = config.GetModules()[i]->GetPosition();
865 }
866 return positions;
867 }
868};
869
870} // namespace yams::mechanisms::swerve
871
873
874// SwerveDriveTelemetry::SetupTelemetry()/Publish()/ApplyTuningValues() are templated on the
875// SwerveDrive's module count so that SwerveDriveTelemetry itself need not be a template. Their
876// bodies live here after SwerveDrive<NumModules> is fully defined above because this is the
877// only place both SwerveDriveTelemetry's private members and SwerveDrive<NumModules>'s interface
878// are simultaneously visible.
879
880template <std::size_t NumModules>
882 auto driveName = drive->GetName();
883 for (auto* module : drive->GetConfig().GetModules()) {
884 module->SetupTelemetry(driveName);
885 }
886
887 auto inst = wpi::nt::NetworkTableInstance::GetDefault();
888 m_dataTable = inst.GetTable("Mechanisms")->GetSubTable(driveName);
889 m_tuningTable = inst.GetTable("Tuning")->GetSubTable(driveName);
890
891 bool nt4Enabled = m_config.GetNT4Enabled();
892 auto dataLogName = m_config.GetDataLogName();
893
894 {
895 // Only publish the drive to pose gains and auto-align fields for controllers that exist.
896 auto translationPID = drive->GetConfig().GetTranslationPID();
897 auto rotationPID = drive->GetConfig().GetRotationPID();
898 auto& doubleFields = m_config.GetDoubleFields();
899 if (translationPID) {
900 doubleFields.at(DoubleTelemetryField::TranslationP)
901 .SetDefaultValue(translationPID->get().GetP());
902 doubleFields.at(DoubleTelemetryField::TranslationI)
903 .SetDefaultValue(translationPID->get().GetI());
904 doubleFields.at(DoubleTelemetryField::TranslationD)
905 .SetDefaultValue(translationPID->get().GetD());
906 } else {
907 doubleFields.at(DoubleTelemetryField::TranslationP).Disable();
908 doubleFields.at(DoubleTelemetryField::TranslationI).Disable();
909 doubleFields.at(DoubleTelemetryField::TranslationD).Disable();
910 }
911 if (rotationPID) {
912 doubleFields.at(DoubleTelemetryField::RotationP).SetDefaultValue(rotationPID->get().GetP());
913 doubleFields.at(DoubleTelemetryField::RotationI).SetDefaultValue(rotationPID->get().GetI());
914 doubleFields.at(DoubleTelemetryField::RotationD).SetDefaultValue(rotationPID->get().GetD());
915 } else {
916 doubleFields.at(DoubleTelemetryField::RotationP).Disable();
917 doubleFields.at(DoubleTelemetryField::RotationI).Disable();
918 doubleFields.at(DoubleTelemetryField::RotationD).Disable();
919 }
920 // Auto-align drives to a pose, which needs both controllers.
921 if (!translationPID || !rotationPID) {
922 doubleFields.at(DoubleTelemetryField::AutoAlignPoseX).Disable();
923 doubleFields.at(DoubleTelemetryField::AutoAlignPoseY).Disable();
924 doubleFields.at(DoubleTelemetryField::AutoAlignPoseRotation).Disable();
925 m_config.GetBoolFields().at(BooleanTelemetryField::AutoAlignEnabled).Disable();
926 }
927
928 auto& modules = drive->GetConfig().GetModules();
929 if (!modules.empty()) {
930 auto* driveMotor = modules[0]->GetDriveMotorController();
931 auto driveGains =
932 driveMotor->GetConfig().GetSlotGains(driveMotor->GetClosedLoopControllerSlot());
933 m_config.GetDoubleFields()
934 .at(DoubleTelemetryField::ModulesDriveP)
935 .SetDefaultValue(driveGains.kP);
936 m_config.GetDoubleFields()
937 .at(DoubleTelemetryField::ModulesDriveI)
938 .SetDefaultValue(driveGains.kI);
939 m_config.GetDoubleFields()
940 .at(DoubleTelemetryField::ModulesDriveD)
941 .SetDefaultValue(driveGains.kD);
942 m_config.GetDoubleFields()
943 .at(DoubleTelemetryField::ModulesDriveKs)
944 .SetDefaultValue(driveGains.kS);
945 m_config.GetDoubleFields()
946 .at(DoubleTelemetryField::ModulesDriveKv)
947 .SetDefaultValue(driveGains.kV);
948 m_config.GetDoubleFields()
949 .at(DoubleTelemetryField::ModulesDriveKa)
950 .SetDefaultValue(driveGains.kA);
951
952 auto* azimuthMotor = modules[0]->GetAzimuthMotorController();
953 auto azimuthGains =
954 azimuthMotor->GetConfig().GetSlotGains(azimuthMotor->GetClosedLoopControllerSlot());
955 m_config.GetDoubleFields()
956 .at(DoubleTelemetryField::ModulesAzimuthP)
957 .SetDefaultValue(azimuthGains.kP);
958 m_config.GetDoubleFields()
959 .at(DoubleTelemetryField::ModulesAzimuthI)
960 .SetDefaultValue(azimuthGains.kI);
961 m_config.GetDoubleFields()
962 .at(DoubleTelemetryField::ModulesAzimuthD)
963 .SetDefaultValue(azimuthGains.kD);
964 m_config.GetDoubleFields()
965 .at(DoubleTelemetryField::ModulesAzimuthKs)
966 .SetDefaultValue(azimuthGains.kS);
967 m_config.GetDoubleFields()
968 .at(DoubleTelemetryField::ModulesAzimuthKv)
969 .SetDefaultValue(azimuthGains.kV);
970 m_config.GetDoubleFields()
971 .at(DoubleTelemetryField::ModulesAzimuthKa)
972 .SetDefaultValue(azimuthGains.kA);
973 }
974 }
975
976 for (auto& [field, dt] : m_config.GetDoubleFields()) {
977 if (!dt.IsEnabled()) continue;
978 if (nt4Enabled) dt.SetupNetworkTables(m_dataTable, m_tuningTable);
979 if (dataLogName) dt.SetupDataLog(*dataLogName);
980 }
981 for (auto& [field, bt] : m_config.GetBoolFields()) {
982 if (!bt.IsEnabled()) continue;
983 if (nt4Enabled) bt.SetupNetworkTables(m_dataTable, m_tuningTable);
984 if (dataLogName) bt.SetupDataLog(*dataLogName);
985 }
986 for (auto& [field, stt] : m_config.GetPoseFields()) {
987 if (!stt.IsEnabled()) continue;
988 if (nt4Enabled) stt.SetupNetworkTables(m_dataTable, m_tuningTable);
989 if (dataLogName) stt.SetupDataLog(*dataLogName);
990 }
991 for (auto& [field, stt] : m_config.GetChassisSpeedsFields()) {
992 if (!stt.IsEnabled()) continue;
993 if (nt4Enabled) stt.SetupNetworkTables(m_dataTable, m_tuningTable);
994 if (dataLogName) stt.SetupDataLog(*dataLogName);
995 }
996 for (auto& [field, stat] : m_config.GetModuleStatesFields()) {
997 if (!stat.IsEnabled()) continue;
998 if (nt4Enabled) stat.SetupNetworkTables(m_dataTable, m_tuningTable);
999 if (dataLogName) stat.SetupDataLog(*dataLogName);
1000 }
1001}
1002
1003template <std::size_t NumModules>
1005 for (auto& [field, stt] : m_config.GetPoseFields()) {
1006 if (!stt.IsEnabled()) continue;
1007 switch (field) {
1008 case StructTelemetryField::Pose:
1009 stt.Set(drive->GetPose());
1010 break;
1011 default:
1012 break;
1013 }
1014 }
1015 for (auto& [field, stt] : m_config.GetChassisSpeedsFields()) {
1016 if (!stt.IsEnabled()) continue;
1017 switch (field) {
1018 case StructTelemetryField::DesiredRobotRelativeChassisSpeeds:
1019 stt.Set(drive->GetDesiredChassisSpeeds());
1020 break;
1021 case StructTelemetryField::CurrentRobotRelativeChassisSpeeds:
1022 stt.Set(drive->GetRobotRelativeSpeed());
1023 break;
1024 case StructTelemetryField::FieldRelativeChassisSpeeds:
1025 stt.Set(drive->GetFieldRelativeSpeed());
1026 break;
1027 default:
1028 break;
1029 }
1030 }
1031 for (auto& [field, dt] : m_config.GetDoubleFields()) {
1032 if (!dt.IsEnabled()) continue;
1033 switch (field) {
1034 case DoubleTelemetryField::Gyro:
1035 dt.Set(drive->GetGyroAngle().value());
1036 break;
1037 default:
1038 break;
1039 }
1040 }
1041 for (auto& [field, stat] : m_config.GetModuleStatesFields()) {
1042 if (!stat.IsEnabled()) continue;
1043 switch (field) {
1044 case StructArrayTelemetryField::DesiredModuleStates: {
1045 auto states = drive->GetDesiredModuleStates();
1046 stat.Set(std::vector<wpi::math::SwerveModuleVelocity>(states.begin(), states.end()));
1047 break;
1048 }
1049 case StructArrayTelemetryField::CurrentModuleStates: {
1050 auto states = drive->GetModuleStates();
1051 stat.Set(std::vector<wpi::math::SwerveModuleVelocity>(states.begin(), states.end()));
1052 break;
1053 }
1054 default:
1055 break;
1056 }
1057 }
1058}
1059
1060template <std::size_t NumModules>
1062 if (!m_config.GetNT4Enabled()) return;
1063 auto& modules = drive->GetConfig().GetModules();
1064 auto& boolFields = m_config.GetBoolFields();
1065 auto& doubleFields = m_config.GetDoubleFields();
1066 // Copies of the configured controllers; gain fields for missing controllers are disabled.
1067 std::optional<wpi::math::PIDController> translationPID;
1068 if (auto pid = drive->GetConfig().GetTranslationPID()) translationPID = pid->get();
1069 std::optional<wpi::math::PIDController> rotationPID;
1070 if (auto pid = drive->GetConfig().GetRotationPID()) rotationPID = pid->get();
1071
1072 // Read which tuning modes are on. Only one may drive the chassis at a time: auto-align, then
1073 // drive tuning, then azimuth tuning. A mode that is on turns the ones it outranks back off, in
1074 // NetworkTables too so the dashboard reflects what's actually happening, so the order the fields
1075 // are read in does not matter. Auto-align fields are disabled when either drive to pose
1076 // controller is missing.
1077 std::set<TuningMode> modes;
1078 for (auto& [field, bt] : boolFields) {
1079 if (!bt.IsEnabled() || !bt.Get()) continue;
1080 switch (field) {
1081 case BooleanTelemetryField::AutoAlignEnabled:
1082 modes.insert(TuningMode::AutoAlign);
1083 modes.erase(TuningMode::Drive);
1084 modes.erase(TuningMode::Azimuth);
1085 boolFields.at(BooleanTelemetryField::ModulesDriveTuningEnabled).ForceSet(false);
1086 boolFields.at(BooleanTelemetryField::ModulesAzimuthTuningEnabled).ForceSet(false);
1087 break;
1088 case BooleanTelemetryField::ModulesDriveTuningEnabled:
1089 if (!modes.contains(TuningMode::AutoAlign)) {
1090 modes.insert(TuningMode::Drive);
1091 modes.erase(TuningMode::Azimuth);
1092 boolFields.at(BooleanTelemetryField::ModulesAzimuthTuningEnabled).ForceSet(false);
1093 }
1094 break;
1095 case BooleanTelemetryField::ModulesDriveInPlace:
1096 modes.insert(TuningMode::DriveInPlace);
1097 break;
1098 case BooleanTelemetryField::ModulesDriveVoltageEnabled:
1099 modes.insert(TuningMode::DriveVoltage);
1100 break;
1101 case BooleanTelemetryField::ModulesAzimuthTuningEnabled:
1102 if (!modes.contains(TuningMode::AutoAlign) && !modes.contains(TuningMode::Drive)) {
1103 modes.insert(TuningMode::Azimuth);
1104 }
1105 break;
1106 case BooleanTelemetryField::ModulesAzimuthVoltageEnabled:
1107 modes.insert(TuningMode::AzimuthVoltage);
1108 break;
1109 }
1110 }
1111 // Spinning in place and a voltage only apply while their tuning mode is on.
1112 if (!modes.contains(TuningMode::Drive)) {
1113 modes.erase(TuningMode::DriveInPlace);
1114 modes.erase(TuningMode::DriveVoltage);
1115 }
1116 if (!modes.contains(TuningMode::Azimuth)) modes.erase(TuningMode::AzimuthVoltage);
1117 // The angle each module points while drive tuning: forward, or, to spin in place, tangent to its
1118 // position around the robot center so a positive velocity spins the robot counter-clockwise
1119 // (WPILib's positive rotation direction).
1120 wpi::util::array<wpi::math::Rotation2d, NumModules> driveAngles{wpi::util::empty_array};
1121 for (size_t i = 0; i < NumModules; ++i) {
1122 driveAngles[i] = wpi::math::Rotation2d{};
1123 if (modes.contains(TuningMode::DriveInPlace)) {
1124 auto moduleLocation = *modules[i]->GetConfig().GetLocation();
1125 driveAngles[i] = moduleLocation.Angle().value_or(wpi::math::Rotation2d{}) +
1126 wpi::math::Rotation2d{wpi::units::degree_t{90}};
1127 }
1128 }
1129
1130 // Auto-align drives to its target pose every loop.
1131 if (modes.contains(TuningMode::AutoAlign)) {
1132 wpi::math::Pose2d targetPose{
1133 wpi::units::meter_t{doubleFields.at(DoubleTelemetryField::AutoAlignPoseX).Get()},
1134 wpi::units::meter_t{doubleFields.at(DoubleTelemetryField::AutoAlignPoseY).Get()},
1135 wpi::math::Rotation2d{
1136 wpi::units::degree_t{doubleFields.at(DoubleTelemetryField::AutoAlignPoseRotation).Get()}}};
1137 drive->SetRobotRelativeChassisSpeeds(drive->DriveToPoseSetpoint(targetPose));
1138 }
1139
1140 // When the tuning modes change, switch the motors over, and send the setpoints of the modes now
1141 // on.
1142 if (modes != m_lastModes) {
1143 const std::set<TuningMode> lastModes = m_lastModes.value_or(std::set<TuningMode>{});
1144 m_lastModes = modes;
1145 // Back to the closed loops when a voltage turns off.
1146 for (auto* module : modules) {
1147 if (lastModes.contains(TuningMode::DriveVoltage) && !modes.contains(TuningMode::DriveVoltage)) {
1148 module->GetDriveMotorController()->StartClosedLoopController();
1149 }
1150 if (lastModes.contains(TuningMode::AzimuthVoltage) &&
1151 !modes.contains(TuningMode::AzimuthVoltage)) {
1152 module->GetAzimuthMotorController()->StartClosedLoopController();
1153 }
1154 }
1155 wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> states{wpi::util::empty_array};
1156 if (modes.empty()) {
1157 // No tuning mode is driving the chassis; make sure the modules don't keep spinning at
1158 // whatever velocity was last commanded while drive tuning was on.
1159 for (size_t i = 0; i < NumModules; ++i) {
1160 states[i] = wpi::math::SwerveModuleVelocity{wpi::units::meters_per_second_t{0},
1161 modules[i]->GetState().angle};
1162 }
1163 drive->SetSwerveModuleStates(states);
1164 }
1165 for (auto mode : modes) {
1166 switch (mode) {
1167 case TuningMode::AutoAlign:
1168 // Driven every loop, above.
1169 [[fallthrough]];
1170 case TuningMode::DriveInPlace:
1171 // Sent with drive tuning's setpoint.
1172 break;
1173 case TuningMode::Drive:
1174 if (!modes.contains(TuningMode::DriveVoltage)) {
1175 for (size_t i = 0; i < NumModules; ++i) {
1176 states[i] = wpi::math::SwerveModuleVelocity{
1177 wpi::units::meters_per_second_t{
1178 doubleFields.at(DoubleTelemetryField::ModulesDriveVelocity).Get()},
1179 driveAngles[i]};
1180 }
1181 drive->SetSwerveModuleStates(states);
1182 }
1183 break;
1184 case TuningMode::DriveVoltage:
1185 for (size_t i = 0; i < NumModules; ++i) {
1186 // A closed loop run on the SystemCore would replace the voltage on its next loop.
1187 if (!lastModes.contains(TuningMode::DriveVoltage)) {
1188 modules[i]->GetDriveMotorController()->StopClosedLoopController();
1189 }
1190 // The wheels are not optimized, so they always drive the same way.
1191 modules[i]->GetDriveMotorController()->SetVoltage(
1192 wpi::units::volt_t{doubleFields.at(DoubleTelemetryField::ModulesDriveVoltage).Get()});
1193 modules[i]->GetAzimuthMotorController()->SetPosition(
1194 wpi::units::degree_t{driveAngles[i].Degrees()});
1195 }
1196 break;
1197 case TuningMode::Azimuth:
1198 if (!modes.contains(TuningMode::AzimuthVoltage)) {
1199 // Zero speed states would be held where the wheels are by the velocity deadband, so turn
1200 // the wheels without driving them.
1201 drive->SetAzimuthAngles(wpi::math::Rotation2d{wpi::units::degree_t{
1202 doubleFields.at(DoubleTelemetryField::ModulesAzimuthAngle).Get()}});
1203 }
1204 break;
1205 case TuningMode::AzimuthVoltage:
1206 for (auto* module : modules) {
1207 // A closed loop run on the SystemCore would replace the voltage on its next loop.
1208 if (!lastModes.contains(TuningMode::AzimuthVoltage)) {
1209 module->GetAzimuthMotorController()->StopClosedLoopController();
1210 }
1211 // Hold the drive motors still while the azimuth motors turn.
1212 module->GetDriveMotorController()->SetVelocity(wpi::units::meters_per_second_t{0} +
1213 module->GetCouplingVelocity());
1214 module->GetAzimuthMotorController()->SetVoltage(wpi::units::volt_t{
1215 doubleFields.at(DoubleTelemetryField::ModulesAzimuthVoltage).Get()});
1216 }
1217 break;
1218 }
1219 }
1220 }
1221
1222 // Set every gain and setpoint that changed.
1223 for (auto field : {DoubleTelemetryField::TranslationP,
1224 DoubleTelemetryField::TranslationI,
1225 DoubleTelemetryField::TranslationD,
1226 DoubleTelemetryField::RotationP,
1227 DoubleTelemetryField::RotationI,
1228 DoubleTelemetryField::RotationD,
1229 DoubleTelemetryField::AutoAlignPoseX,
1230 DoubleTelemetryField::AutoAlignPoseY,
1231 DoubleTelemetryField::AutoAlignPoseRotation,
1232 DoubleTelemetryField::ModulesDriveP,
1233 DoubleTelemetryField::ModulesDriveI,
1234 DoubleTelemetryField::ModulesDriveD,
1235 DoubleTelemetryField::ModulesDriveKs,
1236 DoubleTelemetryField::ModulesDriveKv,
1237 DoubleTelemetryField::ModulesDriveKa,
1238 DoubleTelemetryField::ModulesDriveVelocity,
1239 DoubleTelemetryField::ModulesDriveVoltage,
1240 DoubleTelemetryField::ModulesAzimuthP,
1241 DoubleTelemetryField::ModulesAzimuthI,
1242 DoubleTelemetryField::ModulesAzimuthD,
1243 DoubleTelemetryField::ModulesAzimuthKs,
1244 DoubleTelemetryField::ModulesAzimuthKv,
1245 DoubleTelemetryField::ModulesAzimuthKa,
1246 DoubleTelemetryField::ModulesAzimuthAngle,
1247 DoubleTelemetryField::ModulesAzimuthVoltage}) {
1248 auto& dt = doubleFields.at(field);
1249 if (!dt.IsEnabled() || !dt.IsTunable()) continue;
1250 switch (field) {
1251 case DoubleTelemetryField::TranslationP:
1252 if (translationPID) {
1253 translationPID->SetP(dt.Get());
1254 drive->SetTranslationPID(*translationPID);
1255 }
1256 break;
1257 case DoubleTelemetryField::TranslationI:
1258 if (translationPID) {
1259 translationPID->SetI(dt.Get());
1260 drive->SetTranslationPID(*translationPID);
1261 }
1262 break;
1263 case DoubleTelemetryField::TranslationD:
1264 if (translationPID) {
1265 translationPID->SetD(dt.Get());
1266 drive->SetTranslationPID(*translationPID);
1267 }
1268 break;
1269 case DoubleTelemetryField::RotationP:
1270 if (rotationPID) {
1271 rotationPID->SetP(dt.Get());
1272 drive->SetRotationPID(*rotationPID);
1273 }
1274 break;
1275 case DoubleTelemetryField::RotationI:
1276 if (rotationPID) {
1277 rotationPID->SetI(dt.Get());
1278 drive->SetRotationPID(*rotationPID);
1279 }
1280 break;
1281 case DoubleTelemetryField::RotationD:
1282 if (rotationPID) {
1283 rotationPID->SetD(dt.Get());
1284 drive->SetRotationPID(*rotationPID);
1285 }
1286 break;
1287 case DoubleTelemetryField::AutoAlignPoseX:
1288 case DoubleTelemetryField::AutoAlignPoseY:
1289 case DoubleTelemetryField::AutoAlignPoseRotation:
1290 // Auto-align reads its target pose every loop, above.
1291 break;
1292 case DoubleTelemetryField::ModulesDriveP:
1293 for (auto* module : modules) module->GetDriveMotorController()->SetKp(dt.Get());
1294 break;
1295 case DoubleTelemetryField::ModulesDriveI:
1296 for (auto* module : modules) module->GetDriveMotorController()->SetKi(dt.Get());
1297 break;
1298 case DoubleTelemetryField::ModulesDriveD:
1299 for (auto* module : modules) module->GetDriveMotorController()->SetKd(dt.Get());
1300 break;
1301 case DoubleTelemetryField::ModulesDriveKs:
1302 for (auto* module : modules) module->GetDriveMotorController()->SetKs(dt.Get());
1303 break;
1304 case DoubleTelemetryField::ModulesDriveKv:
1305 for (auto* module : modules) module->GetDriveMotorController()->SetKv(dt.Get());
1306 break;
1307 case DoubleTelemetryField::ModulesDriveKa:
1308 for (auto* module : modules) module->GetDriveMotorController()->SetKa(dt.Get());
1309 break;
1310 case DoubleTelemetryField::ModulesAzimuthP:
1311 for (auto* module : modules) module->GetAzimuthMotorController()->SetKp(dt.Get());
1312 break;
1313 case DoubleTelemetryField::ModulesAzimuthI:
1314 for (auto* module : modules) module->GetAzimuthMotorController()->SetKi(dt.Get());
1315 break;
1316 case DoubleTelemetryField::ModulesAzimuthD:
1317 for (auto* module : modules) module->GetAzimuthMotorController()->SetKd(dt.Get());
1318 break;
1319 case DoubleTelemetryField::ModulesAzimuthKs:
1320 for (auto* module : modules) module->GetAzimuthMotorController()->SetKs(dt.Get());
1321 break;
1322 case DoubleTelemetryField::ModulesAzimuthKv:
1323 for (auto* module : modules) module->GetAzimuthMotorController()->SetKv(dt.Get());
1324 break;
1325 case DoubleTelemetryField::ModulesAzimuthKa:
1326 for (auto* module : modules) module->GetAzimuthMotorController()->SetKa(dt.Get());
1327 break;
1328 case DoubleTelemetryField::ModulesDriveVelocity:
1329 if (modes.contains(TuningMode::Drive) && !modes.contains(TuningMode::DriveVoltage)) {
1330 wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> states{
1331 wpi::util::empty_array};
1332 for (size_t i = 0; i < NumModules; ++i) {
1333 states[i] = wpi::math::SwerveModuleVelocity{wpi::units::meters_per_second_t{dt.Get()},
1334 driveAngles[i]};
1335 }
1336 drive->SetSwerveModuleStates(states);
1337 }
1338 break;
1339 case DoubleTelemetryField::ModulesDriveVoltage:
1340 if (modes.contains(TuningMode::DriveVoltage)) {
1341 for (size_t i = 0; i < NumModules; ++i) {
1342 // The wheels are not optimized, so they always drive the same way.
1343 modules[i]->GetDriveMotorController()->SetVoltage(wpi::units::volt_t{dt.Get()});
1344 modules[i]->GetAzimuthMotorController()->SetPosition(
1345 wpi::units::degree_t{driveAngles[i].Degrees()});
1346 }
1347 }
1348 break;
1349 case DoubleTelemetryField::ModulesAzimuthAngle:
1350 if (modes.contains(TuningMode::Azimuth) && !modes.contains(TuningMode::AzimuthVoltage)) {
1351 // Zero speed states would be held where the wheels are by the velocity deadband, so turn
1352 // the wheels without driving them.
1353 drive->SetAzimuthAngles(wpi::math::Rotation2d{wpi::units::degree_t{dt.Get()}});
1354 }
1355 break;
1356 case DoubleTelemetryField::ModulesAzimuthVoltage:
1357 if (modes.contains(TuningMode::AzimuthVoltage)) {
1358 for (auto* module : modules) {
1359 // Hold the drive motors still while the azimuth motors turn.
1360 module->GetDriveMotorController()->SetVelocity(wpi::units::meters_per_second_t{0} +
1361 module->GetCouplingVelocity());
1362 module->GetAzimuthMotorController()->SetVoltage(wpi::units::volt_t{dt.Get()});
1363 }
1364 }
1365 break;
1366 default:
1367 break;
1368 }
1369 }
1370}
1371
1372template <std::size_t NumModules>
1374 // Turn every tuning mode off and apply it, so a voltage stops and the wheels are held where they
1375 // are.
1376 for (auto& [field, bt] : m_config.GetBoolFields()) {
1377 if (bt.IsEnabled()) bt.ForceSet(false);
1378 }
1379 ApplyTuningValues(drive);
1380 // Send everything again when tuning starts again.
1381 m_lastModes.reset();
1382}
1383
1384} // namespace yams::telemetry
static void Show()
Definition SwerveDriveConfig.hpp:38
const std::vector< SwerveModule * > & GetModules() const
std::optional< std::reference_wrapper< wpi::math::PIDController > > GetRotationPID()
std::optional< std::reference_wrapper< wpi::math::PIDController > > GetTranslationPID()
Definition SwerveDrive.hpp:138
wpi::cmd::CommandPtr DriveToPose(wpi::math::Pose2d pose)
Definition SwerveDrive.hpp:197
void AddVisionMeasurement(wpi::math::Pose2d robotPose, wpi::units::second_t timestamp, const wpi::util::array< double, 3 > &stdDevs)
Definition SwerveDrive.hpp:505
wpi::Field2d & GetField2d()
Definition SwerveDrive.hpp:739
wpi::math::ChassisVelocities GetRobotRelativeChassisSpeedsFromState(const wpi::util::array< wpi::math::SwerveModuleVelocity, NumModules > &states)
Definition SwerveDrive.hpp:379
void ZeroGyro()
Definition SwerveDrive.hpp:483
wpi::math::ChassisVelocities GetFieldRelativeSpeed()
Definition SwerveDrive.hpp:621
void SetFieldRelativeChassisSpeeds(wpi::math::ChassisVelocities fieldRelativeSpeeds)
Definition SwerveDrive.hpp:422
wpi::math::Rotation3d GetGyroRotation3d()
Definition SwerveDrive.hpp:584
wpi::util::array< wpi::math::SwerveModuleVelocity, NumModules > GetDesiredModuleStates()
Definition SwerveDrive.hpp:759
wpi::util::array< wpi::math::SwerveModuleVelocity, NumModules > GetModuleStates()
Definition SwerveDrive.hpp:636
void AddVisionMeasurement(wpi::math::Pose2d robotPose, wpi::units::second_t timestamp)
Definition SwerveDrive.hpp:494
void SetSwerveModuleStates(wpi::util::array< wpi::math::SwerveModuleVelocity, NumModules > states, std::span< const wpi::units::newton_t > feedforwardForces)
Definition SwerveDrive.hpp:337
wpi::cmd::CommandPtr Drive(std::function< wpi::math::ChassisVelocities()> robotRelativeSpeeds)
Definition SwerveDrive.hpp:179
wpi::math::ChassisVelocities DriveToPoseSetpoint(wpi::math::Pose2d targetPose, wpi::math::Pose2d currentPose, wpi::math::Rotation2d heading)
Definition SwerveDrive.hpp:282
wpi::math::Pose2d GetSimPose()
Definition SwerveDrive.hpp:613
void ApplyTuningValues()
Definition SwerveDrive.hpp:601
void ResetOdometry(wpi::math::Pose2d pose)
Definition SwerveDrive.hpp:471
void SetAzimuthAngles(wpi::math::Rotation2d angle)
Definition SwerveDrive.hpp:450
wpi::util::array< wpi::math::SwerveModuleVelocity, NumModules > GetStateFromRobotRelativeChassisSpeeds(wpi::math::ChassisVelocities robotRelativeSpeeds)
Definition SwerveDrive.hpp:352
wpi::units::degree_t GetGyroAngle()
Definition SwerveDrive.hpp:573
void SimIterate()
Definition SwerveDrive.hpp:549
wpi::math::ChassisVelocities GetDesiredChassisSpeeds()
Definition SwerveDrive.hpp:751
void UpdateTelemetry()
Definition SwerveDrive.hpp:517
wpi::math::Pose2d GetPose()
Definition SwerveDrive.hpp:462
wpi::math::SwerveDriveKinematics< NumModules > & GetKinematics()
Definition SwerveDrive.hpp:733
wpi::units::degree_t GetAngleDifferenceFromPose(wpi::math::Pose2d pose)
Definition SwerveDrive.hpp:660
void LockPose()
Definition SwerveDrive.hpp:431
void Stop()
Definition SwerveDrive.hpp:253
void StartDriveToPoseTuning()
Definition SwerveDrive.hpp:593
void SetRotationPID(wpi::math::PIDController controller)
Definition SwerveDrive.hpp:692
void SetTranslationPID(wpi::math::PIDController controller)
Definition SwerveDrive.hpp:707
SwerveDriveConfig & GetConfig()
Definition SwerveDrive.hpp:730
std::string GetName() const
Definition SwerveDrive.hpp:727
wpi::cmd::CommandPtr DriveToPose(wpi::math::Pose2d pose, wpi::units::meter_t translationTolerance, wpi::units::degree_t rotationTolerance)
Definition SwerveDrive.hpp:221
wpi::util::array< wpi::math::SwerveModulePosition, NumModules > GetModulePositions()
Definition SwerveDrive.hpp:627
SwerveDrive(SwerveDriveConfig *config)
Definition SwerveDrive.hpp:145
wpi::math::ChassisVelocities DriveToPoseSetpoint(wpi::math::Pose2d targetPose)
Definition SwerveDrive.hpp:266
void ResetTranslationPID()
Definition SwerveDrive.hpp:683
std::optional< SwerveModule * > GetModule(const std::string &name)
Definition SwerveDrive.hpp:670
bool IsNear(wpi::math::Pose2d pose, wpi::units::meter_t translationTolerance, wpi::units::degree_t rotationTolerance)
Definition SwerveDrive.hpp:246
void SetVisionMeasurementStdDevs(const wpi::util::array< double, 3 > &stdDevs)
Definition SwerveDrive.hpp:722
wpi::math::ChassisVelocities GetRobotRelativeSpeed()
Definition SwerveDrive.hpp:616
wpi::units::meter_t GetDistanceFromPose(wpi::math::Pose2d pose)
Definition SwerveDrive.hpp:650
void SetRobotRelativeChassisSpeeds(wpi::math::ChassisVelocities robotRelativeSpeeds)
Definition SwerveDrive.hpp:389
void SetRobotRelativeChassisSpeeds(wpi::math::ChassisVelocities robotRelativeSpeeds, std::span< const wpi::units::newton_t > feedforwardForces)
Definition SwerveDrive.hpp:402
void SetSwerveModuleStates(wpi::util::array< wpi::math::SwerveModuleVelocity, NumModules > states)
Definition SwerveDrive.hpp:325
void ResetRotationPID()
Definition SwerveDrive.hpp:678
TelemetryVerbosity
Definition SmartMotorControllerConfig.hpp:73
Definition MechanismTelemetry.hpp:34
void SetupTelemetry(const std::string &mechanismName, motorcontrollers::SmartMotorController &motorController)
void Publish(mechanisms::swerve::SwerveDrive< NumModules > *drive)
Definition SwerveDrive.hpp:1004
void SetupTelemetry(mechanisms::swerve::SwerveDrive< NumModules > *drive)
Definition SwerveDrive.hpp:881
void StopTuning(mechanisms::swerve::SwerveDrive< NumModules > *drive)
Definition SwerveDrive.hpp:1373
void ApplyTuningValues(mechanisms::swerve::SwerveDrive< NumModules > *drive)
Definition SwerveDrive.hpp:1061
Definition ArmConfig.hpp:17
Definition SwerveDrive.hpp:51
Definition SwerveDrive.hpp:872
void EnsureMechanismsTunableBackend()
Definition NetworkTablesBackends.hpp:43
void EnsureTuningTunableBackend()
Definition NetworkTablesBackends.hpp:57