SparkBase.hpp

Go to the documentation of this file.

 
#ifndef SPARKBASE_HPP
#define SPARKBASE_HPP
 
#include <array>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <limits>
#include <map>
#include <stdexcept>
#include <system_error>
#include <thread>
#include <variant>
#include <optional>
 
#include <linux/can.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <unistd.h>
 
#define RED "\033[31m"  
#define RESET "\033[0m" 
 
constexpr uint8_t PARAM_TYPE_UINT = 0x01;  
constexpr uint8_t PARAM_TYPE_FLOAT = 0x02; 
constexpr uint8_t PARAM_TYPE_BOOL = 0x03;  
 
enum class SystemControl : uint32_t
{
    BurnFlash = 0x205FC80,
    FactoryDefaults = 0x2051D00,
    FactoryReset = 0x2051D40,
    Identify = 0x2051D80,
    ResetFaults = 0x2053000,
    ClearStickyFaults = 0x2054400
};
 
enum class MotorControl : uint32_t
{
    Setpoint = 0x2050040,
    DutyCycle = 0x2050080,
    Velocity = 0x2050480,
    SmartVelocity = 0x20504C0,
    Position = 0x2050C80,
    Voltage = 0x2051080,
    Current = 0x20510C0,
    SmartMotion = 0x2051480
};
 
enum class Status : uint32_t
{
    Period0 = 0x2051800,
    Period1 = 0x2051840,
    Period2 = 0x2051880,
    Period3 = 0x20518C0,
    Period4 = 0x2051900
};
 
