Initial commit
This commit is contained in:
@@ -0,0 +1,23 @@
|
||||
namespace RobotNet10.CANOpen.CiA402.Enums;
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Control mode: SDO only, PDO only, or Hybrid (SDO for config, PDO for real-time)
|
||||
/// </summary>
|
||||
public enum ControlMode
|
||||
{
|
||||
/// <summary>
|
||||
/// Chỉ dùng SDO - chậm nhưng không cần cấu hình PDO
|
||||
/// </summary>
|
||||
SdoOnly,
|
||||
|
||||
/// <summary>
|
||||
/// Chỉ dùng PDO - nhanh nhất, cần cấu hình PDO mapping trên device
|
||||
/// </summary>
|
||||
PdoOnly,
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid: SDO cho configuration, PDO cho real-time control (recommended)
|
||||
/// </summary>
|
||||
Hybrid
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace RobotNet10.CANOpen.CiA402.Enums;
|
||||
|
||||
public enum DriveState
|
||||
{
|
||||
Unknown,
|
||||
NotReadyToSwitchOn,
|
||||
SwitchOnDisabled,
|
||||
ReadyToSwitchOn,
|
||||
SwitchedOn,
|
||||
OperationEnabled,
|
||||
QuickStopActive,
|
||||
FaultReactionActive,
|
||||
Fault
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
namespace RobotNet10.CANOpen.CiA402.Enums;
|
||||
|
||||
public enum OperationMode : sbyte
|
||||
{
|
||||
NoMode = 0,
|
||||
ProfilePosition = 1,
|
||||
Velocity = 2,
|
||||
ProfileVelocity = 3,
|
||||
ProfileTorque = 4,
|
||||
Homing = 6,
|
||||
InterpolatedPosition = 7,
|
||||
CyclicSyncPosition = 8,
|
||||
CyclicSyncVelocity = 9,
|
||||
CyclicSyncTorque = 10
|
||||
}
|
||||
@@ -0,0 +1,431 @@
|
||||
using RobotNet10.CANOpen.Models;
|
||||
|
||||
namespace RobotNet10.CANOpen.CiA402;
|
||||
|
||||
/// <summary>
|
||||
/// CiA402 Object Dictionary - có thể customize địa chỉ cho từng device
|
||||
/// Format mô tả: [Access] [Type/Size] - Ý nghĩa
|
||||
/// Access: RO=Read Only, WO=Write Only, RW=Read/Write
|
||||
/// </summary>
|
||||
public class CiA402ObjectDictionary
|
||||
{
|
||||
// ==================== State Machine Control ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x6040 - [WO] UINT16 (2 bytes) - Controlword
|
||||
/// Điều khiển state machine của motor (Enable, Disable, Fault Reset, Quick Stop, etc.)
|
||||
/// Bit-mapped command word để transition giữa các states
|
||||
/// </summary>
|
||||
public ushort Controlword { get; set; } = 0x6040;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6041 - [RO] UINT16 (2 bytes) - Statusword
|
||||
/// Trạng thái hiện tại của motor drive (Ready, Enabled, Fault, Warning, etc.)
|
||||
/// Bit-mapped status word cho biết state và các cờ trạng thái
|
||||
/// </summary>
|
||||
public ushort Statusword { get; set; } = 0x6041;
|
||||
|
||||
// ==================== Operation Mode ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x6060 - [RW] INT8 (1 byte) - Modes of Operation
|
||||
/// Chọn chế độ điều khiển: 1=PP, 3=PV, 4=PT, 6=HM, 8=CSP, 9=CSV, 10=CST
|
||||
/// Write để set mode, read để xem mode được request
|
||||
/// </summary>
|
||||
public ushort ModesOfOperation { get; set; } = 0x6060;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6061 - [RO] INT8 (1 byte) - Modes of Operation Display
|
||||
/// Chế độ điều khiển hiện tại đang active/running
|
||||
/// Read để kiểm tra mode thực tế đã được apply
|
||||
/// </summary>
|
||||
public ushort ModesOfOperationDisplay { get; set; } = 0x6061;
|
||||
|
||||
// ==================== Position Control ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x6064 - [RO] INT32 (4 bytes) - Position Actual Value
|
||||
/// Vị trí thực tế hiện tại của motor (encoder counts)
|
||||
/// Real-time feedback từ encoder, cập nhật liên tục
|
||||
/// </summary>
|
||||
public ushort PositionActualValue { get; set; } = 0x6064;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6062 - [RO] INT32 (4 bytes) - Position Demand Value
|
||||
/// Vị trí mục tiêu nội bộ của trajectory generator
|
||||
/// Giá trị trajectory planner đang yêu cầu motor đến
|
||||
/// </summary>
|
||||
public ushort PositionDemandValue { get; set; } = 0x6062;
|
||||
|
||||
/// <summary>
|
||||
/// 0x607A - [RW] INT32 (4 bytes) - Target Position
|
||||
/// Vị trí đích mà motor cần di chuyển đến (Profile Position mode)
|
||||
/// Write target position, motor sẽ di chuyển đến đó theo profile
|
||||
/// </summary>
|
||||
public ushort TargetPosition { get; set; } = 0x607A;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6067 - [RW] UINT32 (4 bytes) - Position Window
|
||||
/// Dung sai vị trí cho phép (encoder counts)
|
||||
/// Nếu |Actual - Target| < Window → Target reached
|
||||
/// </summary>
|
||||
public ushort PositionWindow { get; set; } = 0x6067;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6068 - [RW] UINT16 (2 bytes) - Position Window Time
|
||||
/// Thời gian motor phải ở trong Position Window (ms)
|
||||
/// Phải trong window ít nhất time này mới set "Target Reached" bit
|
||||
/// </summary>
|
||||
public ushort PositionWindowTime { get; set; } = 0x6068;
|
||||
|
||||
/// <summary>
|
||||
/// 0x607B - [RW] INT32 (4 bytes) - Position Range Limit
|
||||
/// Giới hạn khoảng di chuyển tối đa cho 1 lần move
|
||||
/// Sub-index 1: Min position, Sub-index 2: Max position
|
||||
/// </summary>
|
||||
public ushort PositionRangeLimit { get; set; } = 0x607B;
|
||||
|
||||
/// <summary>
|
||||
/// 0x607D - [RW] INT32 (4 bytes) - Software Position Limit
|
||||
/// Giới hạn vị trí phần mềm (software limit switches)
|
||||
/// Sub-index 1: Min limit, Sub-index 2: Max limit
|
||||
/// </summary>
|
||||
public ushort SoftwarePositionLimit { get; set; } = 0x607D;
|
||||
|
||||
// ==================== Profile Parameters ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x6081 - [RW] UINT32 (4 bytes) - Profile Velocity
|
||||
/// Vận tốc tối đa trong Profile Position mode (user units/s)
|
||||
/// Vận tốc motor sẽ di chuyển khi execute position command
|
||||
/// </summary>
|
||||
public ushort ProfileVelocity { get; set; } = 0x6081;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6083 - [RW] UINT32 (4 bytes) - Profile Acceleration
|
||||
/// Gia tốc tăng tốc trong profile motion (user units/s²)
|
||||
/// Tốc độ tăng vận tốc khi bắt đầu di chuyển
|
||||
/// </summary>
|
||||
public ushort ProfileAcceleration { get; set; } = 0x6083;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6084 - [RW] UINT32 (4 bytes) - Profile Deceleration
|
||||
/// Gia tốc giảm tốc trong profile motion (user units/s²)
|
||||
/// Tốc độ giảm vận tốc khi gần đến target
|
||||
/// </summary>
|
||||
public ushort ProfileDeceleration { get; set; } = 0x6084;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6085 - [RW] UINT32 (4 bytes) - Quick Stop Deceleration
|
||||
/// Gia tốc phanh khẩn cấp (user units/s²)
|
||||
/// Sử dụng khi Quick Stop command được trigger
|
||||
/// </summary>
|
||||
public ushort QuickStopDeceleration { get; set; } = 0x6085;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6086 - [RW] INT16 (2 bytes) - Motion Profile Type
|
||||
/// Loại trajectory: 0=Linear ramp, 1=Sin² ramp
|
||||
/// Xác định hình dạng của velocity profile
|
||||
/// </summary>
|
||||
public ushort MotionProfileType { get; set; } = 0x6086;
|
||||
|
||||
// ==================== Velocity Control ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x606C - [RO] INT32 (4 bytes) - Velocity Actual Value
|
||||
/// Vận tốc thực tế hiện tại của motor (user units/s hoặc RPM)
|
||||
/// Real-time feedback, cập nhật liên tục
|
||||
/// </summary>
|
||||
public ushort VelocityActualValue { get; set; } = 0x606C;
|
||||
|
||||
/// <summary>
|
||||
/// 0x606B - [RO] INT32 (4 bytes) - Velocity Demand Value
|
||||
/// Vận tốc mục tiêu nội bộ của controller
|
||||
/// Giá trị controller đang yêu cầu motor chạy
|
||||
/// </summary>
|
||||
public ushort VelocityDemandValue { get; set; } = 0x606B;
|
||||
|
||||
/// <summary>
|
||||
/// 0x60FF - [RW] INT32 (4 bytes) - Target Velocity
|
||||
/// Vận tốc đích trong Profile Velocity mode (user units/s)
|
||||
/// Write target velocity, motor sẽ tăng/giảm tốc đến giá trị này
|
||||
/// </summary>
|
||||
public ushort TargetVelocity { get; set; } = 0x60FF;
|
||||
|
||||
/// <summary>
|
||||
/// 0x60FF - [RW] INT32 (4 bytes) - Profile Speed
|
||||
/// Vận tốc đích trong Profile Position mode (user units/s)
|
||||
/// Write Profile Speed, motor sẽ tăng/giảm tốc đến giá trị này
|
||||
/// </summary>
|
||||
public ushort ProfileSpeed { get; set; } = 0x6081;
|
||||
|
||||
/// <summary>
|
||||
/// 0x606D - [RW] UINT16 (2 bytes) - Velocity Window
|
||||
/// Dung sai vận tốc (user units/s)
|
||||
/// Nếu |Actual - Target| < Window → Target reached
|
||||
/// </summary>
|
||||
public ushort VelocityWindow { get; set; } = 0x606D;
|
||||
|
||||
/// <summary>
|
||||
/// 0x606E - [RW] UINT16 (2 bytes) - Velocity Window Time
|
||||
/// Thời gian trong Velocity Window để set "Target Reached" (ms)
|
||||
/// </summary>
|
||||
public ushort VelocityWindowTime { get; set; } = 0x606E;
|
||||
|
||||
/// <summary>
|
||||
/// 0x606F - [RW] UINT16 (2 bytes) - Velocity Threshold
|
||||
/// Ngưỡng vận tốc tối thiểu để coi như motor đang chạy
|
||||
/// Dùng để detect motor đã dừng hoàn toàn
|
||||
/// </summary>
|
||||
public ushort VelocityThreshold { get; set; } = 0x606F;
|
||||
|
||||
/// <summary>
|
||||
/// 0x607F - [RW] UINT32 (4 bytes) - Max Profile Velocity
|
||||
/// Vận tốc tối đa cho phép trong profile modes (user units/s)
|
||||
/// Giới hạn trên cho ProfileVelocity và TargetVelocity
|
||||
/// </summary>
|
||||
public ushort MaxProfileVelocity { get; set; } = 0x607F;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6080 - [RW] UINT32 (4 bytes) - Max Motor Speed
|
||||
/// Tốc độ tối đa tuyệt đối của motor (RPM)
|
||||
/// Hardware limit, vượt quá sẽ trigger error
|
||||
/// </summary>
|
||||
public ushort MaxMotorSpeed { get; set; } = 0x6080;
|
||||
|
||||
// ==================== Torque Control ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x6077 - [RO] INT16 (2 bytes) - Torque Actual Value
|
||||
/// Mô-men xoắn thực tế hiện tại (phần nghìn của rated torque)
|
||||
/// 1000 = 100% rated torque, real-time feedback
|
||||
/// </summary>
|
||||
public ushort TorqueActualValue { get; set; } = 0x6077;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6071 - [RW] INT16 (2 bytes) - Target Torque
|
||||
/// Mô-men xoắn đích trong Torque mode (phần nghìn của rated torque)
|
||||
/// 1000 = 100%, 500 = 50%, -500 = -50% (reverse direction)
|
||||
/// </summary>
|
||||
public ushort TargetTorque { get; set; } = 0x6071;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6072 - [RW] UINT16 (2 bytes) - Max Torque
|
||||
/// Mô-men xoắn tối đa cho phép (phần nghìn của rated torque)
|
||||
/// Safety limit, motor không được vượt quá giá trị này
|
||||
/// </summary>
|
||||
public ushort MaxTorque { get; set; } = 0x6072;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6087 - [RW] UINT32 (4 bytes) - Torque Slope
|
||||
/// Tốc độ thay đổi torque tối đa (delta torque per second)
|
||||
/// Giới hạn tốc độ tăng/giảm torque để bảo vệ cơ khí
|
||||
/// </summary>
|
||||
public ushort TorqueSlope { get; set; } = 0x6087;
|
||||
|
||||
// ==================== Homing ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x6098 - [RW] INT8 (1 byte) - Homing Method
|
||||
/// Phương pháp tìm home: 1-14 (negative limit), 17-30 (positive limit), 33-35 (index pulse)
|
||||
/// Xác định cách motor tìm vị trí home (zero position)
|
||||
/// </summary>
|
||||
public ushort HomingMethod { get; set; } = 0x6098;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6099 - [RW] UINT32 (4 bytes) - Homing Speeds
|
||||
/// Vận tốc khi tìm home (user units/s)
|
||||
/// Sub-index 1: Speed during search, Sub-index 2: Speed during zero search
|
||||
/// </summary>
|
||||
public ushort HomingSpeed { get; set; } = 0x6099;
|
||||
|
||||
/// <summary>
|
||||
/// 0x607C - [RW] INT32 (4 bytes) - Home Offset / Homing Offset
|
||||
/// Offset áp dụng sau khi homing hoàn tất (user units / encoder counts)
|
||||
/// </summary>
|
||||
public ushort HomingOffset { get; set; } = 0x607C;
|
||||
|
||||
/// <summary>
|
||||
/// 0x609A - [RW] UINT32 (4 bytes) - Homing Acceleration
|
||||
/// Gia tốc khi thực hiện homing (user units/s²)
|
||||
/// </summary>
|
||||
public ushort HomingAcceleration { get; set; } = 0x609A;
|
||||
|
||||
// ==================== Interpolation (Cyclic Synchronous Modes) ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x60C2 - [RW] UINT8 (1 byte) - Interpolation Time Period
|
||||
/// Chu kỳ thời gian giữa các setpoint trong CSP/CSV/CST mode (ms)
|
||||
/// Thường là 1-10ms, phải match với cycle time của master
|
||||
/// </summary>
|
||||
public ushort InterpolationTimePeriod { get; set; } = 0x60C2;
|
||||
|
||||
/// <summary>
|
||||
/// 0x60C1 - [RW] Variable - Interpolation Data Record
|
||||
/// Buffer chứa dữ liệu interpolation cho cyclic modes
|
||||
/// Dùng trong các mode CSP, CSV, CST
|
||||
/// </summary>
|
||||
public ushort InterpolationDataRecord { get; set; } = 0x60C1;
|
||||
|
||||
// ==================== Following Error ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x6065 - [RW] UINT32 (4 bytes) - Following Error Window
|
||||
/// Dung sai sai số theo dõi cho phép (encoder counts)
|
||||
/// Nếu |Demand - Actual| > Window → Following error fault
|
||||
/// </summary>
|
||||
public ushort FollowingErrorWindow { get; set; } = 0x6065;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6066 - [RW] UINT16 (2 bytes) - Following Error Timeout
|
||||
/// Thời gian cho phép vượt quá Following Error Window (ms)
|
||||
/// Vượt quá window trong time này → Trigger fault
|
||||
/// </summary>
|
||||
public ushort FollowingErrorTimeout { get; set; } = 0x6066;
|
||||
|
||||
// ==================== Motor Data ====================
|
||||
|
||||
/// <summary>
|
||||
/// 0x6075 - [RW] UINT32 (4 bytes) - Motor Rated Current
|
||||
/// Dòng điện định mức của motor (mA)
|
||||
/// Thông số motor được ghi trong datasheet
|
||||
/// </summary>
|
||||
public ushort MotorRatedCurrent { get; set; } = 0x6075;
|
||||
|
||||
/// <summary>
|
||||
/// 0x6076 - [RW] UINT32 (4 bytes) - Motor Rated Torque
|
||||
/// Mô-men xoắn định mức của motor (mNm - milli Newton-meter)
|
||||
/// Basis cho tính toán phần trăm torque (1000 = rated torque)
|
||||
/// </summary>
|
||||
public ushort MotorRatedTorque { get; set; } = 0x6076;
|
||||
|
||||
// ==================== Factory Methods ====================
|
||||
|
||||
/// <summary>
|
||||
/// Tạo Object Dictionary mặc định theo CiA402 standard
|
||||
/// </summary>
|
||||
public static CiA402ObjectDictionary CreateDefault()
|
||||
{
|
||||
return new CiA402ObjectDictionary();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tạo Object Dictionary với custom addresses (cho vendor-specific devices)
|
||||
/// </summary>
|
||||
/// <param name="configure">Action để customize addresses</param>
|
||||
/// <example>
|
||||
/// var dict = CiA402ObjectDictionary.CreateCustom(od => {
|
||||
/// od.Controlword = 0x2040; // Vendor-specific address
|
||||
/// od.TargetPosition = 0x2100;
|
||||
/// });
|
||||
/// </example>
|
||||
public static CiA402ObjectDictionary CreateCustom(Action<CiA402ObjectDictionary> configure)
|
||||
{
|
||||
var dict = new CiA402ObjectDictionary();
|
||||
configure(dict);
|
||||
return dict;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clone Object Dictionary (tạo bản sao độc lập)
|
||||
/// </summary>
|
||||
public CiA402ObjectDictionary Clone()
|
||||
{
|
||||
return (CiA402ObjectDictionary)MemberwiseClone();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Validate all object addresses (check for conflicts)
|
||||
/// </summary>
|
||||
public ValidationResult ValidateAddresses()
|
||||
{
|
||||
var addresses = new Dictionary<ushort, string>
|
||||
{
|
||||
{ Controlword, nameof(Controlword) },
|
||||
{ Statusword, nameof(Statusword) },
|
||||
{ ModesOfOperation, nameof(ModesOfOperation) },
|
||||
{ ModesOfOperationDisplay, nameof(ModesOfOperationDisplay) },
|
||||
{ PositionActualValue, nameof(PositionActualValue) },
|
||||
{ PositionDemandValue, nameof(PositionDemandValue) },
|
||||
{ TargetPosition, nameof(TargetPosition) },
|
||||
{ PositionWindow, nameof(PositionWindow) },
|
||||
{ PositionWindowTime, nameof(PositionWindowTime) },
|
||||
{ PositionRangeLimit, nameof(PositionRangeLimit) },
|
||||
{ SoftwarePositionLimit, nameof(SoftwarePositionLimit) },
|
||||
{ ProfileVelocity, nameof(ProfileVelocity) },
|
||||
{ ProfileAcceleration, nameof(ProfileAcceleration) },
|
||||
{ ProfileDeceleration, nameof(ProfileDeceleration) },
|
||||
{ QuickStopDeceleration, nameof(QuickStopDeceleration) },
|
||||
{ MotionProfileType, nameof(MotionProfileType) },
|
||||
{ VelocityActualValue, nameof(VelocityActualValue) },
|
||||
{ VelocityDemandValue, nameof(VelocityDemandValue) },
|
||||
{ TargetVelocity, nameof(TargetVelocity) },
|
||||
{ VelocityWindow, nameof(VelocityWindow) },
|
||||
{ VelocityWindowTime, nameof(VelocityWindowTime) },
|
||||
{ VelocityThreshold, nameof(VelocityThreshold) },
|
||||
{ MaxProfileVelocity, nameof(MaxProfileVelocity) },
|
||||
{ MaxMotorSpeed, nameof(MaxMotorSpeed) },
|
||||
{ TorqueActualValue, nameof(TorqueActualValue) },
|
||||
{ TargetTorque, nameof(TargetTorque) },
|
||||
{ MaxTorque, nameof(MaxTorque) },
|
||||
{ TorqueSlope, nameof(TorqueSlope) },
|
||||
{ HomingMethod, nameof(HomingMethod) },
|
||||
{ HomingSpeed, nameof(HomingSpeed) },
|
||||
{ HomingOffset, nameof(HomingOffset) },
|
||||
{ HomingAcceleration, nameof(HomingAcceleration) },
|
||||
{ InterpolationTimePeriod, nameof(InterpolationTimePeriod) },
|
||||
{ InterpolationDataRecord, nameof(InterpolationDataRecord) },
|
||||
{ FollowingErrorWindow, nameof(FollowingErrorWindow) },
|
||||
{ FollowingErrorTimeout, nameof(FollowingErrorTimeout) },
|
||||
{ MotorRatedCurrent, nameof(MotorRatedCurrent) },
|
||||
{ MotorRatedTorque, nameof(MotorRatedTorque) }
|
||||
};
|
||||
|
||||
var duplicates = addresses.GroupBy(x => x.Key)
|
||||
.Where(g => g.Count() > 1)
|
||||
.ToList();
|
||||
|
||||
if (duplicates.Any())
|
||||
{
|
||||
var errors = duplicates.Select(g =>
|
||||
$"Address 0x{g.Key:X4} is used by: {string.Join(", ", g.Select(x => x.Value))}")
|
||||
.ToList();
|
||||
return ValidationResult.Failure(errors);
|
||||
}
|
||||
|
||||
return ValidationResult.Success();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get object address by name
|
||||
/// </summary>
|
||||
public ushort? GetAddress(string objectName)
|
||||
{
|
||||
var property = GetType().GetProperty(objectName);
|
||||
return property?.GetValue(this) as ushort?;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Set object address by name
|
||||
/// </summary>
|
||||
public bool SetAddress(string objectName, ushort address)
|
||||
{
|
||||
var property = GetType().GetProperty(objectName);
|
||||
if (property?.PropertyType == typeof(ushort))
|
||||
{
|
||||
property.SetValue(this, address);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get all object addresses as dictionary
|
||||
/// </summary>
|
||||
public Dictionary<string, ushort> GetAllAddresses()
|
||||
{
|
||||
return GetType().GetProperties()
|
||||
.Where(p => p.PropertyType == typeof(ushort))
|
||||
.ToDictionary(p => p.Name, p => (ushort)p.GetValue(this)!);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
namespace RobotNet10.CANOpen.CiA402.Models;
|
||||
|
||||
public readonly struct Controlword
|
||||
{
|
||||
public ushort Value { get; init; }
|
||||
|
||||
public Controlword(ushort value)
|
||||
{
|
||||
Value = value;
|
||||
}
|
||||
|
||||
public bool SwitchOn => (Value & 0x0001) != 0;
|
||||
public bool EnableVoltage => (Value & 0x0002) != 0;
|
||||
public bool QuickStop => (Value & 0x0004) != 0;
|
||||
public bool EnableOperation => (Value & 0x0008) != 0;
|
||||
public bool FaultReset => (Value & 0x0080) != 0;
|
||||
public bool Halt => (Value & 0x0100) != 0;
|
||||
|
||||
public static Controlword Shutdown => new(0x0006);
|
||||
public static Controlword SwitchOnCmd => new(0x0007);
|
||||
public static Controlword DisableVoltage => new(0x0000);
|
||||
public static Controlword QuickStopCmd => new(0x0002);
|
||||
public static Controlword DisableOperation => new(0x0007);
|
||||
public static Controlword EnableOperationCmd => new(0x000F);
|
||||
public static Controlword FaultResetCmd => new(0x0080);
|
||||
|
||||
public override string ToString() => $"0x{Value:X4}";
|
||||
|
||||
public static implicit operator ushort(Controlword cw) => cw.Value;
|
||||
public static implicit operator Controlword(ushort value) => new(value);
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
using RobotNet10.CANOpen.CiA402.Enums;
|
||||
|
||||
namespace RobotNet10.CANOpen.CiA402.Models;
|
||||
|
||||
public readonly struct Statusword(ushort value)
|
||||
{
|
||||
public ushort Value { get; init; } = value;
|
||||
|
||||
public bool ReadyToSwitchOn => (Value & 0x0001) != 0;
|
||||
public bool SwitchedOn => (Value & 0x0002) != 0;
|
||||
public bool OperationEnabled => (Value & 0x0004) != 0;
|
||||
public bool Fault => (Value & 0x0008) != 0;
|
||||
public bool VoltageEnabled => (Value & 0x0010) != 0;
|
||||
public bool QuickStop => (Value & 0x0020) != 0;
|
||||
public bool SwitchOnDisabled => (Value & 0x0040) != 0;
|
||||
public bool Warning => (Value & 0x0080) != 0;
|
||||
public bool TargetReached => (Value & 0x0400) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// CiA402 Statusword bit 12: Homing attained (homing completed successfully)
|
||||
/// </summary>
|
||||
public bool HomingAttained => (Value & 0x1000) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// CiA402 Statusword bit 13: Homing error (homing failed)
|
||||
/// </summary>
|
||||
public bool HomingError => (Value & 0x2000) != 0;
|
||||
|
||||
public DriveState GetState()
|
||||
{
|
||||
if (!ReadyToSwitchOn && !SwitchedOn && !OperationEnabled && !Fault && SwitchOnDisabled)
|
||||
return DriveState.SwitchOnDisabled;
|
||||
|
||||
if (!ReadyToSwitchOn && !SwitchedOn && !OperationEnabled && !Fault && !SwitchOnDisabled)
|
||||
return DriveState.NotReadyToSwitchOn;
|
||||
|
||||
if (ReadyToSwitchOn && !SwitchedOn && !OperationEnabled && !Fault)
|
||||
return DriveState.ReadyToSwitchOn;
|
||||
|
||||
if (ReadyToSwitchOn && SwitchedOn && !OperationEnabled && !Fault)
|
||||
return DriveState.SwitchedOn;
|
||||
|
||||
if (ReadyToSwitchOn && SwitchedOn && OperationEnabled && !Fault)
|
||||
return DriveState.OperationEnabled;
|
||||
|
||||
if (!ReadyToSwitchOn && !SwitchedOn && !OperationEnabled && Fault)
|
||||
return DriveState.Fault;
|
||||
|
||||
return DriveState.Unknown;
|
||||
}
|
||||
|
||||
public override string ToString() => $"0x{Value:X4} ({GetState()})";
|
||||
|
||||
public static implicit operator ushort(Statusword sw) => sw.Value;
|
||||
public static implicit operator Statusword(ushort value) => new(value);
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net10.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\RobotNet10.CANOpen\RobotNet10.CANOpen.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,338 @@
|
||||
using System;
|
||||
using RobotNet10.CANOpen.Models;
|
||||
using RobotNet10.CANOpen.CiA402.Models;
|
||||
|
||||
namespace RobotNet10.CANOpen.CiA402.Strategies;
|
||||
|
||||
/// <summary>
|
||||
/// Interface cho PDO multiplexing strategy
|
||||
/// </summary>
|
||||
public interface IPdoMultiplexingStrategy : IDisposable
|
||||
{
|
||||
/// <summary>
|
||||
/// Configure PDO mappings for the strategy
|
||||
/// </summary>
|
||||
void ConfigurePdoMappings(byte nodeId, Action<PdoConfiguration> configureRpdo, Action<PdoConfiguration> configureTpdo);
|
||||
|
||||
/// <summary>
|
||||
/// Handle incoming PDO data
|
||||
/// </summary>
|
||||
void HandlePdoData(byte pdoNumber, byte[] data, Action<int, byte[]> motorDataHandler);
|
||||
|
||||
/// <summary>
|
||||
/// Send control data for specific motor
|
||||
/// </summary>
|
||||
void SendMotorControl(int motorIndex, byte[] controlData, Action<byte, byte[]> sendRpdo);
|
||||
|
||||
/// <summary>
|
||||
/// Number of motors supported
|
||||
/// </summary>
|
||||
int MotorCount { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Time-multiplexed strategy - xoay vòng giữa các motors theo thời gian
|
||||
/// </summary>
|
||||
public class TimeMultiplexedStrategy : IPdoMultiplexingStrategy
|
||||
{
|
||||
private int _currentMotorIndex = 0;
|
||||
private readonly Timer _rotationTimer;
|
||||
private readonly int _rotationIntervalMs;
|
||||
|
||||
public int MotorCount { get; }
|
||||
|
||||
public TimeMultiplexedStrategy(int motorCount, int rotationIntervalMs = 100)
|
||||
{
|
||||
MotorCount = motorCount;
|
||||
_rotationIntervalMs = rotationIntervalMs;
|
||||
_rotationTimer = new Timer(RotateMotor, null, rotationIntervalMs, rotationIntervalMs);
|
||||
}
|
||||
|
||||
private void RotateMotor(object? state)
|
||||
{
|
||||
_currentMotorIndex = (_currentMotorIndex + 1) % MotorCount;
|
||||
}
|
||||
|
||||
public void ConfigurePdoMappings(byte nodeId, Action<PdoConfiguration> configureRpdo, Action<PdoConfiguration> configureTpdo)
|
||||
{
|
||||
// RPDO1-3: Individual Controlwords
|
||||
for (byte i = 1; i <= Math.Min(MotorCount, 3); i++)
|
||||
{
|
||||
var rpdo = new PdoConfiguration(i, (uint)(0x200 + (i-1) * 0x100 + nodeId));
|
||||
// Motor controlword mapping would be added by caller
|
||||
configureRpdo(rpdo);
|
||||
}
|
||||
|
||||
// RPDO4: Multiplexed position control
|
||||
var rpdo4 = new PdoConfiguration(4, (uint)(0x500 + nodeId));
|
||||
// Motor ID + Position data
|
||||
configureRpdo(rpdo4);
|
||||
|
||||
// TPDO1-3: Individual Status + Position
|
||||
for (byte i = 1; i <= Math.Min(MotorCount, 3); i++)
|
||||
{
|
||||
var tpdo = new PdoConfiguration(i, (uint)(0x180 + (i-1) * 0x100 + nodeId));
|
||||
// Statusword + Position mapping would be added by caller
|
||||
configureTpdo(tpdo);
|
||||
}
|
||||
|
||||
// TPDO4: Multiplexed velocity + torque (rotates between motors)
|
||||
var tpdo4 = new PdoConfiguration(4, (uint)(0x480 + nodeId));
|
||||
// Motor ID + Velocity + Torque
|
||||
configureTpdo(tpdo4);
|
||||
}
|
||||
|
||||
public void HandlePdoData(byte pdoNumber, byte[] data, Action<int, byte[]> motorDataHandler)
|
||||
{
|
||||
if (pdoNumber <= 3)
|
||||
{
|
||||
// Direct mapping: PDO1 -> Motor 0, PDO2 -> Motor 1, PDO3 -> Motor 2
|
||||
int motorIndex = pdoNumber - 1;
|
||||
if (motorIndex < MotorCount)
|
||||
{
|
||||
motorDataHandler(motorIndex, data);
|
||||
}
|
||||
}
|
||||
else if (pdoNumber == 4)
|
||||
{
|
||||
// Multiplexed data - current motor được xác định bởi timer
|
||||
motorDataHandler(_currentMotorIndex, data);
|
||||
}
|
||||
}
|
||||
|
||||
public void SendMotorControl(int motorIndex, byte[] controlData, Action<byte, byte[]> sendRpdo)
|
||||
{
|
||||
if (motorIndex < 3)
|
||||
{
|
||||
// Send via dedicated RPDO (1-3)
|
||||
sendRpdo((byte)(motorIndex + 1), controlData);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Send via multiplexed RPDO4 với motor ID prefix
|
||||
var multiplexedData = new byte[controlData.Length + 1];
|
||||
multiplexedData[0] = (byte)motorIndex; // Motor ID
|
||||
controlData.CopyTo(multiplexedData, 1);
|
||||
|
||||
sendRpdo(4, multiplexedData);
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
_rotationTimer?.Dispose();
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Priority-based strategy - motors có priority khác nhau
|
||||
/// </summary>
|
||||
public class PriorityBasedStrategy : IPdoMultiplexingStrategy
|
||||
{
|
||||
private readonly int[] _motorPriorities;
|
||||
private readonly Dictionary<int, DateTime> _lastUpdateTimes;
|
||||
private readonly int _highPriorityIntervalMs;
|
||||
private readonly int _lowPriorityIntervalMs;
|
||||
|
||||
public int MotorCount { get; }
|
||||
|
||||
public PriorityBasedStrategy(int[] motorPriorities, int highPriorityIntervalMs = 10, int lowPriorityIntervalMs = 100)
|
||||
{
|
||||
_motorPriorities = motorPriorities;
|
||||
MotorCount = motorPriorities.Length;
|
||||
_highPriorityIntervalMs = highPriorityIntervalMs;
|
||||
_lowPriorityIntervalMs = lowPriorityIntervalMs;
|
||||
_lastUpdateTimes = new Dictionary<int, DateTime>();
|
||||
}
|
||||
|
||||
public void ConfigurePdoMappings(byte nodeId, Action<PdoConfiguration> configureRpdo, Action<PdoConfiguration> configureTpdo)
|
||||
{
|
||||
// High priority motors get dedicated PDOs
|
||||
// Low priority motors share multiplexed PDOs
|
||||
|
||||
var highPriorityMotors = _motorPriorities
|
||||
.Select((priority, index) => new { Priority = priority, Index = index })
|
||||
.Where(x => x.Priority >= 8) // High priority threshold
|
||||
.Take(3) // Max 3 dedicated PDOs
|
||||
.ToList();
|
||||
|
||||
// Configure dedicated PDOs for high priority motors
|
||||
for (int i = 0; i < highPriorityMotors.Count; i++)
|
||||
{
|
||||
var rpdo = new PdoConfiguration((byte)(i + 1), (uint)(0x200 + i * 0x100 + nodeId));
|
||||
configureRpdo(rpdo);
|
||||
|
||||
var tpdo = new PdoConfiguration((byte)(i + 1), (uint)(0x180 + i * 0x100 + nodeId));
|
||||
configureTpdo(tpdo);
|
||||
}
|
||||
|
||||
// Remaining PDO for low priority multiplexing
|
||||
if (highPriorityMotors.Count < 4)
|
||||
{
|
||||
var rpdo4 = new PdoConfiguration(4, (uint)(0x500 + nodeId));
|
||||
configureRpdo(rpdo4);
|
||||
|
||||
var tpdo4 = new PdoConfiguration(4, (uint)(0x480 + nodeId));
|
||||
configureTpdo(tpdo4);
|
||||
}
|
||||
}
|
||||
|
||||
public void HandlePdoData(byte pdoNumber, byte[] data, Action<int, byte[]> motorDataHandler)
|
||||
{
|
||||
if (pdoNumber <= 3)
|
||||
{
|
||||
// Dedicated PDO mapping
|
||||
var highPriorityMotors = _motorPriorities
|
||||
.Select((priority, index) => new { Priority = priority, Index = index })
|
||||
.Where(x => x.Priority >= 8)
|
||||
.Take(3)
|
||||
.ToList();
|
||||
|
||||
if (pdoNumber - 1 < highPriorityMotors.Count)
|
||||
{
|
||||
var motorIndex = highPriorityMotors[pdoNumber - 1].Index;
|
||||
motorDataHandler(motorIndex, data);
|
||||
}
|
||||
}
|
||||
else if (pdoNumber == 4 && data.Length > 0)
|
||||
{
|
||||
// Multiplexed PDO - first byte is motor ID
|
||||
int motorIndex = data[0];
|
||||
if (motorIndex < MotorCount)
|
||||
{
|
||||
var motorData = data.Skip(1).ToArray();
|
||||
motorDataHandler(motorIndex, motorData);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void SendMotorControl(int motorIndex, byte[] controlData, Action<byte, byte[]> sendRpdo)
|
||||
{
|
||||
var highPriorityMotors = _motorPriorities
|
||||
.Select((priority, index) => new { Priority = priority, Index = index })
|
||||
.Where(x => x.Priority >= 8)
|
||||
.Take(3)
|
||||
.ToList();
|
||||
|
||||
// Check if motor has dedicated PDO
|
||||
var dedicatedPdoIndex = highPriorityMotors.FindIndex(x => x.Index == motorIndex);
|
||||
|
||||
if (dedicatedPdoIndex >= 0)
|
||||
{
|
||||
// Send via dedicated PDO
|
||||
sendRpdo((byte)(dedicatedPdoIndex + 1), controlData);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Send via multiplexed PDO4
|
||||
var multiplexedData = new byte[controlData.Length + 1];
|
||||
multiplexedData[0] = (byte)motorIndex;
|
||||
controlData.CopyTo(multiplexedData, 1);
|
||||
|
||||
sendRpdo(4, multiplexedData);
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
// PriorityBasedStrategy doesn't use any disposable resources
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adaptive strategy - tự động điều chỉnh dựa trên activity
|
||||
/// </summary>
|
||||
public class AdaptiveStrategy : IPdoMultiplexingStrategy
|
||||
{
|
||||
private readonly Dictionary<int, MotorActivity> _motorActivities;
|
||||
private readonly Timer _adaptiveTimer;
|
||||
|
||||
public int MotorCount { get; }
|
||||
|
||||
private class MotorActivity
|
||||
{
|
||||
public int UpdateCount { get; set; }
|
||||
public DateTime LastUpdate { get; set; }
|
||||
public bool IsActive => DateTime.UtcNow - LastUpdate < TimeSpan.FromSeconds(5);
|
||||
public double ActivityScore => IsActive ? UpdateCount / Math.Max(1, (DateTime.UtcNow - LastUpdate).TotalSeconds) : 0;
|
||||
}
|
||||
|
||||
public AdaptiveStrategy(int motorCount)
|
||||
{
|
||||
MotorCount = motorCount;
|
||||
_motorActivities = Enumerable.Range(0, motorCount)
|
||||
.ToDictionary(i => i, i => new MotorActivity { LastUpdate = DateTime.UtcNow });
|
||||
|
||||
_adaptiveTimer = new Timer(AdaptConfiguration, null, TimeSpan.FromSeconds(10), TimeSpan.FromSeconds(10));
|
||||
}
|
||||
|
||||
private void AdaptConfiguration(object? state)
|
||||
{
|
||||
// Periodically reconfigure based on motor activity
|
||||
var activeMotors = _motorActivities
|
||||
.Where(kv => kv.Value.IsActive)
|
||||
.OrderByDescending(kv => kv.Value.ActivityScore)
|
||||
.Take(3)
|
||||
.Select(kv => kv.Key)
|
||||
.ToList();
|
||||
|
||||
// Logic to reconfigure PDO mappings would go here
|
||||
// For simplicity, just update activity scores
|
||||
foreach (var activity in _motorActivities.Values)
|
||||
{
|
||||
activity.UpdateCount = Math.Max(0, activity.UpdateCount - 1); // Decay
|
||||
}
|
||||
}
|
||||
|
||||
public void ConfigurePdoMappings(byte nodeId, Action<PdoConfiguration> configureRpdo, Action<PdoConfiguration> configureTpdo)
|
||||
{
|
||||
// Similar to time-multiplexed but can be reconfigured
|
||||
for (byte i = 1; i <= 4; i++)
|
||||
{
|
||||
var rpdo = new PdoConfiguration(i, (uint)(0x200 + (i-1) * 0x100 + nodeId));
|
||||
configureRpdo(rpdo);
|
||||
|
||||
var tpdo = new PdoConfiguration(i, (uint)(0x180 + (i-1) * 0x100 + nodeId));
|
||||
configureTpdo(tpdo);
|
||||
}
|
||||
}
|
||||
|
||||
public void HandlePdoData(byte pdoNumber, byte[] data, Action<int, byte[]> motorDataHandler)
|
||||
{
|
||||
// Determine motor index based on current active configuration
|
||||
int motorIndex = (pdoNumber - 1) % MotorCount;
|
||||
|
||||
// Update activity tracking
|
||||
if (_motorActivities.ContainsKey(motorIndex))
|
||||
{
|
||||
var activity = _motorActivities[motorIndex];
|
||||
activity.UpdateCount++;
|
||||
activity.LastUpdate = DateTime.UtcNow;
|
||||
}
|
||||
|
||||
motorDataHandler(motorIndex, data);
|
||||
}
|
||||
|
||||
public void SendMotorControl(int motorIndex, byte[] controlData, Action<byte, byte[]> sendRpdo)
|
||||
{
|
||||
// Update activity
|
||||
if (_motorActivities.ContainsKey(motorIndex))
|
||||
{
|
||||
var activity = _motorActivities[motorIndex];
|
||||
activity.UpdateCount++;
|
||||
activity.LastUpdate = DateTime.UtcNow;
|
||||
}
|
||||
|
||||
// Use cyclic assignment for now
|
||||
byte pdoNumber = (byte)((motorIndex % 4) + 1);
|
||||
sendRpdo(pdoNumber, controlData);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
_adaptiveTimer?.Dispose();
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user