private int m_wCnt = 0; // watchdog "W" consecutive count to make sure we've grabbed the controller all right
#endregion Fields
#region Constructors
public ControllerRQAX2850(string portName)
{
m_portName = portName;
m_queues[0] = m_commandPowerLeftQueue = new RQMotorCommandQueue("commandPowerLeft");
m_queues[1] = m_commandPowerRightQueue = new RQMotorCommandQueue("commandPowerRight");
m_queues[2] = m_commandQueue = new RQInteractionQueue("command");
m_queues[3] = m_queryQueue = new RQInteractionQueue("query");
m_querySet.Add(new RQQueryAnalogInputs(m_measuredValues));
m_querySet.Add(new RQQueryDigitalInputs(m_measuredValues));
m_querySet.Add(new RQQueryHeatsinkTemperature(m_measuredValues));
m_querySet.Add(new RQQueryMotorAmps(m_measuredValues));
m_querySet.Add(new RQQueryMotorPower(m_measuredValues));
m_querySet.Add(new RQQueryVoltage(m_measuredValues));
m_querySet.Add(new RQQueryEncoderLeftAbsolute(m_measuredValues));
m_querySet.Add(new RQQueryEncoderRightAbsolute(m_measuredValues));
m_querySet.Add(new RQQueryEncoderSpeed(m_measuredValues));
foreach (RQQuery query in m_querySet)
{
foreach(string vName in query.ValueNames)
{
m_loggedValueNames.Add(vName);
}
}
m_logger = new Logger(m_measuredValues, m_loggedValueNames);
startMonitoringThread();
}
public ControllerRQAX2850(string portName)
{
m_portName = portName;
m_queues[0] = m_commandPowerLeftQueue = new RQMotorCommandQueue("commandPowerLeft");
m_queues[1] = m_commandPowerRightQueue = new RQMotorCommandQueue("commandPowerRight");
m_queues[2] = m_commandQueue = new RQInteractionQueue("command");
m_queues[3] = m_queryQueue = new RQInteractionQueue("query");
m_querySet.Add(new RQQueryAnalogInputs(m_measuredValues));
m_querySet.Add(new RQQueryDigitalInputs(m_measuredValues));
m_querySet.Add(new RQQueryHeatsinkTemperature(m_measuredValues));
m_querySet.Add(new RQQueryMotorAmps(m_measuredValues));
m_querySet.Add(new RQQueryMotorPower(m_measuredValues));
m_querySet.Add(new RQQueryVoltage(m_measuredValues));
m_querySet.Add(new RQQueryEncoderLeftAbsolute(m_measuredValues));
m_querySet.Add(new RQQueryEncoderRightAbsolute(m_measuredValues));
m_querySet.Add(new RQQueryEncoderSpeed(m_measuredValues));
foreach (RQQuery query in m_querySet)
{
foreach (string vName in query.ValueNames)
{
m_loggedValueNames.Add(vName);
}
}
m_logger = new Logger(m_measuredValues, m_loggedValueNames);
startMonitoringThread();
}
/// <summary>
/// hopefully not crippled string came in - wether a monitoring stream, a response to query or an echo from query or command
/// </summary>
/// <param name="str"></param>
private void onStringReceived(string str, long timestamp)
{
try
{
if (str.StartsWith(":") && (str.Length == 25 || str.Length == 33))
{
isMonitored = true;
// :0000000000FE00009898396AB7B70000 - page 103 of controller doc
// the 9898 is temperature 1 and 2
// the 396A is voltages
// the 0000 at the end is speed - 00 left 00 right
interpretMonitoringString(str, timestamp);
}
else if (isGrabbed)
{
lock (m_currentQueuePadlock)
{
if (m_currentQueue != null)
{
if (m_currentQueue.isProcessingInteraction)
{
if (m_currentQueue.onStringReceived(str, timestamp)) // last line will return true
{
m_currentQueue = null;
}
}
}
}
}
}
catch (Exception exc)
{
}
}
/// <summary>
/// hopefully not crippled string came in - wether a monitoring stream, a response to query or an echo from query or command
/// </summary>
/// <param name="str"></param>
private void onStringReceived(string str, long timestamp)
{
try
{
if (str.StartsWith(":") && (str.Length == 25 || str.Length == 33))
{
isMonitored = true;
// :0000000000FE00009898396AB7B70000 - page 103 of controller doc
// the 9898 is temperature 1 and 2
// the 396A is voltages
// the 0000 at the end is speed - 00 left 00 right
interpretMonitoringString(str, timestamp);
}
else if (isGrabbed)
{
lock (m_currentQueuePadlock)
{
if (m_currentQueue != null)
{
if (m_currentQueue.isProcessingInteraction)
{
if (m_currentQueue.onStringReceived(str, timestamp)) // last line will return true
{
m_currentQueue = null;
}
}
}
}
}
}
catch(Exception exc)
{
}
}
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);
}
}
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);
}
}
