Stream Motion (J519)

var robot = new FanucRobot();
var parameters = new ConnectionParameters("192.168.1.1");
parameters.StreamMotion.Enable = true;
robot.Connect(parameters);

float nextJ1 = 10, nextJ2 = 20, nextJ3 = 30, nextJ4 = 0, nextJ5 = -45, nextJ6 = 90;

robot.StreamMotion.StateReceived += (sender, e) =>
{
    var state = e.State;
    Console.WriteLine($"Position: J1={state.JointPosition.J1:F2}°");

    if (state.Status.ReadyForCommands &&
        !state.Status.InMotion &&
        !state.Status.CommandReceived)
    {
        var command = new CommandPacket
        {
            DataStyle = DataStyle.Joint,
            MotionData = new MotionData
            {
                J1 = nextJ1,
                J2 = nextJ2,
                J3 = nextJ3,
                J4 = nextJ4,
                J5 = nextJ5,
                J6 = nextJ6
            }
        };
        robot.StreamMotion.SendCommand(command);
    }
};

robot.StreamMotion.Start();
// ... your application logic ...
robot.StreamMotion.Stop();

var robot = new FanucRobot();

// Connect with default settings (port 60015)
var parameters = new ConnectionParameters("192.168.1.1");
parameters.StreamMotion.Enable = true;
robot.Connect(parameters);

// Standalone client
var client = new StreamMotionClient();
client.Connect("192.168.1.1");

// Start / stop streaming
robot.StreamMotion.Start();
Console.WriteLine($"Streaming: {robot.StreamMotion.IsStreaming}");
Console.WriteLine($"Frequency: {robot.StreamMotion.MeasuredFrequency:F1} Hz");
robot.StreamMotion.Stop();
robot.StreamMotion.Disconnect();

robot.StreamMotion.StateReceived += (sender, e) =>
{
    var state = e.State;

    // Read current joint positions
    Console.WriteLine($"J1: {state.JointPosition.J1:F3}°");
    Console.WriteLine($"J2: {state.JointPosition.J2:F3}°");

    // Read Cartesian position
    Console.WriteLine($"X: {state.CartesianPosition.X:F2} mm");
    Console.WriteLine($"Y: {state.CartesianPosition.Y:F2} mm");

    // Check status
    Console.WriteLine($"Ready: {state.Status.ReadyForCommands}");
    Console.WriteLine($"In Motion: {state.Status.InMotion}");

    // Read motor currents
    Console.WriteLine($"Motor 1: {state.MotorCurrents.Axis1:F2} A");

    // Only send command when ready
    bool canSend = state.Status.ReadyForCommands &&
                   !state.Status.CommandReceived &&
                   !state.Status.InMotion;
};

// MotionData: 9 axis values
var motion = new MotionData();
motion.J1 = 10.0f; motion.J2 = 20.0f; motion.J3 = 30.0f;
motion.J4 = 0.0f; motion.J5 = -45.0f; motion.J6 = 90.0f;
// Cartesian: motion.X, motion.Y, motion.Z, motion.W, motion.P, motion.R

// Full command packet
var command = new CommandPacket
{
    DataStyle = DataStyle.Joint,
    MotionData = new MotionData { J1 = 10, J2 = 20, J3 = 30, J4 = 0, J5 = -45, J6 = 90 },
    IsLastData = false
};
robot.StreamMotion.SendCommand(command);

// Convenience: joint motion
robot.StreamMotion.SendJointCommand(
    new MotionData { J1 = 10, J2 = 20, J3 = 30, J4 = 0, J5 = -45, J6 = 90 },
    isLastData: false
);

// Convenience: Cartesian motion
robot.StreamMotion.SendCartesianCommand(
    new MotionData { X = 500, Y = 100, Z = 300, W = 180, P = 0, R = 0 },
    isLastData: false
);

// Simple linear interpolator (educational)
public static float[] LinearInterpolate(float[] start, float[] target, double t)
{
    float[] pos = new float[6];
    for (int i = 0; i < 6; i++)
        pos[i] = start[i] + (float)(t * (target[i] - start[i]));
    return pos;
}

// S-curve interpolation (recommended for smooth motion)
public static float[] SCurveInterpolate(float[] start, float[] target, double t)
{
    // S-curve: s(t) = 6t^5 - 15t^4 + 10t^3
    double s = t * t * t * (t * (t * 6 - 15) + 10);
    float[] pos = new float[6];
    for (int i = 0; i < 6; i++)
        pos[i] = start[i] + (float)(s * (target[i] - start[i]));
    return pos;
}

// Query velocity limits for axis 1
var velocityAck = robot.StreamMotion.RequestThreshold(1, ThresholdType.Velocity);
Console.WriteLine($"Axis 1 max velocity: {velocityAck.ThresholdsNoLoad[19]:F2} deg/s");

// Query acceleration limits
var accelAck = robot.StreamMotion.RequestThreshold(1, ThresholdType.Acceleration);

// Query jerk limits
var jerkAck = robot.StreamMotion.RequestThreshold(1, ThresholdType.Jerk);

// Query all axes
for (uint axis = 1; axis <= 6; axis++)
{
    var ack = robot.StreamMotion.RequestThreshold(axis, ThresholdType.Velocity);
    Console.WriteLine($"J{axis} max velocity: {ack.ThresholdsNoLoad[19]:F2} deg/s");
}

var motionData = new MotionData { J1 = 10, J2 = 20, J3 = 30, J4 = 0, J5 = -45, J6 = 90 };

