/// <summary>
/// Sends data (IPOC, correction) to the robot, raises <see cref="KUKARobot.FrameSent">FrameSent</see> event
/// </summary>
private void SendData()
{
RobotVector correction;
lock (forceMoveSyncLock) {
correction = generator.GetNextCorrection(position);
}
correction = new RobotVector(correction.X, correction.Y, correction.Z, 0, 0, 0);
// CAŁY IF DO ZAKOMENTOWANIA DLA POZYCJI ABSOLUTNEJ
if (!Limits.CheckCorrection(correction))
{
Uninitialize();
throw new InvalidOperationException("Correction limit has been exceeded:" +
$"{Environment.NewLine}{correction}");
}
OutputFrame outputFrame = new OutputFrame()
{
Correction = correction,
IPOC = IPOC
};
rsiAdapter.SendData(outputFrame);
FrameSent?.Invoke(outputFrame);
}
internal override void OnMessage(string method, JsonElement?serverParams)
{
switch (method)
{
case "close":
Close?.Invoke(this, EventArgs.Empty);
break;
case "frameSent":
FrameSent?.Invoke(
this,
new WebSocketFrame(
serverParams?.GetProperty("data").ToObject <string>(),
serverParams?.GetProperty("opcode").ToObject <int>() == OpcodeBase64));
break;
case "frameReceived":
FrameReceived?.Invoke(
this,
new WebSocketFrame(
serverParams?.GetProperty("data").ToObject <string>(),
serverParams?.GetProperty("opcode").ToObject <int>() == OpcodeBase64));
break;
case "socketError":
SocketError?.Invoke(this, serverParams?.GetProperty("error").ToObject <string>());
break;
}
}
private void SendData(long IPOC)
{
RobotVector correction;
RobotVector currentVelocity;
RobotVector currentAcceleration;
RobotVector targetPosition;
RobotVector targetVelocity;
double targetDuration;
lock (generatorSyncLock) {
// GetNextCorrection() updates theoretical values
correction = generator.GetNextCorrection();
currentVelocity = generator.Velocity;
currentAcceleration = generator.Acceleration;
targetPosition = generator.TargetPosition;
targetVelocity = generator.TargetVelocity;
targetDuration = generator.TargetDuration;
}
if (!Limits.CheckCorrection(correction))
{
throw new InvalidOperationException("Correction limit has been exceeded:" +
$"{Environment.NewLine}{correction}");
}
if (!Limits.CheckVelocity(currentVelocity))
{
throw new InvalidOperationException("Velocity limit has been exceeded:" +
$"{Environment.NewLine}{currentVelocity}");
}
if (!Limits.CheckAcceleration(currentAcceleration))
{
throw new InvalidOperationException("Acceleration limit has been exceeded:" +
$"{Environment.NewLine}{currentAcceleration}");
}
OutputFrame outputFrame = new OutputFrame()
{
Correction = correction,
IPOC = IPOC
};
rsiAdapter.SendData(outputFrame);
FrameSent?.Invoke(this, new FrameSentEventArgs {
FrameSent = outputFrame,
Position = position,
TargetPosition = targetPosition,
TargetVelocity = targetVelocity,
TargetDuration = targetDuration
});
}
private void RequestSendingMessage(TxMessageBuffer buffer)
{
var message = buffer.UnloadMessage(swapEndian.Value);
if (message == null)
{
this.Log(LogLevel.Error, "No message in mailbox.");
return;
}
FrameSent?.Invoke(message);
this.Log(LogLevel.Info, "Message sent: {0}.", message);
txMessageInterruptsStatus.Value = true;
UpdateInterrupts();
}
internal WebSocket(IChannelOwner parent, string guid, WebSocketInitializer initializer) : base(parent, guid)
{
_channel = new(guid, parent.Connection, this);
_initializer = initializer;
_channel.Close += (_, _) =>
{
IsClosed = true;
Close?.Invoke(this, this);
};
_channel.FrameReceived += (_, e) => FrameReceived?.Invoke(this, e);
_channel.FrameSent += (_, e) => FrameSent?.Invoke(this, e);
_channel.SocketError += (_, e) => SocketError?.Invoke(this, e);
}
internal override void OnMessage(string method, JsonElement?serverParams)
{
bool IsTextOrBinaryFrame(out int opcode)
{
opcode = serverParams?.GetProperty("opcode").ToObject <int>() ?? 0;
return(opcode != 1 && opcode != 2);
}
int opcode;
switch (method)
{
case "close":
Close?.Invoke(this, EventArgs.Empty);
break;
case "frameSent":
if (IsTextOrBinaryFrame(out opcode))
{
break;
}
FrameSent?.Invoke(
this,
new WebSocketFrame(
serverParams?.GetProperty("data").ToObject <string>(),
opcode == OpcodeBase64));
break;
case "frameReceived":
if (IsTextOrBinaryFrame(out opcode))
{
break;
}
FrameReceived?.Invoke(
this,
new WebSocketFrame(
serverParams?.GetProperty("data").ToObject <string>(),
opcode == OpcodeBase64));
break;
case "socketError":
SocketError?.Invoke(this, serverParams?.GetProperty("error").ToObject <string>());
break;
}
}
private void SendData()
{
if (txQueue.Count == 0)
{
this.Log(LogLevel.Warning, "Attempted to transmit an empty frame.");
return;
}
irqHandler.RequestInterrupt(InterruptSource.StartOfFrameDelimiter);
//ignore the first byte, it's length. Don't drop it though, as the same packet might get resent.
var crc = Frame.CalculateCRC(txQueue.Skip(1));
var frame = new Frame(txQueue.Skip(1).Concat(crc).ToArray());
this.DebugLog("Sending frame {0}.", frame.Bytes.Select(x => "0x{0:X}".FormatWith(x)).Stringify());
FrameSent?.Invoke(this, frame.Bytes);
irqHandler.RequestInterrupt(InterruptSource.TxDone);
}
private void SendData(long IPOC)
{
correction = generator.GetNextCorrection();
if (!Limits.CheckCorrection(correction))
{
Uninitialize();
throw new InvalidOperationException("Correction limit has been exceeded:" +
$"{Environment.NewLine}{correction}");
}
OutputFrame outputFrame = new OutputFrame()
{
Correction = correction,
IPOC = IPOC
};
FrameSent?.Invoke(this, new FrameSentEventArgs {
FrameSent = outputFrame,
Position = position
});
}
private void SendPacket()
{
var dataAddress = packetPointer.Value;
var headerLengthInAir = HeaderLengthInAir();
var addressLength = (int)baseAddressLength.Value + 1;
var headerLengthInRAM = HeaderLengthInRAM();
if (headerLengthInAir != headerLengthInRAM)
{
this.Log(LogLevel.Noisy, "Header length difference between onAir={0} and inRam={1}", headerLengthInAir, headerLengthInRAM);
}
var data = new byte[addressLength + headerLengthInRAM + maxPacketLength.Value];
FillCurrentAddress(data, 0, txAddress.Value);
machine.SystemBus.ReadBytes(dataAddress, headerLengthInRAM, data, addressLength);
this.Log(LogLevel.Noisy, "Header: {0} S0 {1} Length {2} S1 {3} s1inc {4}", Misc.PrettyPrintCollectionHex(data), s0Length.Value, lengthFieldLength.Value, s1Length.Value, s1Include.Value);
var payloadLength = data[addressLength + s0Length.Value];
if (payloadLength > maxPacketLength.Value)
{
this.Log(LogLevel.Error, "Payload length ({0}) longer than the max packet length ({1}), trimming...", payloadLength, maxPacketLength.Value);
payloadLength = (byte)maxPacketLength.Value;
}
machine.SystemBus.ReadBytes((ulong)(dataAddress + headerLengthInRAM), payloadLength, data, addressLength + headerLengthInAir);
this.Log(LogLevel.Noisy, "Data: {0} Maxlen {1} statlen {2}", Misc.PrettyPrintCollectionHex(data), maxPacketLength.Value, staticLength.Value);
FrameSent?.Invoke(this, data);
var crcLen = 4;
ScheduleRadioEvents((uint)(headerLengthInAir + payloadLength + crcLen));
LogUnhandledShort(shorts.EndStart, nameof(shorts.EndStart)); // not sure how to support it. It's instant from our perspective.
}
internal override void OnMessage(string method, JsonElement?serverParams)
{
switch (method)
{
case "close":
Close?.Invoke(this, EventArgs.Empty);
break;
case "frameSent":
FrameSent?.Invoke(
this,
new WebSocketFrameEventArgs
{
Payload = serverParams?.GetProperty("data").ToObject <string>(),
});
break;
case "frameReceived":
FrameReceived?.Invoke(
this,
new WebSocketFrameEventArgs
{
Payload = serverParams?.GetProperty("data").ToObject <string>(),
});
break;
case "error":
SocketError?.Invoke(
this,
new WebSocketErrorEventArgs
{
ErrorMessage = serverParams?.GetProperty("error").ToObject <string>(),
});
break;
}
}
protected virtual void HandleFrame(byte[] frame)
{
FrameSent?.Invoke(this, frame);
}
