SwerveModule.hpp

Go to the documentation of this file.

/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
#pragma once
 
#include "ctre/phoenix6/swerve/impl/SwerveModuleImpl.hpp"
#include "ctre/phoenix6/CANcoder.hpp"
#include "ctre/phoenix6/TalonFX.hpp"
 
namespace ctre {
namespace phoenix6 {
namespace swerve {
 
using impl::SteerRequestType;
using impl::DriveRequestType;
 
/**
 * \brief Swerve Module class that encapsulates a swerve module powered by
 * CTR Electronics devices.
 *
 * This class handles the hardware devices and configures them for
 * swerve module operation using the Phoenix 6 API.
 *
 * This class constructs hardware devices internally, so the user
 * only specifies the constants (IDs, PID gains, gear ratios, etc).
 * Getters for these hardware devices are available.
 */
template <
    typename DriveMotorT,
    typename SteerMotorT,
    typename EncoderT,
    typename = std::enable_if_t<std::is_base_of_v<hardware::traits::CommonTalon, DriveMotorT>>,
    typename = std::enable_if_t<std::is_base_of_v<hardware::traits::CommonTalon, SteerMotorT>>,
    typename = std::enable_if_t<std::is_base_of_v<hardware::ParentDevice, EncoderT>>
>
class SwerveModule {
public:
    /**
     * \brief Contains everything the swerve module needs to apply a request.
     */
    using ModuleRequest = impl::SwerveModuleImpl::ModuleRequest;
 
protected:
    /** \brief Number of times to attempt config applies. */
    static constexpr int kNumConfigAttempts = 2;
 
    /** \brief The underlying swerve module instance. */
    impl::SwerveModuleImpl *_module;
 
private:
    DriveMotorT _driveMotor;
    SteerMotorT _steerMotor;
    EncoderT _encoder;
 
public:
    /**
     * \brief Construct a SwerveModule with the specified constants.
     *
     * \param constants  Constants used to construct the module
     * \param canbusName The name of the CAN bus this module is on
     * \param module     The impl#SwerveModuleImpl to use
     */
    template <
        typename DriveMotorConfigsT,
        typename SteerMotorConfigsT,
        typename EncoderConfigsT,
        typename = std::enable_if_t<std::is_base_of_v<configs::ParentConfiguration, DriveMotorConfigsT>>,
        typename = std::enable_if_t<std::is_base_of_v<configs::ParentConfiguration, SteerMotorConfigsT>>,
        typename = std::enable_if_t<std::is_base_of_v<configs::ParentConfiguration, EncoderConfigsT>>
    >
    SwerveModule(
        SwerveModuleConstants<DriveMotorConfigsT, SteerMotorConfigsT, EncoderConfigsT> const &constants,
        std::string_view canbusName,
        impl::SwerveModuleImpl &module
    ) :
        _module{&module},
        _driveMotor{constants.DriveMotorId, std::string{canbusName}},
        _steerMotor{constants.SteerMotorId, std::string{canbusName}},
        _encoder{constants.EncoderId, std::string{canbusName}}
    {
        ctre::phoenix::StatusCode response{};
 
        DriveMotorConfigsT driveConfigs = constants.DriveMotorInitialConfigs;
        driveConfigs.MotorOutput.NeutralMode = signals::NeutralModeValue::Brake;
 
        driveConfigs.Slot0 = constants.DriveMotorGains;
        driveConfigs.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent;
        driveConfigs.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent;
        driveConfigs.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent;
        driveConfigs.CurrentLimits.StatorCurrentLimitEnable = true;
 
        driveConfigs.MotorOutput.Inverted = constants.DriveMotorInverted ? signals::InvertedValue::Clockwise_Positive
                : signals::InvertedValue::CounterClockwise_Positive;
        for (int i = 0; i < kNumConfigAttempts; ++i) {
            response = GetDriveMotor().GetConfigurator().Apply(driveConfigs);
            if (response.IsOK()) break;
        }
        if (!response.IsOK()) {
            printf("Talon ID %d failed config with error %s\n", GetDriveMotor().GetDeviceID(), response.GetName());
        }
 
        SteerMotorConfigsT steerConfigs = constants.SteerMotorInitialConfigs;
        steerConfigs.MotorOutput.NeutralMode = signals::NeutralModeValue::Brake;
 
        steerConfigs.Slot0 = constants.SteerMotorGains;
        /* Modify configuration to use remote encoder setting */
        steerConfigs.Feedback.FeedbackRemoteSensorID = constants.EncoderId;
        switch (constants.FeedbackSource) {
            case SteerFeedbackType::FusedCANcoder:
                steerConfigs.Feedback.FeedbackSensorSource = signals::FeedbackSensorSourceValue::FusedCANcoder;
                break;
            case SteerFeedbackType::SyncCANcoder:
                steerConfigs.Feedback.FeedbackSensorSource = signals::FeedbackSensorSourceValue::SyncCANcoder;
                break;
            case SteerFeedbackType::RemoteCANcoder:
                steerConfigs.Feedback.FeedbackSensorSource = signals::FeedbackSensorSourceValue::RemoteCANcoder;
                break;
        }
        steerConfigs.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio;
 
        steerConfigs.MotionMagic.MotionMagicExpo_kV = 0.12_V / 1_tps * constants.SteerMotorGearRatio;
        steerConfigs.MotionMagic.MotionMagicExpo_kA = 0.8_V / 1_tr_per_s_sq / constants.SteerMotorGearRatio;
 
        steerConfigs.ClosedLoopGeneral.ContinuousWrap = true; // Enable continuous wrap for swerve modules
 
        steerConfigs.MotorOutput.Inverted = constants.SteerMotorInverted
                ? signals::InvertedValue::Clockwise_Positive
                : signals::InvertedValue::CounterClockwise_Positive;
        for (int i = 0; i < kNumConfigAttempts; ++i) {
            response = GetSteerMotor().GetConfigurator().Apply(steerConfigs);
            if (response.IsOK()) break;
        }
        if (!response.IsOK()) {
            printf("Talon ID %d failed config with error %s\n", GetSteerMotor().GetDeviceID(), response.GetName());
        }
 
        configs::CANcoderConfiguration encoderConfigs = constants.EncoderInitialConfigs;
        encoderConfigs.MagnetSensor.MagnetOffset = constants.EncoderOffset;
        encoderConfigs.MagnetSensor.SensorDirection = constants.EncoderInverted
                ? signals::SensorDirectionValue::Clockwise_Positive
                : signals::SensorDirectionValue::CounterClockwise_Positive;
        for (int i = 0; i < kNumConfigAttempts; ++i) {
            response = GetEncoder().GetConfigurator().Apply(encoderConfigs);
            if (response.IsOK()) break;
        }
        if (!response.IsOK()) {
            printf("Encoder ID %d failed config with error %s\n", GetEncoder().GetDeviceID(), response.GetName());
        }
    }
 
