NovaWrapper.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 <thrifty/canEncoder/CanEncoder.h>
#include <thrifty/core/Motor.h>
#include <thrifty/nova/Nova.h>
 
#include <optional>
#include <string>
#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 NovaWrapper : public SmartMotorController {
 public:
  // Expose SetVelocity(velocity, feedforwardForce) from the base class alongside the overrides.
  using SmartMotorController::SetVelocity;
 
  NovaWrapper(thrifty::Nova* nova, wpi::math::DCMotor motor, SmartMotorControllerConfig* config);
  ~NovaWrapper();
 
  // ---- 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;
 
  // ---- 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:
  thrifty::Nova* m_nova{nullptr};
  std::optional<thrifty::NovaConfigBatch> m_vendorConfig;
  wpi::math::DCMotor m_motor;
  std::optional<wpi::sim::DCMotorSim> m_motorSim;
  std::optional<thrifty::Motor::FeedbackSensorType> m_dataPortEncoder;
  thrifty::CanEncoder* m_canEncoder{nullptr};
  wpi::units::turn_t m_absoluteEncoderDiscontinuityPoint{1.0};
  double m_simDutyCycle{0.0};
  math::DerivativeTimeFilter m_accelFilter{20_ms};
 
  std::optional<wpi::util::Alert> m_systemCoreClosedLoopAlert;
 
  std::string AlertId(const std::string& alertType) const;
  double MechanismToRotorRatio() const;
  double MechanismToExternalEncoderRatio() const;
  void LoadClosedLoopController(const SmartMotorControllerConfig& config);
  void ApplyFeedback(double kP, double kI, double kD);
  void CreateMotorSim();
  void ApplyExternalEncoder(const SmartMotorControllerConfig& config,
                            thrifty::NovaConfigBatch& novaConfig);
  std::optional<wpi::units::turn_t> GetExternalEncoderSensorPosition();
  std::optional<wpi::units::turns_per_second_t> GetExternalEncoderSensorVelocity();
};
 
}  // namespace yams::motorcontrollers::local