public override void ResetEncoderLeft()
{
RQCommandResetEncoderLeft cmd = new RQCommandResetEncoderLeft();
// we want to reset encoder in somewhat synchronous manner - prior to motor commands
cmd.queue = m_commandQueue; // to allow re-queueing of a failed command
m_commandQueue.Enqueue(cmd);
}
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)
{
whenReceivedTicks = timestamp;
if (!isResponseSane)
{
string errMsg = "RQCommand: 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;
}
if (doTrace)
{
long ticksElapsed = 0L;
StringBuilder strb = null;
ticksElapsed = timestamp - whenSentTicks;
strb = new StringBuilder();
for (int i = 0; i < received.Count; i++)
{
strb.Append(received[i]);
strb.Append(" ");
}
Tracer.Trace("RQCommand: interpretResponse: " + received.Count + " " + String.Format("{0:F1}", ticksElapsed / 10000.0d) + " ms " + strb.ToString());
}
}
private bool isShortQueryMode = false; // if true, we are replentishing query queue from the m_querySetShort. when false - from m_querySet
public void ExecuteMain()
{
string statusLabel = "";
lock (this)
{
if (isGrabbed)
{
statusLabel = "grabbed";
lock (m_currentQueuePadlock)
{
if (m_currentQueue != null)
{
// still processing current query
if (m_currentQueue.checkForTimeout())
{
m_currentQueue = null;
statusLabel = "timeout on AX2850 serial connection";
isUnknownState = true;
isInError = true;
}
}
}
trySendNow:
if (m_currentQueue == null && !isInError)
{
// we are (almost) for sure not processing any interaction (talking to the controller).
// if no queue is talking, find first queue in priority list that has a processable interaction (has something to send).
// PriorityCurrent depends on time spent waiting grows fast with time multiplied by the queue priority factor; isProcessingInteraction==true always gets first place.
RQInteractionQueue queue = (from q in m_queues
where q.HasInteractionsQueued || q.isProcessingInteraction
orderby q.isProcessingInteraction descending, q.PriorityCurrent descending
select q).FirstOrDefault();
DateTime now = DateTime.Now;
if (queue != null)
{
// queue is either talking to the controller, or has something to send
lock (queue.padlock)
{
if (!queue.isProcessingInteraction) // if queue is processing interaction, we skip this cycle and let the read handlers do their job.
{
// done with the current interaction, get next one:
if (queue.HasInteractionsQueued)
{
m_currentQueue = queue; // when responses are processed, this is the queue to use.
if (queue is RQMotorCommandQueue)
{
// we cannot send motor commands more frequently than 16ms. Mark the moment when we send a command
// - this is static, common for both Left and Right motor queues:
RQMotorCommandQueue.lastSentTicksMotorCmd = now.Ticks;
if (!isShortQueryMode)
{
m_queryQueue.Clear(); // prepare to be filled from m_querySetShort
isShortQueryMode = (now - querySetLastSent).TotalSeconds < 1.2d;
if (isShortQueryMode)
{
foreach (RQQuery q in m_querySetShort)
{
q.reset();
m_queryQueue.Enqueue(q);
}
}
else
{
foreach (RQQuery q in m_querySet)
{
q.reset();
m_queryQueue.Enqueue(q);
}
querySetLastSent = now;
}
}
//Console.Write("M");
}
//else
//{
// //Console.Write(isShortQueryMode ? "Q" : "L");
//}
queue.waitingSinceTicks = now.Ticks;
m_port.WriteLine(queue.dequeueForSend()); // send a command or a query to controller.
}
}
}
}
else
{
// all queues are empty (nothing to send), replentish the query queue.
// decide which query set to send - when moving fast, encoders and whiskers have priority.
if ((now - querySetLastSent).TotalSeconds > 1.2d || maxEncoderSpeed == 0)
{
// sending from m_querySet:
//Tracer.Trace("====================================================================== LONG : " + maxEncoderSpeed + " " + prev_encoderLeftSpeed + " " + prev_encoderRightSpeed);
isShortQueryMode = false;
foreach (RQQuery q in m_querySet)
{
q.reset();
m_queryQueue.Enqueue(q);
}
querySetLastSent = now;
}
else
{
// sending from m_querySetShort:
//Tracer.Trace("-------------------------------- SHORT -------------------------------------: " + maxEncoderSpeed + " " + prev_encoderLeftSpeed + " " + prev_encoderRightSpeed);
isShortQueryMode = true;
foreach (RQQuery q in m_querySetShort)
{
q.reset();
m_queryQueue.Enqueue(q);
}
}
// we now definitely have something to send, use this cycle to start the next interaction:
goto trySendNow;
}
}
}
else if (isMonitored)
{
isInError = false;
statusLabel = "monitored";
}
else if (isOnline)
{
isInError = false;
statusLabel = "online - receiving data";
}
else
{
isInError = false;
statusLabel = "not connected";
}
// trace when state changes:
if (!statusLabel.Equals(lastStatusLabel))
{
Tracer.Trace2("AX2850 : " + statusLabel);
lastStatusLabel = statusLabel;
lastStatusLabelChanged = DateTime.Now;
}
if (doLogMeasuredValues)
{
// create and append to a file like c:\temp\log_20120916_144334.csv for post-run motor performance analysis
logMeasuredValues();
}
}
}