    virtual ~SwerveModule() = default;
 
    /**
     * \brief Applies the desired ModuleRequest to this module.
     *
     * \param moduleRequest The request to apply to this module
     */
    virtual void Apply(ModuleRequest const &moduleRequest)
    {
        return _module->Apply(moduleRequest);
    }
 
    /**
     * \brief Controls this module using the specified drive and steer control requests.
     *
     * This is intended only to be used for characterization of the robot; do not use this for normal use.
     *
     * \param driveRequest The control request to apply to the drive motor
     * \param steerRequest The control request to apply to the steer motor
     */
    template <typename DriveReq, typename SteerReq>
    void Apply(DriveReq &&driveRequest, SteerReq &&steerRequest)
    {
        return _module->Apply(std::forward<DriveReq>(driveRequest), std::forward<SteerReq>(steerRequest));
    }
 
    /**
     * \brief Gets the state of this module and passes it back as a
     * SwerveModulePosition object with latency compensated values.
     *
     * This function is blocking when it performs a refresh.
     *
     * \param refresh True if the signals should be refreshed
     * \returns SwerveModulePosition containing this module's state.
     */
    SwerveModulePosition GetPosition(bool refresh)
    {
        return _module->GetPosition(refresh);
    }
 
    /**
     * \brief Gets the last cached swerve module position.
     * This differs from #GetPosition in that it will not
     * perform any latency compensation or refresh the signals.
     *
     * \returns Last cached SwerveModulePosition
     */
    SwerveModulePosition GetCachedPosition() const
    {
        return _module->GetCachedPosition();
    }
 
    /**
     * \brief Get the current state of the module.
     *
     * This is typically used for telemetry, as the SwerveModulePosition
     * is used for odometry.
     *
     * \returns Current state of the module
     */
    SwerveModuleState GetCurrentState() const
    {
        return _module->GetCurrentState();
    }
 
    /**
     * \brief Get the target state of the module.
     *
     * This is typically used for telemetry.
     *
     * \returns Target state of the module
     */
    SwerveModuleState GetTargetState() const
    {
        return _module->GetTargetState();
    }
 
    /**
     * \brief Resets this module's drive motor position to 0 rotations.
     */
    virtual void ResetPosition()
    {
        return _module->ResetPosition();
    }
 
    /**
     * \brief Gets the closed-loop output type to use for the drive motor.
     *
     * \returns Drive motor closed-loop output type
     */
    ClosedLoopOutputType GetDriveClosedLoopOutputType() const
    {
        return _module->GetDriveClosedLoopOutputType();
    }
 
    /**
     * \brief Gets the closed-loop output type to use for the steer motor.
     *
     * \returns Steer motor closed-loop output type
     */
    ClosedLoopOutputType GetSteerClosedLoopOutputType() const
    {
        return _module->GetSteerClosedLoopOutputType();
    }
 
    /**
     * \brief Gets this module's Drive Motor reference.
     *
     * This should be used only to access signals and change configurations that the
     * swerve drivetrain does not configure itself.
     *
     * \returns This module's Drive Motor reference
     */
    DriveMotorT &GetDriveMotor()
    {
        return _driveMotor;
    }
 
    /**
     * \brief Gets this module's Steer Motor reference.
     *
     * This should be used only to access signals and change configurations that the
     * swerve drivetrain does not configure itself.
     *
     * \returns This module's Steer Motor reference
     */
    SteerMotorT &GetSteerMotor()
    {
        return _steerMotor;
    }
 
    /**
     * \brief Gets this module's azimuth encoder reference.
     *
     * This should be used only to access signals and change configurations that the
     * swerve drivetrain does not configure itself.
     *
     * \returns This module's azimuth encoder reference
     */
    EncoderT &GetEncoder()
    {
        return _encoder;
    }
};
 
}
}
}