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); } }