enum class Parameter : uint32_t
{
    kInputMode = 1,
    kMotorType = 2,
    kCommAdvance = 3,
    kSensorType = 4,
    kCtrlType = 5,
    kIdleMode = 6,
    kInputDeadband = 7,
    kFeedbackSensorPID0 = 8,
    kFeedbackSensorPID1 = 9,
    kPolePairs = 10,
    kCurrentChop = 11,
    kCurrentChopCycles = 12,
    kP_0 = 13,
    kI_0 = 14,
    kD_0 = 15,
    kF_0 = 16,
    kIZone_0 = 17,
    kDFilter_0 = 18,
    kOutputMin_0 = 19,
    kOutputMax_0 = 20,
    kP_1 = 21,
    kI_1 = 22,
    kD_1 = 23,
    kF_1 = 24,
    kIZone_1 = 25,
    kDFilter_1 = 26,
    kOutputMin_1 = 27,
    kOutputMax_1 = 28,
    kP_2 = 29,
    kI_2 = 30,
    kD_2 = 31,
    kF_2 = 32,
    kIZone_2 = 33,
    kDFilter_2 = 34,
    kOutputMin_2 = 35,
    kOutputMax_2 = 36,
    kP_3 = 37,
    kI_3 = 38,
    kD_3 = 39,
    kF_3 = 40,
    kIZone_3 = 41,
    kDFilter_3 = 42,
    kOutputMin_3 = 43,
    kOutputMax_3 = 44,
    kInverted = 45,
    kOutputRatio = 46,
    kSerialNumberLow = 47,
    kSerialNumberMid = 48,
    kSerialNumberHigh = 49,
    kLimitSwitchFwdPolarity = 50,
    kLimitSwitchRevPolarity = 51,
    kHardLimitFwdEn = 52,
    kHardLimitRevEn = 53,
    kSoftLimitFwdEn = 54,
    kSoftLimitRevEn = 55,
    kRampRate = 56,
    kFollowerID = 57,
    kFollowerConfig = 58,
    kSmartCurrentStallLimit = 59,
    kSmartCurrentFreeLimit = 60,
    kSmartCurrentConfig = 61,
    kMotorKv = 63,
    kMotorR = 64,
    kMotorL = 65,
    kEncoderCountsPerRev = 69,
    kEncoderAverageDepth = 70,
    kEncoderSampleDelta = 71,
    kEncoderInverted = 72,
    kClosedLoopVoltageMode = 74,
    kCompensatedNominalVoltage = 75,
    kSmartMotionMaxVelocity_0 = 76,
    kSmartMotionMaxAccel_0 = 77,
    kSmartMotionMinVelOutput_0 = 78,
    kSmartMotionAllowedClosedLoopError_0 = 79,
    kSmartMotionAccelStrategy_0 = 80,
    kSmartMotionMaxVelocity_1 = 81,
    kSmartMotionMaxAccel_1 = 82,
    kSmartMotionMinVelOutput_1 = 83,
    kSmartMotionAllowedClosedLoopError_1 = 84,
    kSmartMotionAccelStrategy_1 = 85,
    kSmartMotionMaxVelocity_2 = 86,
    kSmartMotionMaxAccel_2 = 87,
    kSmartMotionMinVelOutput_2 = 88,
    kSmartMotionAllowedClosedLoopError_2 = 89,
    kSmartMotionAccelStrategy_2 = 90,
    kSmartMotionMaxVelocity_3 = 91,
    kSmartMotionMaxAccel_3 = 92,
    kSmartMotionMinVelOutput_3 = 93,
    kSmartMotionAllowedClosedLoopError_3 = 94,
    kSmartMotionAccelStrategy_3 = 95,
    kIMaxAccum_0 = 96,
    kSlot3Placeholder1_0 = 97,
    kSlot3Placeholder2_0 = 98,
    kSlot3Placeholder3_0 = 99,
    kIMaxAccum_1 = 100,
    kSlot3Placeholder1_1 = 101,
    kSlot3Placeholder2_1 = 102,
    kSlot3Placeholder3_1 = 103,
    kIMaxAccum_2 = 104,
    kSlot3Placeholder1_2 = 105,
    kSlot3Placeholder2_2 = 106,
    kSlot3Placeholder3_2 = 107,
    kIMaxAccum_3 = 108,
    kSlot3Placeholder1_3 = 109,
    kSlot3Placeholder2_3 = 110,
    kSlot3Placeholder3_3 = 111,
    kPositionConversionFactor = 112,
    kVelocityConversionFactor = 113,
    kClosedLoopRampRate = 114,
    kSoftLimitFwd = 115,
    kSoftLimitRev = 116,
    kAnalogPositionConversion = 119,
    kAnalogVelocityConversion = 120,
    kAnalogAverageDepth = 121,
    kAnalogSensorMode = 122,
    kAnalogInverted = 123,
    kAnalogSampleDelta = 124,
    kDataPortConfig = 127,
    kAltEncoderCountsPerRev = 128,
    kAltEncoderAverageDepth = 129,
    kAltEncoderSampleDelta = 130,
    kAltEncoderInverted = 131,
    kAltEncoderPositionFactor = 132,
    kAltEncoderVelocityFactor = 133,
    kHallSensorSampleRate = 136,
    kHallSensorAverageDepth = 137,
    kDutyCyclePositionFactor = 139,
    kDutyCycleVelocityFactor = 140,
    kDutyCycleInverted = 141,
    kDutyCycleAverageDepth = 143,
    kPositionPIDWrapEnable = 149,
    kPositionPIDMinInput = 150,
    kPositionPIDMaxInput = 151,
    kDutyCyclePrescalar = 153,
    kDutyCycleZeroOffset = 154
};
 
enum class MotorType : uint8_t
{
    kBrushed = 0,
    kBrushless = 1
};
 
enum class SensorType : uint8_t
{
    kNoSensor = 0,
    kHallSensor = 1,
    kEncoder = 2
};
 
enum class CtrlType : uint8_t
{
    kDutyCycle = 0,
    kVelocity = 1,
    kVoltage = 2,
    kPosition = 3
};
 
enum class IdleMode : uint8_t
{
    kCoast = 0,
    kBrake = 1
};
 
class SparkBase
{
private:
    static int soc;                   
    std::string interfaceName; 
    uint8_t deviceId;          
    struct sockaddr_can addr;  
    struct ifreq ifr;          
    mutable std::map<Status,
                     std::pair<uint64_t, std::chrono::steady_clock::time_point>>
        cachedStatus; 
 
