338 lines
12 KiB
C#
338 lines
12 KiB
C#
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);
|
|
}
|
|
} |