// Read I/O during motion
var readCommand = new CommandPacket
{
    DataStyle = DataStyle.Joint,
    MotionData = motionData,
    ReadIOType = IOType.DI,
    ReadIOIndex = 1,
    ReadIOMask = 0xFFFF
};
robot.StreamMotion.SendCommand(readCommand);

// Write I/O during motion
var writeCommand = new CommandPacket
{
    DataStyle = DataStyle.Joint,
    MotionData = motionData,
    WriteIOType = IOType.DO,
    WriteIOIndex = 1,
    WriteIOMask = 0x0001,
    WriteIOValue = 0x0001
};
robot.StreamMotion.SendCommand(writeCommand);

var command = new CommandPacket
{
    DataStyle = DataStyle.Joint,
    MotionData = new MotionData { J1 = 10, J2 = 20, J3 = 30, J4 = 0, J5 = -45, J6 = 90 }
};

// Always check state before sending
robot.StreamMotion.StateReceived += (s, e) =>
{
    if (e.State.Status.ReadyForCommands &&
        !e.State.Status.CommandReceived &&
        !e.State.Status.InMotion)
    {
        robot.StreamMotion.SendCommand(command);
    }
};

// Handle errors
robot.StreamMotion.ReceiveError += (s, e) =>
{
    Console.WriteLine($"Error: {e.Exception.Message}");
};

// Query limits before motion
for (uint i = 1; i <= 6; i++)
{
    var ack = robot.StreamMotion.RequestThreshold(i, ThresholdType.Velocity);
}

// Clean shutdown
try { robot.StreamMotion.Start(); }
finally
{
    if (robot.StreamMotion.IsStreaming) robot.StreamMotion.Stop();
}

robot.StreamMotion.StateReceived += (s, e) =>
{
    var state = e.State;
    Console.WriteLine($"[{DateTime.Now:HH:mm:ss.fff}] " +
        $"Seq={state.SequenceNumber} " +
        $"Ready={state.Status.ReadyForCommands} " +
        $"CmdRcv={state.Status.CommandReceived} " +
        $"Motion={state.Status.InMotion} " +
        $"J1={state.JointPosition.J1:F3}");
};

Console.WriteLine($"Robot frequency: {robot.StreamMotion.RobotFrequency:F1} Hz");
Console.WriteLine($"Measured frequency: {robot.StreamMotion.MeasuredFrequency:F1} Hz");
Console.WriteLine($"Packets received: {robot.StreamMotion.PacketCount}");

class StreamMotionExample
{
    private FanucRobot _robot;
    private float[][] _waypoints;
    private int _currentWaypoint;
    private bool _waitingForAck;
    private readonly object _lock = new object();

    public void Run()
    {
        _robot = new FanucRobot();

        // Define waypoints (J1-J6 in degrees)
        _waypoints = new float[][]
        {
            new float[] { 0, 0, 0, 0, 0, 0 },
            new float[] { 30, 10, -20, 0, 45, 0 },
            new float[] { -30, 20, -40, 90, 0, 45 },
            new float[] { 0, 0, 0, 0, 0, 0 }
        };
        _currentWaypoint = 0;

        try
        {
            // Connect
            Console.WriteLine("Connecting to robot...");
            var parameters = new ConnectionParameters("192.168.1.1");
            parameters.StreamMotion.Enable = true;
            _robot.Connect(parameters);
            Console.WriteLine("Connected!");

            // Query axis limits
            for (uint axis = 1; axis <= 6; axis++)
            {
                var ack = _robot.StreamMotion.RequestThreshold(axis, ThresholdType.Velocity);
                Console.WriteLine($"  J{axis} max velocity: {ack.ThresholdsNoLoad[19]:F1} deg/s");
            }

            // Setup event handlers
            _robot.StreamMotion.StateReceived += OnStateReceived;
            _robot.StreamMotion.ReceiveError += OnReceiveError;

            // Start streaming
            _robot.StreamMotion.Start();

            // Wait for user to stop
            Console.WriteLine("Moving through waypoints... Press Enter to stop.");
            Console.ReadLine();
        }
        finally
        {
            // Clean shutdown
            if (_robot.StreamMotion.IsStreaming)
                _robot.StreamMotion.Stop();
            _robot.StreamMotion.Disconnect();
        }
    }

    private void OnStateReceived(object sender, StateReceivedEventArgs e)
    {
        lock (_lock)
        {
            var status = e.State.Status;

            if (_waitingForAck)
            {
                if (!status.InMotion && !status.CommandReceived)
                    _waitingForAck = false;
                else
                    return;
            }

            if (!status.ReadyForCommands || status.InMotion || status.CommandReceived)
                return;

            // Advance to next waypoint
            _currentWaypoint = (_currentWaypoint + 1) % _waypoints.Length;
            float[] target = _waypoints[_currentWaypoint];

            Console.WriteLine($"Moving to waypoint {_currentWaypoint}");

            var command = new CommandPacket
            {
                DataStyle = DataStyle.Joint,
                MotionData = new MotionData
                {
                    J1 = target[0],
                    J2 = target[1],
                    J3 = target[2],
                    J4 = target[3],
                    J5 = target[4],
                    J6 = target[5]
                },
                IsLastData = true
            };

            _robot.StreamMotion.SendCommand(command);
            _waitingForAck = true;
        }
    }

    private void OnReceiveError(object sender, ReceiveErrorEventArgs e)
    {
        Console.WriteLine($"Error: {e.Exception.Message}");
    }

    static void Main(string[] args)
    {
        new StreamMotionExample().Run();
    }
}