    void SendCanFrame(
        uint32_t arbitrationId, uint8_t dlc,
        const std::array<uint8_t, 8> &data = std::array<uint8_t, 8>{}) const;
 
    void SendControlMessage(
        std::variant<MotorControl, SystemControl> command, std::string commandName, float value, std::optional<float> minValue = std::nullopt,
        std::optional<float> maxValue = std::nullopt) const;
 
    uint64_t ReadPeriodicStatus(Status period) const;
 
    void SetParameter(
        Parameter parameterId,
        uint8_t parameterType,
        std::string parameterName,
        std::variant<float, uint32_t, uint16_t, uint8_t, bool> value,
        std::optional<float> minValue = std::nullopt,
        std::optional<float> maxValue = std::nullopt,
        std::optional<std::string> customErrorMessage = std::nullopt);
 
    std::variant<float, uint32_t, bool> ReadParameter(Parameter parameterId);
 
public:
    SparkBase(const std::string &interfaceName, uint8_t deviceId);
 
    virtual ~SparkBase();
 
    // SystemControl Methods //
 
    static void Heartbeat();
 
    void ResetFaults();
 
    void ClearStickyFaults();
 
    void BurnFlash();
 
    void FactoryDefaults();
 
    void FactoryReset();
 
    void Identify();
 
    // MotorControl Methods //
 
    void SetSetpoint(float setpoint);
 
    void SetDutyCycle(float dutyCycle);
 
    void SetVelocity(float velocity);
 
    void SetSmartVelocity(float smartVelocity);
 
    void SetPosition(float position);
 
    void SetVoltage(float voltage);
 
    void SetCurrent(float current);
 
    void SetSmartMotion(float smartMotion);
 
    // Status Methods //
 
    float GetDutyCycle() const;
 
    uint16_t GetFaults() const;
 
    uint16_t GetStickyFaults() const;
 
    bool GetInverted() const;
 
    bool GetIdleMode() const;
 
    bool IsFollower() const;
 
    float GetVelocity() const;
 
    float GetTemperature() const;
 
    float GetVoltage() const;
 
    float GetCurrent() const;
 
    float GetPosition() const;
 
    float GetAnalogVoltage() const;
 
    float GetAnalogVelocity() const;
 
    float GetAnalogPosition() const;
 
    float GetAlternateEncoderVelocity() const;
 
    float GetAlternateEncoderPosition() const;
 
    // Parameter Setters //
 
    void SetInputMode(uint8_t mode);
 
    void SetMotorType(MotorType type);
 
    void SetSensorType(SensorType sensor);
 
    void SetIdleMode(IdleMode mode);
 
    void SetInputDeadband(float deadband);
 
    void SetInverted(bool inverted);
 
    void SetRampRate(float rate);
 
    // Advanced //
 
    void SetMotorKv(uint16_t kv);
 
    void SetMotorR(uint16_t r);
 
    void SetMotorL(uint16_t l);
 
    // Closed Loop //
 
    void SetCtrlType(CtrlType type);
 
    void SetFeedbackSensorPID0(uint16_t sensor);
 
    void SetClosedLoopVoltageMode(uint8_t mode);
 
    void SetCompensatedNominalVoltage(float voltage);
 
    void SetPositionPIDWrapEnable(bool enable);
 
    void SetPositionPIDMinInput(float minInput);
 
    void SetPositionPIDMaxInput(float maxInput);
 
    // Brushless //
 
    void SetPolePairs(uint16_t pairs);
 
    // Current Limit //
 
    void SetCurrentChop(float chop);
 
    void SetCurrentChopCycles(uint16_t cycles);
 
    void SetSmartCurrentStallLimit(uint16_t limit);
 
    void SetSmartCurrentFreeLimit(uint16_t limit);
 
    void SetSmartCurrentConfig(uint16_t config);
 
    // PIDF //
 
    void SetP(uint8_t slot, float p);
 
    void SetI(uint8_t slot, float i);
 
