SparkWrapper.hpp

Go to the documentation of this file.

// Copyright (c) 2026 Yet Another Software Suite
// SPDX-License-Identifier: LGPL-3.0-or-later
 
#pragma once
 
#include <rev/DetachedEncoder.h>
#include <rev/RelativeEncoder.h>
#include <rev/SparkAbsoluteEncoder.h>
#include <rev/SparkClosedLoopController.h>
#include <rev/SparkFlex.h>
#include <rev/SparkFlexExternalEncoder.h>
#include <rev/SparkMax.h>
#include <rev/SparkMaxAlternateEncoder.h>
#include <rev/SparkRelativeEncoder.h>
#include <rev/SparkSim.h>
#include <rev/config/SparkFlexConfig.h>
#include <rev/config/SparkMaxConfig.h>
#include <rev/sim/SparkAbsoluteEncoderSim.h>
#include <rev/sim/SparkExternalEncoderSim.h>
#include <rev/sim/SparkMaxAlternateEncoderSim.h>
#include <rev/sim/SparkRelativeEncoderSim.h>
 
#include <functional>
#include <memory>
#include <optional>
#include <string>
#include <wpi/math/trajectory/TrapezoidProfile.hpp>
#include <wpi/simulation/DCMotorSim.hpp>
#include <wpi/util/Alert.hpp>
 
#include "yams/math/DerivativeTimeFilter.hpp"
#include "yams/motorcontrollers/SmartMotorController.hpp"
 
namespace yams::motorcontrollers::local {
 
class SparkWrapper : public SmartMotorController {
 public:
  // Expose SetVelocity(velocity, feedforwardForce) from the base class alongside the overrides.
  using SmartMotorController::SetVelocity;
 
  SparkWrapper(rev::spark::SparkMax* spark, wpi::math::DCMotor motor,
               SmartMotorControllerConfig* config);
 
  SparkWrapper(rev::spark::SparkFlex* spark, wpi::math::DCMotor motor,
               SmartMotorControllerConfig* config);
  ~SparkWrapper();
 
  // ---- Telemetry ----------------------------------------------------------
  telemetry::UnsupportedTelemetryFields GetUnsupportedTelemetryFields() override;
 
  // ---- Configuration ------------------------------------------------------
  bool ApplyConfig(const SmartMotorControllerConfig& config) override;
 
  // ---- Simulation ---------------------------------------------------------
  void SetupSimulation() override;
  void SimIterate() override;
 
  // ---- Encoder sync -------------------------------------------------------
  void SeedRelativeEncoder() override;
  void SynchronizeRelativeEncoder() override;
 
  // ---- Open-loop outputs --------------------------------------------------
  void SetDutyCycle(double dutyCycle) override;
  double GetDutyCycle() override;
  void SetVoltage(wpi::units::volt_t voltage) override;
  wpi::units::volt_t GetVoltage() override;
  std::optional<wpi::units::second_t> GetVelocityMeasurementDelay() const override;
 
  // ---- Closed-loop setpoints ----------------------------------------------
  void SetPosition(wpi::units::turn_t angle) override;
  void SetPosition(wpi::units::meter_t distance) override;
  void SetVelocity(wpi::units::turns_per_second_t velocity) override;
  void SetVelocity(wpi::units::turns_per_second_t velocity,
                   wpi::units::newton_t feedforwardForce) override;
  void SetVelocity(wpi::units::meters_per_second_t velocity) override;
 
  // ---- Encoder writes -----------------------------------------------------
  void SetEncoderPosition(wpi::units::turn_t angle) override;
  void SetEncoderPosition(wpi::units::meter_t distance) override;
  void SetEncoderVelocity(wpi::units::turns_per_second_t velocity) override;
  void SetEncoderVelocity(wpi::units::meters_per_second_t velocity) override;
 
