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