    void SetD(uint8_t slot, float d);
 
    void SetF(uint8_t slot, float f);
 
    void SetIZone(uint8_t slot, float iZone);
 
    void SetDFilter(uint8_t slot, float dFilter);
 
    void SetOutputMin(uint8_t slot, float min);
 
    void SetOutputMax(uint8_t slot, float max);
 
    // Limits //
 
    void SetHardLimitFwdEn(bool enable);
 
    void SetHardLimitRevEn(bool enable);
 
    void SetLimitSwitchFwdPolarity(bool polarity);
 
    void SetLimitSwitchRevPolarity(bool polarity);
 
    void SetSoftLimitFwdEn(bool enable);
 
    void SetSoftLimitRevEn(bool enable);
 
    void SetSoftLimitFwd(float limit);
 
    void SetSoftLimitRev(float limit);
 
    // Follower //
 
    void SetFollowerID(uint32_t id);
 
    void SetFollowerConfig(uint32_t config);
 
    // Encoder Port //
 
    void SetEncoderCountsPerRev(uint16_t counts);
 
    void SetEncoderAverageDepth(uint8_t depth);
 
    void SetEncoderSampleDelta(uint8_t delta);
 
    void SetEncoderInverted(bool inverted);
 
    void SetPositionConversionFactor(float factor);
 
    void SetVelocityConversionFactor(float factor);
 
    void SetClosedLoopRampRate(float rampRate);
 
    void SetHallSensorSampleRate(float rate);
 
    void SetHallSensorAverageDepth(uint16_t depth);
 
    // Smart Motion //
 
    void SetSmartMotionMaxVelocity(uint8_t slot, float maxVel);
 
    void SetSmartMotionMaxAccel(uint8_t slot, float maxAccel);
 
    void SetSmartMotionMinVelOutput(uint8_t slot, float minVel);
 
    void SetSmartMotionAllowedClosedLoopError(uint8_t slot, float error);
 
    void SetSmartMotionAccelStrategy(uint8_t slot, float strategy);
 
    void SetIMaxAccum(uint8_t slot, float maxAccum);
 
    void SetSlot3Placeholder1(uint8_t slot, float value);
 
    void SetSlot3Placeholder2(uint8_t slot, float value);
 
    void SetSlot3Placeholder3(uint8_t slot, float value);
 
    // Analog Sensor //
 
    void SetAnalogPositionConversion(float factor);
 
    void SetAnalogVelocityConversion(float factor);
 
    void SetAnalogAverageDepth(uint16_t depth);
 
    void SetAnalogSensorMode(uint8_t mode);
 
    void SetAnalogInverted(bool inverted);
 
    void SetAnalogSampleDelta(uint16_t delta);
 
    // Alternate Encoder  //
 
    void SetDataPortConfig(uint8_t config);
 
    void SetAltEncoderCountsPerRev(uint16_t counts);
 
    void SetAltEncoderAverageDepth(uint8_t depth);
 
    void SetAltEncoderSampleDelta(uint8_t delta);
 
    void SetAltEncoderInverted(bool inverted);
 
    void SetAltEncoderPositionFactor(float factor);
 
    void SetAltEncoderVelocityFactor(float factor);
 
    // Duty Cycle Absolute Encoder //
 
    void SetDutyCyclePositionFactor(float factor);
 
    void SetDutyCycleVelocityFactor(float factor);
 
    void SetDutyCycleInverted(bool inverted);
 
    void SetDutyCycleAverageDepth(uint8_t depth);
 
    void SetDutyCyclePrescalar(uint8_t prescalar);
 
    void SetDutyCycleZeroOffset(float offset);
 
    // Parameter Getters //
 
    // Basic //
    uint8_t GetMotorType();
 
    uint8_t GetSensorType();
 
    uint8_t GetIdleMode();
 
    float GetInputDeadband();
 
    bool GetInverted();
 
    float GetRampRate();
 
    // Advanced //
 
    uint16_t GetMotorKv();
 
    uint16_t GetMotorR();
 
