public override int SetMotorPowerOrSpeedRight(int powerOrSpeed)
{
RQCommandMotorPowerRight cmd = new RQCommandMotorPowerRight(powerOrSpeed);
cmd.queue = m_commandPowerRightQueue; // to allow re-queueing of a failed command
m_commandPowerRightQueue.Enqueue(cmd);
return(powerOrSpeed);
}
internal override void interpretResponse(long timestamp)
{
if (!isResponseSane)
{
string errMsg = "bad response to '" + toSend + "' - received '" + received[0] + "' count=" + received.Count;
Tracer.Error(errMsg);
if (queue != null && m_resendCount++ < 3)
{
if (queue.HasInteractionsQueued)
{
Tracer.Trace("--- fresher command in queue, not resending: " + toSend);
}
else
{
Tracer.Trace("--- resending: " + toSend);
queue.Enqueue(this);
}
}
return;
}
long ticksElapsed = 0L;
StringBuilder strb = null;
if (doTrace)
{
ticksElapsed = timestamp - whenSentTicks;
strb = new StringBuilder();
for (int i = 0; i < received.Count; i++)
{
strb.Append(received[i]);
strb.Append(" ");
}
Tracer.Trace("interpretResponse: " + received.Count + " " + String.Format("{0:F1}", ticksElapsed / 10000.0d) + " ms " + strb.ToString());
}
}
private void controlLoop()
{
while (true)
{
lock (this)
{
if (isGrabbed)
{
Tracer.Trace2("grabbed");
lock (m_currentQueuePadlock)
{
if (m_currentQueue != null)
{
// still processing current query
if (m_currentQueue.checkForTimeout())
{
m_currentQueue = null;
}
}
}
if (m_currentQueue == null)
{
// we are for sure not processing any interaction.
// find first queue in priority list that has a processable interaction:
RQInteractionQueue queue = null;
foreach (RQInteractionQueue q in m_queues)
{
if (q.isProcessingInteraction || q.HasInteractionsQueued)
{
queue = q;
break;
}
}
if (queue != null)
{
lock (queue.padlock)
{
if (!queue.isProcessingInteraction)
{
// done with the current interaction, get next one:
if (queue.HasInteractionsQueued)
{
m_currentQueue = queue; // when responses are processed, this is the queue to use.
queue.lastSentTicks = DateTime.Now.Ticks;
m_port.WriteLine(queue.dequeueForSend());
}
}
}
}
else
{
// all queues are empty, replentish the query queue:
foreach (RQQuery q in m_querySet)
{
q.reset();
m_queryQueue.Enqueue(q);
}
}
}
}
else if (isMonitored)
{
Tracer.Trace2("monitored");
}
else if (isOnline)
{
Tracer.Trace2("online - receiving data");
}
else
{
Tracer.Trace2("not connected");
}
logMeasuredValues();
}
Thread.Sleep(1);
}
}