void HandleOperatorMessage()
{
while (IGC.UnicastListener.HasPendingMessage)
{
var message = IGC.UnicastListener.AcceptMessage();
if (message.Source == _turtleOperatorID)
{
var data = message.Data.ToString();
string[] split = data.Split(';');
_messageBuilder.AppendLine(data);
var toEnqueue = new stateStruct();
var state = split[0];
var param = split[1];
switch (state)
{
case "R": //Rotate
toEnqueue.stateFunc = Rotate;
toEnqueue.paramFunc = () => _rotationGoal = int.Parse(param);
break;
case "F": //forward
break;
case "M": //Move
break;
}
_stateQueue.Enqueue(toEnqueue);
}
}
}
private void beat(object s)
{
stateStruct st = (stateStruct)s;
p_StopVerify = false;
while (!p_Stop)
{
long callbackStart = DateTime.Now.Ticks;
st.callback(st.state);
long nowTicks = DateTime.Now.Ticks;
p_TotalFrames++;
p_FrameSecond++;
p_LastLatency = (nowTicks - callbackStart) * 1.0f / TimeSpan.TicksPerMillisecond;
p_TotalLatency += p_LastLatency;
p_LastTime = nowTicks;
//has one second passed?
if (nowTicks - p_Second >= (TimeSpan.TicksPerSecond))
{
p_FrameRate = p_FrameSecond;
p_FrameSecond = 0;
p_Second = nowTicks;
}
if (p_SleepInterval != -1)
{
Thread.Sleep(p_SleepInterval);
}
}
Console.WriteLine("Thread " + p_Name + " is stopping!");
p_StopVerify = true;
}
public void Main(string argument, UpdateType updateSource)
{
_messageBuilder.Clear();
if ((updateSource & UpdateType.Once) == UpdateType.Once)
{
if (!_suspended & _initialized)
{
_messageBuilder.AppendLine("\tRun");
_runCount++;
_stateMachine.MoveNext();
var sCase = _stateMachine.Current;
switch (sCase)
{
case 0:
break;
case 1:
case 2:
_prevRunCount = _runCount;
_runCount = 0;
_stateMachine.Dispose();
_stateMachine = null;
if (sCase == 1)
{
_stateQueue.Dequeue();
}
if (_stateQueue.Count > 0)
{
stateStruct next = _stateQueue.Peek();
next.paramFunc();
_stateMachine = next.stateFunc();
}
else
{
_stateMachine = Idle();
}
break;
}
if (_runCount == 0)
{
string consoleString = string.Format("Queue: {0}\nState: {1}\nPrevious state took {2} ticks.", _stateQueue.Count, _currentState, _prevRunCount);
Echo(consoleString);
_messageBuilder.AppendLine(consoleString);
Me.CustomData += "\n" + _messageBuilder;
}
}
Runtime.UpdateFrequency |= UpdateFrequency.Once;
return;
}
//receive broadcasts
if ((updateSource & UpdateType.IGC) == UpdateType.IGC)
{
_messageBuilder.AppendLine("UpdateType: IGC");
HandleOperatorMessage();
HandleMessageInit();
Me.CustomData += "\n" + _messageBuilder;
return;
}
if ((updateSource & UpdateType.Trigger) == UpdateType.Trigger)
{
_messageBuilder.AppendLine("UpdateType: Trigger");
// sensor abrupt stop set _suspend
Me.CustomData += "\n" + _messageBuilder;
return;
}
}