  // ---- Encoder reads ------------------------------------------------------
  wpi::units::turn_t GetMechanismPosition() override;
  wpi::units::turns_per_second_t GetMechanismVelocity() override;
  wpi::units::turn_t GetRelativeMechanismPosition() override;
  wpi::units::turns_per_second_t GetRelativeMechanismVelocity() override;
  wpi::units::turns_per_second_squared_t GetMechanismAcceleration() override;
  wpi::units::turn_t GetRotorPosition() override;
  wpi::units::turns_per_second_t GetRotorVelocity() override;
  wpi::units::meter_t GetMeasurementPosition() override;
  wpi::units::meters_per_second_t GetMeasurementVelocity() override;
  wpi::units::meters_per_second_squared_t GetMeasurementAcceleration() override;
  std::optional<wpi::units::turn_t> GetExternalEncoderMechanismPosition() override;
  std::optional<wpi::units::turns_per_second_t> GetExternalEncoderMechanismVelocity() override;
 
  // ---- Motor status -------------------------------------------------------
  std::optional<wpi::units::ampere_t> GetSupplyCurrent() override;
  wpi::units::ampere_t GetStatorCurrent() override;
  wpi::units::celsius_t GetTemperature() override;
  wpi::math::DCMotor GetDCMotor() override;
 
  // ---- Configuration setters (live tuning) --------------------------------
  void SetZeroPower(MotorMode mode) override;
  void SetMotorInverted(bool inverted) override;
  void SetEncoderInverted(bool inverted) override;
  void SetKp(double kP) override;
  void SetKi(double kI) override;
  void SetKd(double kD) override;
  void SetFeedback(double kP, double kI, double kD) override;
  void SetKs(double kS) override;
  void SetKv(double kV) override;
  void SetKa(double kA) override;
  void SetKg(double kG) override;
  void SetFeedforward(double kS, double kV, double kA, double kG) override;
  void SetStatorCurrentLimit(wpi::units::ampere_t currentLimit) override;
  void SetSupplyCurrentLimit(wpi::units::ampere_t currentLimit) override;
  void SetClosedLoopRampRate(wpi::units::second_t rampRate) override;
  void SetOpenLoopRampRate(wpi::units::second_t rampRate) override;
  void SetMechanismUpperLimit(wpi::units::turn_t upperLimit) override;
  void SetMechanismLowerLimit(wpi::units::turn_t lowerLimit) override;
  void SetMechanismLimits(wpi::units::turn_t lower, wpi::units::turn_t upper) override;
  void SetMechanismLimitsEnabled(bool enabled) override;
  void SetMeasurementUpperLimit(wpi::units::meter_t upperLimit) override;
  void SetMeasurementLowerLimit(wpi::units::meter_t lowerLimit) override;
  void SetMotionProfileMaxVelocity(wpi::units::turns_per_second_t maxVelocity) override;
  void SetMotionProfileMaxVelocity(wpi::units::meters_per_second_t maxVelocity) override;
  void SetMotionProfileMaxAcceleration(wpi::units::turns_per_second_squared_t maxAcc) override;
  void SetMotionProfileMaxAcceleration(wpi::units::meters_per_second_squared_t maxAcc) override;
  void SetMotionProfileMaxJerk(wpi::units::angular_jerk::turns_per_second_cubed_t maxJerk) override;
  void SetExponentialProfile(std::optional<double> kV, std::optional<double> kA,
                             std::optional<wpi::units::volt_t> maxInput) override;
  void SetClosedLoopSlot(ClosedLoopControllerSlot slot) override;
  void SetMechanismGearing(const gearing::MechanismGearing& gearing) override;
  void SetMechanismCircumference(wpi::units::meter_t circumference) override;
 
  SmartMotorControllerConfig& GetConfig() override;
  void* GetMotorController() override;
  void* GetMotorControllerConfig() override;
 
 private:
  rev::spark::SparkBase* m_spark{nullptr};
  rev::spark::SparkClosedLoopController* m_sparkPid{nullptr};
  rev::spark::SparkRelativeEncoder* m_relEncoder{nullptr};
  rev::spark::SparkAbsoluteEncoder* m_absEncoder{nullptr};
  rev::RelativeEncoder* m_quadratureEncoder{nullptr};
  rev::detached::DetachedEncoder* m_detachedEncoder{nullptr};
  double m_detachedEncoderZeroOffset{0.0};
  double m_absEncoderZeroOffset{0.0};
  wpi::units::turn_t m_absoluteEncoderDiscontinuityPoint{1.0};
  wpi::units::turn_t m_detachedEncoderSimAngle{0.0};
  wpi::math::DCMotor m_motor;
 