    uint16_t GetMotorL();
 
    // Closed Loop //
 
    uint8_t GetCtrlType();
 
    uint16_t GetFeedbackSensorPID0();
 
    uint8_t GetClosedLoopVoltageMode();
 
    float GetCompensatedNominalVoltage();
 
    bool GetPositionPIDWrapEnable();
 
    float GetPositionPIDMinInput();
 
    float GetPositionPIDMaxInput();
 
    // Brushless //
 
    uint16_t GetPolePairs();
 
    // Current Limit //
 
    float GetCurrentChop();
 
    uint16_t GetCurrentChopCycles();
 
    uint16_t GetSmartCurrentStallLimit();
 
    uint16_t GetSmartCurrentFreeLimit();
 
    uint16_t GetSmartCurrentConfig();
 
    // PIDF //
 
    float GetP(uint8_t slot);
 
    float GetI(uint8_t slot);
 
    float GetD(uint8_t slot);
 
    float GetF(uint8_t slot);
 
    float GetIZone(uint8_t slot);
 
    float GetDFilter(uint8_t slot);
 
    float GetOutputMin(uint8_t slot);
 
    float GetOutputMax(uint8_t slot);
 
    // Limits //
 
    bool GetHardLimitFwdEn();
 
    bool GetHardLimitRevEn();
 
    bool GetLimitSwitchFwdPolarity();
 
    bool GetLimitSwitchRevPolarity();
 
    bool GetSoftLimitFwdEn();
 
    bool GetSoftLimitRevEn();
 
    float GetSoftLimitFwd();
 
    float GetSoftLimitRev();
 
    // Follower //
 
    uint32_t GetFollowerID();
 
    uint32_t GetFollowerConfig();
 
    // Encoder Port //
 
    uint16_t GetEncoderCountsPerRev();
 
    uint8_t GetEncoderAverageDepth();
 
    uint8_t GetEncoderSampleDelta();
 
    bool GetEncoderInverted();
 
    float GetPositionConversionFactor();
 
    float GetVelocityConversionFactor();
 
    float GetClosedLoopRampRate();
 
    float GetHallSensorSampleRate();
 
    uint16_t GetHallSensorAverageDepth();
 
    // Smart Motion //
 
    float GetSmartMotionMaxVelocity(uint8_t slot);
 
    float GetSmartMotionMaxAccel(uint8_t slot);
 
    float GetSmartMotionMinVelOutput(uint8_t slot);
 
    float GetSmartMotionAllowedClosedLoopError(uint8_t slot);
 
    float GetSmartMotionAccelStrategy(uint8_t slot);
 
    float GetIMaxAccum(uint8_t slot);
 
    float GetSlot3Placeholder1(uint8_t slot);
 
    float GetSlot3Placeholder2(uint8_t slot);
 
    float GetSlot3Placeholder3(uint8_t slot);
 
    // Analog Sensor //
 
    float GetAnalogPositionConversion();
 
    float GetAnalogVelocityConversion();
 
    uint16_t GetAnalogAverageDepth();
 
    uint8_t GetAnalogSensorMode();
 
    bool GetAnalogInverted();
 
    uint16_t GetAnalogSampleDelta();
 
    // Alternate Encoder //
 
    uint8_t GetDataPortConfig();
 
    uint16_t GetAltEncoderCountsPerRev();
 
    uint8_t GetAltEncoderAverageDepth();
 
    uint8_t GetAltEncoderSampleDelta();
 
    bool GetAltEncoderInverted();
 
    float GetAltEncoderPositionFactor();
 
    float GetAltEncoderVelocityFactor();
 
    // Duty Cycle Absolute Encoder //
 
    float GetDutyCyclePositionFactor();
 
    float GetDutyCycleVelocityFactor();
 
    bool GetDutyCycleInverted();
 
    uint8_t GetDutyCycleAverageDepth();
 
    uint8_t GetDutyCyclePrescalar();
 
    float GetDutyCycleZeroOffset();
};
 
#endif // SPARKBASE_HPP