private void setState(byte[] machineStateBuffer)
{
lock (_L_instructionQueue)
{
_currentState.SetState(machineStateBuffer);
_plannedState = _currentState.Copy();
Monitor.Pulse(_L_instructionQueue);
}
reportConfirmation();
}
private void _port_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
var data = readData(_port);
foreach (var dataByte in data)
{
var response = (char)dataByte;
if (_stateDataRemainingBytes > 0)
{
_stateDataBuffer[_stateDataBuffer.Length - _stateDataRemainingBytes] = (byte)response;
--_stateDataRemainingBytes;
if (_stateDataRemainingBytes == 0)
{
lock (_L_instructionCompletition)
{
_incompleteInstructionQueue.Dequeue();
_currentState.SetState(_stateDataBuffer);
--_incompleteInstructions;
if (_incompleteInstructionQueue.Count > 0)
{
throw new NotSupportedException("State retrieval requires empty queue");
}
_plannedState = _currentState.Copy();
Monitor.Pulse(_L_instructionCompletition);
}
}
continue;
}
if (_isCommentEnabled)
{
if (response == '\n')
{
_isCommentEnabled = false;
}
continue;
}
switch (response)
{
case '1':
//_resetConnection = true;
clearDriverState();
break;
case 'a':
//authentication is required
clearDriverState();
_port.Write("$%#");
break;
case 'A':
//authentication succeeded
break;
case 'I':
//first reset plans - the driver is blocked on expects confirmation
clearDriverState();
send(new StateDataInstruction());
break;
case 'D':
lock (_L_confirmation)
{
_expectsConfirmation = false;
Monitor.Pulse(_L_confirmation);
_stateDataRemainingBytes = _stateDataBuffer.Length;
}
break;
case 'H':
lock (_L_confirmation)
{
_expectsConfirmation = false;
Monitor.Pulse(_L_confirmation);
}
lock (_L_instructionCompletition)
{
onInstructionCompleted();
Monitor.Pulse(_L_instructionCompletition);
}
//homing was finished successfuly
OnHomeCalibrated?.Invoke();
break;
case 'Y':
lock (_L_confirmation)
{
_expectsConfirmation = false;
Monitor.Pulse(_L_confirmation);
}
break;
case 'F':
var incompleteInstrutionCount = 0;
lock (_L_instructionCompletition)
{
onInstructionCompleted();
incompleteInstrutionCount = _incompleteInstructionQueue.Count;
Monitor.Pulse(_L_instructionCompletition);
}
if (incompleteInstrutionCount == 0 && OnInstructionQueueIsComplete != null)
{
OnInstructionQueueIsComplete();
}
break;
case 'S':
//scheduler was enabled
System.Diagnostics.Debug.WriteLine("S: " + IncompleteInstructionCount);
break;
case 'M':
//step time was missed
break;
case 'E':
throw new NotImplementedException("Incomplete message erased");
case '|':
_isCommentEnabled = true;
break;
default:
// throw new NotImplementedException();
System.Diagnostics.Debug.Write(response);
break;
}
}
OnDataReceived?.Invoke(Encoding.ASCII.GetString(data.ToArray()));
}