/// <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>
/// <param name="timestamp"></param>
protected void onStringReceived(string str, long timestamp)
{
//Tracer.Trace("ControllerRQAX2850:onStringReceived() str=" + str);
frameCounter++;
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)
{
Tracer.Error("ControllerRQAX2850: " + exc.Message);
errorCounter++;
}
}
private DateTime querySetLastSent = DateTime.MinValue; // when we last sent from m_querySet
#endregion Fields
#region Constructors
public ControllerRQAX2850(string portName)
{
Tracer.Trace("ControllerRQAX2850(" + portName + ")");
m_portName = portName;
m_queues[0] = m_commandQueue = new RQInteractionQueue("command", 3); // highest priority factor
m_queues[1] = m_commandPowerLeftQueue = new RQMotorCommandQueue("commandPowerLeft"); // one-slot queue; must be "equalized" with the m_commandPowerRightQueue in terms of processing order. PriorityFactor = 2;
m_queues[2] = m_commandPowerRightQueue = new RQMotorCommandQueue("commandPowerRight");
m_queues[3] = m_queryQueue = new RQInteractionQueue("query", 1); // lowest priority factor
RQQuery rqQueryEncoderSpeed = new RQQueryEncoderSpeed(m_measuredValues);
rqQueryEncoderSpeed.onValueReceived += new OnValueReceived(rqQueryEncoderSpeed_onValueReceived);
m_querySet.Add(rqQueryEncoderSpeed);
m_querySetShort.Add(rqQueryEncoderSpeed);
RQQuery rqQueryEncoderLeftAbsolute = new RQQueryEncoderLeftAbsolute(m_measuredValues);
rqQueryEncoderLeftAbsolute.onValueReceived += new OnValueReceived(rqQueryEncoderLeftAbsolute_onValueReceived);
m_querySet.Add(rqQueryEncoderLeftAbsolute);
m_querySetShort.Add(rqQueryEncoderLeftAbsolute);
RQQuery rqQueryEncoderRightAbsolute = new RQQueryEncoderRightAbsolute(m_measuredValues);
rqQueryEncoderRightAbsolute.onValueReceived += new OnValueReceived(rqQueryEncoderRightAbsolute_onValueReceived);
m_querySet.Add(rqQueryEncoderRightAbsolute);
m_querySetShort.Add(rqQueryEncoderRightAbsolute);
RQQuery rqQueryAnalogInputs = new RQQueryAnalogInputs(m_measuredValues);
rqQueryAnalogInputs.onValueReceived += new OnValueReceived(rqQueryAnalogInputs_onValueReceived);
m_querySet.Add(rqQueryAnalogInputs);
RQQuery rqQueryDigitalInputs = new RQQueryDigitalInputs(m_measuredValues);
rqQueryDigitalInputs.onValueReceived += new OnValueReceived(rqQueryDigitalInputs_onValueReceived);
m_querySet.Add(rqQueryDigitalInputs);
m_querySetShort.Add(rqQueryDigitalInputs); // whiskers are in priority queue
RQQuery rqQueryHeatsinkTemperature = new RQQueryHeatsinkTemperature(m_measuredValues);
rqQueryHeatsinkTemperature.onValueReceived += new OnValueReceived(rqQueryHeatsinkTemperature_onValueReceived);
m_querySet.Add(rqQueryHeatsinkTemperature);
RQQuery rqQueryMotorAmps = new RQQueryMotorAmps(m_measuredValues);
rqQueryMotorAmps.onValueReceived += new OnValueReceived(rqQueryMotorAmps_onValueReceived);
m_querySet.Add(rqQueryMotorAmps);
RQQuery rqueryMotorPower = new RQQueryMotorPower(m_measuredValues);
rqueryMotorPower.onValueReceived += new OnValueReceived(rqueryMotorPower_onValueReceived);
m_querySet.Add(rqueryMotorPower);
RQQuery rqQueryVoltage = new RQQueryVoltage(m_measuredValues);
rqQueryVoltage.onValueReceived += new OnValueReceived(rqQueryVoltage_onValueReceived);
m_querySet.Add(rqQueryVoltage);
foreach (RQQuery query in m_querySet)
{
foreach(string vName in query.ValueNames)
{
m_loggedValueNames.Add(vName);
}
}
isUnknownState = true;
isInError = false;
//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>
/// <param name="timestamp"></param>
protected void onStringReceived(string str, long timestamp)
{
//Tracer.Trace("ControllerRQAX2850:onStringReceived() str=" + str);
frameCounter++;
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)
{
Tracer.Error("ControllerRQAX2850: " + exc.Message);
errorCounter++;
}
}
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();
}
}
}
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();
}
}
}
public ControllerRQAX2850(string portName)
{
Tracer.Trace("ControllerRQAX2850(" + portName + ")");
m_portName = portName;
m_queues[0] = m_commandQueue = new RQInteractionQueue("command", 3); // highest priority factor
m_queues[1] = m_commandPowerLeftQueue = new RQMotorCommandQueue("commandPowerLeft"); // one-slot queue; must be "equalized" with the m_commandPowerRightQueue in terms of processing order. PriorityFactor = 2;
m_queues[2] = m_commandPowerRightQueue = new RQMotorCommandQueue("commandPowerRight");
m_queues[3] = m_queryQueue = new RQInteractionQueue("query", 1); // lowest priority factor
RQQuery rqQueryEncoderSpeed = new RQQueryEncoderSpeed(m_measuredValues);
rqQueryEncoderSpeed.onValueReceived += new OnValueReceived(rqQueryEncoderSpeed_onValueReceived);
m_querySet.Add(rqQueryEncoderSpeed);
m_querySetShort.Add(rqQueryEncoderSpeed);
RQQuery rqQueryEncoderLeftAbsolute = new RQQueryEncoderLeftAbsolute(m_measuredValues);
rqQueryEncoderLeftAbsolute.onValueReceived += new OnValueReceived(rqQueryEncoderLeftAbsolute_onValueReceived);
m_querySet.Add(rqQueryEncoderLeftAbsolute);
m_querySetShort.Add(rqQueryEncoderLeftAbsolute);
RQQuery rqQueryEncoderRightAbsolute = new RQQueryEncoderRightAbsolute(m_measuredValues);
rqQueryEncoderRightAbsolute.onValueReceived += new OnValueReceived(rqQueryEncoderRightAbsolute_onValueReceived);
m_querySet.Add(rqQueryEncoderRightAbsolute);
m_querySetShort.Add(rqQueryEncoderRightAbsolute);
RQQuery rqQueryAnalogInputs = new RQQueryAnalogInputs(m_measuredValues);
rqQueryAnalogInputs.onValueReceived += new OnValueReceived(rqQueryAnalogInputs_onValueReceived);
m_querySet.Add(rqQueryAnalogInputs);
RQQuery rqQueryDigitalInputs = new RQQueryDigitalInputs(m_measuredValues);
rqQueryDigitalInputs.onValueReceived += new OnValueReceived(rqQueryDigitalInputs_onValueReceived);
m_querySet.Add(rqQueryDigitalInputs);
m_querySetShort.Add(rqQueryDigitalInputs); // whiskers are in priority queue
RQQuery rqQueryHeatsinkTemperature = new RQQueryHeatsinkTemperature(m_measuredValues);
rqQueryHeatsinkTemperature.onValueReceived += new OnValueReceived(rqQueryHeatsinkTemperature_onValueReceived);
m_querySet.Add(rqQueryHeatsinkTemperature);
RQQuery rqQueryMotorAmps = new RQQueryMotorAmps(m_measuredValues);
rqQueryMotorAmps.onValueReceived += new OnValueReceived(rqQueryMotorAmps_onValueReceived);
m_querySet.Add(rqQueryMotorAmps);
RQQuery rqueryMotorPower = new RQQueryMotorPower(m_measuredValues);
rqueryMotorPower.onValueReceived += new OnValueReceived(rqueryMotorPower_onValueReceived);
m_querySet.Add(rqueryMotorPower);
RQQuery rqQueryVoltage = new RQQueryVoltage(m_measuredValues);
rqQueryVoltage.onValueReceived += new OnValueReceived(rqQueryVoltage_onValueReceived);
m_querySet.Add(rqQueryVoltage);
foreach (RQQuery query in m_querySet)
{
foreach (string vName in query.ValueNames)
{
m_loggedValueNames.Add(vName);
}
}
isUnknownState = true;
isInError = false;
//startMonitoringThread();
}
