Initial commit
This commit is contained in:
@@ -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