Initial commit
This commit is contained in:
@@ -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)!);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user