cia402.h

Go to the documentation of this file.

#pragma once
 
#include <cstdint>
#include <optional>
#include <string_view>
 
 
namespace mm::node::cia402 {
 
enum Object : uint16_t {
  kErrorCode = 0x603F,                
  kControlword = 0x6040,              
  kStatusword = 0x6041,               
  kQuickStopOptionCode = 0x605A,      
  kModeOfOperation = 0x6060,          
  kModeOfOperationDisplay = 0x6061,   
  kPositionDemandValue = 0x6062,      
  kPositionActualValue = 0x6064,      
  kFollowingErrorWindow = 0x6065,     
  kFollowingErrorTimeout = 0x6066,    
  kPositionWindow = 0x6067,           
  kPositionWindowTime = 0x6068,       
  kVelocityDemandValue = 0x606B,      
  kVelocityActualValue = 0x606C,      
  kVelocityWindow = 0x606D,           
  kVelocityWindowTime = 0x606E,       
  kVelocityThreshold = 0x606F,        
  kVelocityThresholdTime = 0x6070,    
  kTargetTorque = 0x6071,             
  kMaxTorque = 0x6072,                
  kMaxCurrent = 0x6073,               
  kTorqueDemand = 0x6074,             
  kMotorRatedCurrent = 0x6075,        
  kMotorRatedTorque = 0x6076,         
  kTorqueActualValue = 0x6077,        
  kDcLinkCircuitVoltage = 0x6079,     
  kTargetPosition = 0x607A,           
  kPositionRangeLimit = 0x607B,       
  kHomeOffset = 0x607C,               
  kSoftwarePositionLimit = 0x607D,    
  kPolarity = 0x607E,                 
  kMaxMotorSpeed = 0x6080,            
  kProfileVelocity = 0x6081,          
  kProfileAcceleration = 0x6083,      
  kProfileDeceleration = 0x6084,      
  kQuickStopDeceleration = 0x6085,    
  kMotionProfileType = 0x6086,        
  kTorqueSlope = 0x6087,              
  kTorqueProfileType = 0x6088,        
  kGearRatio = 0x6091,                
  kFeedConstant = 0x6092,             
  kHomingMethod = 0x6098,             
  kHomingSpeeds = 0x6099,             
  kHomingAcceleration = 0x609A,       
  kSiUnitVelocity = 0x60A9,           
  kVelocityOffset = 0x60B1,           
  kTorqueOffset = 0x60B2,             
  kTouchProbeFunction = 0x60B8,       
  kTouchProbeStatus = 0x60B9,         
  kTouchProbe1PositiveEdge = 0x60BA,  
  kTouchProbe1NegativeEdge = 0x60BB,  
  kTouchProbeTimeStamp1PositiveValue = 0x60D1,  
  kTouchProbeTimeStamp1NegativeValue = 0x60D2,  
  kPositioningOptionCode = 0x60F2,              
  kFollowingErrorActualValue = 0x60F4,          
  kControlEffort = 0x60FA,                      
  kPositionDemandInternalValue = 0x60FC,        
  kDigitalInputs = 0x60FD,        
  kDigitalOutputs = 0x60FE,       
  kTargetVelocity = 0x60FF,       
  kSupportedDriveModes = 0x6502,  
};
 
enum class OperationMode : int8_t {
  kNoMode = 0,                             
  kProfilePosition = 1,                    
  kVelocity = 2,                           
  kProfileVelocity = 3,                    
  kProfileTorque = 4,                      
  kHoming = 6,                             
  kInterpolatedPosition = 7,               
  kCyclicSyncPosition = 8,                 
  kCyclicSyncVelocity = 9,                 
  kCyclicSyncTorque = 10,                  
  kCyclicSyncTorqueCommutationAngle = 11,  
};
 
constexpr std::optional<OperationMode> toOperationMode(int value) {
  // 0x6060 is an INTEGER8, so a value that does not fit int8_t is not a valid mode — reject it
  // before the narrowing cast, or e.g. 264 would alias to 8 (CSP) and slip past validation.
  if (value < INT8_MIN || value > INT8_MAX) {
    return std::nullopt;
  }
  switch (static_cast<OperationMode>(static_cast<int8_t>(value))) {
    case OperationMode::kNoMode:
    case OperationMode::kProfilePosition:
    case OperationMode::kVelocity:
    case OperationMode::kProfileVelocity:
    case OperationMode::kProfileTorque:
    case OperationMode::kHoming:
    case OperationMode::kInterpolatedPosition:
    case OperationMode::kCyclicSyncPosition:
    case OperationMode::kCyclicSyncVelocity:
    case OperationMode::kCyclicSyncTorque:
    case OperationMode::kCyclicSyncTorqueCommutationAngle:
      return static_cast<OperationMode>(static_cast<int8_t>(value));
  }
  return std::nullopt;
}
 
enum class State : uint8_t {
  kNotReadyToSwitchOn,   
  kSwitchOnDisabled,     
  kReadyToSwitchOn,      
  kSwitchedOn,           
  kOperationEnabled,     
  kQuickStopActive,      
  kFaultReactionActive,  
  kFault,                
};
 
constexpr uint16_t kCommandMask = 0x008F;
 
enum Command : uint16_t {
  kCmdShutdown = 0x0006,         
  kCmdSwitchOn = 0x0007,         
  kCmdEnableOperation = 0x000F,  
  kCmdDisableVoltage = 0x0000,   
  kCmdQuickStop = 0x0002,        
  kCmdFaultReset = 0x0080,       
};
 
constexpr State decodeState(uint16_t statusword) {
  // The two masks the standard uses: 0x4F distinguishes the states that ignore bit 5
  // (quick stop), 0x6F the rest.
  if ((statusword & 0x4F) == 0x40) {
    return State::kSwitchOnDisabled;
  }
  if ((statusword & 0x6F) == 0x21) {
    return State::kReadyToSwitchOn;
  }
  if ((statusword & 0x6F) == 0x23) {
    return State::kSwitchedOn;
  }
  if ((statusword & 0x6F) == 0x27) {
    return State::kOperationEnabled;
  }
  if ((statusword & 0x6F) == 0x07) {
    return State::kQuickStopActive;
  }
  if ((statusword & 0x4F) == 0x0F) {
    return State::kFaultReactionActive;
  }
  if ((statusword & 0x4F) == 0x08) {
    return State::kFault;
  }
  return State::kNotReadyToSwitchOn;
}
 
constexpr bool isFaulted(uint16_t statusword) {
  const State s = decodeState(statusword);
  return s == State::kFault || s == State::kFaultReactionActive;
}
 
constexpr std::string_view toString(State state) {
  switch (state) {
    case State::kNotReadyToSwitchOn:
      return "NotReadyToSwitchOn";
    case State::kSwitchOnDisabled:
      return "SwitchOnDisabled";
    case State::kReadyToSwitchOn:
      return "ReadyToSwitchOn";
    case State::kSwitchedOn:
      return "SwitchedOn";
    case State::kOperationEnabled:
      return "OperationEnabled";
    case State::kQuickStopActive:
      return "QuickStopActive";
    case State::kFaultReactionActive:
      return "FaultReactionActive";
    case State::kFault:
      return "Fault";
  }
  return "Unknown";
}
 
constexpr std::string_view toString(OperationMode mode) {
  switch (mode) {
    case OperationMode::kNoMode:
      return "NoMode";
    case OperationMode::kProfilePosition:
      return "ProfilePosition";
    case OperationMode::kVelocity:
      return "Velocity";
    case OperationMode::kProfileVelocity:
      return "ProfileVelocity";
    case OperationMode::kProfileTorque:
      return "ProfileTorque";
    case OperationMode::kHoming:
      return "Homing";
    case OperationMode::kInterpolatedPosition:
      return "InterpolatedPosition";
    case OperationMode::kCyclicSyncPosition:
      return "CyclicSyncPosition";
    case OperationMode::kCyclicSyncVelocity:
      return "CyclicSyncVelocity";
    case OperationMode::kCyclicSyncTorque:
      return "CyclicSyncTorque";
    case OperationMode::kCyclicSyncTorqueCommutationAngle:
      return "CyclicSyncTorqueCommutationAngle";
  }
  return "Unknown";
}
 
struct StandardOperationMode {
  OperationMode mode{OperationMode::kNoMode};
  int bit = -1;
  std::string_view abbreviation;  
  std::string_view label;         
};
 
inline constexpr StandardOperationMode kStandardOperationModes[] = {
    {OperationMode::kNoMode, -1, "", "No mode selected"},
    {OperationMode::kProfilePosition, 0, "pp", "Profile position mode"},
    {OperationMode::kVelocity, 1, "vl", "Velocity mode (frequency converter)"},
    {OperationMode::kProfileVelocity, 2, "pv", "Profile velocity mode"},
    {OperationMode::kProfileTorque, 3, "tq", "Profile torque mode"},
    {OperationMode::kHoming, 5, "hm", "Homing mode"},
    {OperationMode::kInterpolatedPosition, 6, "ip", "Interpolated position mode"},
    {OperationMode::kCyclicSyncPosition, 7, "csp", "Cyclic synchronous position mode"},
    {OperationMode::kCyclicSyncVelocity, 8, "csv", "Cyclic synchronous velocity mode"},
    {OperationMode::kCyclicSyncTorque, 9, "cst", "Cyclic synchronous torque mode"},
    {OperationMode::kCyclicSyncTorqueCommutationAngle, 10, "cstca",
     "Cyclic synchronous torque mode with commutation angle"},
};
 
constexpr bool quickStopHolds(int16_t optionCode) { return optionCode >= 5 && optionCode <= 8; }
 
inline constexpr int kFirstManufacturerDriveModeBit = 16;
 
constexpr std::optional<State> parseState(std::string_view token) {
  for (const auto state : {State::kNotReadyToSwitchOn, State::kSwitchOnDisabled,
                           State::kReadyToSwitchOn, State::kSwitchedOn, State::kOperationEnabled,
                           State::kQuickStopActive, State::kFaultReactionActive, State::kFault}) {
    if (token == toString(state)) {
      return state;
    }
  }
  return std::nullopt;
}
 
constexpr bool isCommandableState(State state) {
  switch (state) {
    case State::kSwitchOnDisabled:
    case State::kReadyToSwitchOn:
    case State::kSwitchedOn:
    case State::kOperationEnabled:
    case State::kQuickStopActive:
      return true;
    case State::kNotReadyToSwitchOn:
    case State::kFaultReactionActive:
    case State::kFault:
      return false;
  }
  return false;
}
 
enum class FsaAction : uint8_t {
  kArrived,  
  kCommand,  
  kWait,     
  kUnreachable,
};
 
struct FsaTransition {
  FsaAction action{FsaAction::kUnreachable};
  uint16_t command = 0;  
};
 
constexpr FsaTransition nextFsaTransition(State from, State target, bool allowQuickStopOverride) {
  if (!isCommandableState(target)) {
    return {FsaAction::kUnreachable, 0};
  }
  if (from == target) {
    return {FsaAction::kArrived, 0};
  }
  switch (from) {
    case State::kNotReadyToSwitchOn:
    case State::kFaultReactionActive:
      return {FsaAction::kWait, 0};
 
    case State::kFault:
      return {FsaAction::kCommand, Command::kCmdFaultReset};
 
    case State::kSwitchOnDisabled:
      // The only way out is upward, whatever the target beyond it.
      return {FsaAction::kCommand, Command::kCmdShutdown};
 
    case State::kReadyToSwitchOn:
      if (target == State::kSwitchOnDisabled) {
        return {FsaAction::kCommand, Command::kCmdDisableVoltage};
      }
      return {FsaAction::kCommand, Command::kCmdSwitchOn};
 
    case State::kSwitchedOn:
      if (target == State::kSwitchOnDisabled) {
        return {FsaAction::kCommand, Command::kCmdDisableVoltage};
      }
      if (target == State::kReadyToSwitchOn) {
        return {FsaAction::kCommand, Command::kCmdShutdown};
      }
      // Both kOperationEnabled and kQuickStopActive are reached through it.
      return {FsaAction::kCommand, Command::kCmdEnableOperation};
 
    case State::kOperationEnabled:
      if (target == State::kSwitchOnDisabled) {
        return {FsaAction::kCommand, Command::kCmdDisableVoltage};
      }
      if (target == State::kReadyToSwitchOn) {
        return {FsaAction::kCommand, Command::kCmdShutdown};
      }
      if (target == State::kSwitchedOn) {
        return {FsaAction::kCommand, Command::kCmdSwitchOn};
      }
      return {FsaAction::kCommand, Command::kCmdQuickStop};
 
    case State::kQuickStopActive:
      if (target == State::kOperationEnabled) {
        return allowQuickStopOverride
                   ? FsaTransition{FsaAction::kCommand, Command::kCmdEnableOperation}
                   : FsaTransition{FsaAction::kUnreachable, 0};
      }
      // Everything else leaves through SwitchOnDisabled, which then continues the walk.
      return {FsaAction::kCommand, Command::kCmdDisableVoltage};
  }
  return {FsaAction::kUnreachable, 0};
}
 
}  // namespace mm::node::cia402