  // SparkMaxConfig and SparkFlexConfig are non-copyable/non-movable;
  // exactly one will be constructed via emplace().
  std::optional<rev::spark::SparkMaxConfig> m_maxConfig;
  std::optional<rev::spark::SparkFlexConfig> m_flexConfig;
 
  std::optional<wpi::sim::DCMotorSim> m_motorSim;
  std::optional<rev::spark::SparkSim> m_sparkSim;
  std::optional<rev::spark::SparkRelativeEncoderSim> m_relEncoderSim;
  std::optional<rev::spark::SparkAbsoluteEncoderSim> m_absEncoderSim;
  std::optional<rev::spark::SparkMaxAlternateEncoderSim> m_altEncoderSim;
  std::optional<rev::spark::SparkExternalEncoderSim> m_extEncoderSim;
  math::DerivativeTimeFilter m_accelFilter{20_ms};
 
  double m_mechanismToFeedbackSensorRatio{1.0};
  bool m_velocityGainsApplied{false};
  std::optional<wpi::math::TrapezoidProfile<wpi::units::turns>::State> m_simMaxMotionState;
 
  rev::spark::SparkLowLevel::ControlType m_positionControlType{
      rev::spark::SparkLowLevel::ControlType::kPosition};
  rev::spark::SparkLowLevel::ControlType m_velocityControlType{
      rev::spark::SparkLowLevel::ControlType::kVelocity};
 
  rev::spark::ClosedLoopSlot m_closedLoopSlot{rev::spark::ClosedLoopSlot::kSlot0};
 
  std::optional<wpi::util::Alert> m_rioControllerAlert;
  std::optional<wpi::util::Alert> m_externalEncoderGearingDiscontinuityAlert;
  std::optional<wpi::util::Alert> m_feedbackNotAppliedAlert;
  std::optional<wpi::units::second_t> m_velocityMeasurementDelay;
  wpi::units::second_t m_velocitySignalPeriod{0.02};
  std::optional<wpi::util::Alert> m_feedforwardNotAppliedAlert;
 
  void Init(rev::spark::SparkBase* spark, wpi::math::DCMotor motor,
            SmartMotorControllerConfig* config);
  rev::spark::SparkBaseConfig& SparkConfig();
  bool ConfigureSpark(const std::function<rev::REVLibError()>& call,
                      wpi::units::second_t retryFor = wpi::units::second_t{0.015});
 
  bool ConfigureSpark(rev::spark::SparkBaseConfig& config);
 
  bool ConfigureSpark(rev::spark::ClosedLoopConfig& closedLoop);
 
  bool ConfigureSpark(rev::spark::FeedForwardConfig& feedForward);
  rev::PersistMode PersistModeForState() const;
  void ConfigureAsync();
  std::string AlertId(const std::string& alertType) const;
  void ApplyFeedback(double kP, double kI, double kD);
  double MechanismToExternalEncoderRatio() const;
  void UpdateFeedbackSensorRatio(const SmartMotorControllerConfig& config);
  void WriteSparkPID(double kP, double kI, double kD, rev::spark::ClosedLoopSlot slot);
  void WriteSparkFeedforward(double kS, double kV, double kA, rev::spark::ClosedLoopSlot slot);
  void WriteSlotGains(const SmartMotorControllerConfig& config, ClosedLoopControllerSlot slot);
  void WriteMotionProfile(const SmartMotorControllerConfig& config);
  void WriteClosedLoopTolerance(const SmartMotorControllerConfig& config);
  void WriteSoftLimits(const SmartMotorControllerConfig& config);
  void RewriteScaledParameters();
  void UseVelocityGains(bool velocity);
  void ApplyExternalEncoder(const SmartMotorControllerConfig& config, bool useExternalEncoder);
  std::optional<wpi::units::turn_t> GetExternalEncoderSensorPosition();
  std::optional<wpi::units::turns_per_second_t> GetExternalEncoderSensorVelocity();
  void IterateSimulatedMaxMotionPosition();
  void IterateSimulatedArmGravity();
  bool SimulatingMaxMotion() const;
  bool HasExponentialProfile() const;
};
 
}  // namespace yams::motorcontrollers::local