void rqQueryDigitalInputs_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryDigitalInputs query = (RQQueryDigitalInputs)sender;
            double digitalInputE       = query.doubleValues[0];
            double digitalInputF       = query.doubleValues[1];
            double digitalInputEmerg   = query.doubleValues[2];

            if (digitalInputE != prev_digitalInputE)
            {
                Tracer.Trace("ControllerRQAX2850: DigitalInputE changed : " + prev_digitalInputE + " -> " + digitalInputE);
                prev_digitalInputE = digitalInputE;

                if (onValueReceived_DigitalInputE != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1    = digitalInputE;
                    onValueReceived_DigitalInputE(this, ev);
                }
            }

            if (digitalInputF != prev_digitalInputF)                  // WhiskerLeft
            {
                //Tracer.Trace("ControllerRQAX2850: DigitalInputF changed : " + prev_digitalInputF + " -> " + digitalInputF);
                prev_digitalInputF = digitalInputF;

                if (onValueReceived_DigitalInputF != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1    = digitalInputF;
                    onValueReceived_DigitalInputF(this, ev);
                }
            }

            if (digitalInputEmerg != prev_digitalInputEmerg)    // WhiskerRight
            {
                //Tracer.Trace("ControllerRQAX2850: DigitalInputEmerg changed : " + prev_digitalInputEmerg + " -> " + digitalInputEmerg);
                prev_digitalInputEmerg = digitalInputEmerg;

                if (onValueReceived_DigitalInputEmerg != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1    = digitalInputEmerg;
                    onValueReceived_DigitalInputEmerg(this, ev);
                }
            }
        }
        void rqQueryEncoderRightAbsolute_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryEncoderRightAbsolute query = (RQQueryEncoderRightAbsolute)sender;
            double encoderRightAbsolute       = query.doubleValues[0];

            if (encoderRightAbsolute != prev_encoderRightAbsolute)                // note: nonnull value is not equal to a null value
            {
                //Tracer.Trace("ControllerRQAX2850: EncoderRightAbsolute changed : " + prev_encoderRightAbsolute + " -> " + encoderRightAbsolute);
                prev_encoderRightAbsolute = encoderRightAbsolute;

                if (onValueReceived_EncoderRightAbsolute != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1    = encoderRightAbsolute;
                    onValueReceived_EncoderRightAbsolute(this, ev);
                }
            }
        }
        void rqQueryMotorAmps_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryMotorAmps query            = (RQQueryMotorAmps)sender;
            double           Motor_Amps_Left  = query.doubleValues[0];                                                                                           // "Motor_Amps_Left"
            double           Motor_Amps_Right = query.doubleValues[1];                                                                                           // "Motor_Amps_Right"

            if (Motor_Amps_Left != prev_Motor_Amps_Left || Motor_Amps_Right != prev_Motor_Amps_Right || (DateTime.Now - lastSentMotor_Amps).TotalSeconds > 5.0d) // force send every 5 sec
            {
                //Tracer.Trace("MotorAmps=" + Motor_Amps_Left + "   " + Motor_Amps_Right);
                prev_Motor_Amps_Left  = Motor_Amps_Left;
                prev_Motor_Amps_Right = Motor_Amps_Right;

                if (onValueReceived_MotorAmps != null)
                {
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1    = Motor_Amps_Left;
                    ev.value2    = Motor_Amps_Right;
                    onValueReceived_MotorAmps(this, ev);
                }
            }
        }
        void rqQueryAnalogInputs_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryAnalogInputs query = (RQQueryAnalogInputs)sender;
            double Analog_Input_1     = query.doubleValues[0];                                                                                               // "Analog_Input_1"
            double Analog_Input_2     = query.doubleValues[1];                                                                                               // "Analog_Input_2"

            if (Analog_Input_1 != prev_Analog_Input_1 || Analog_Input_2 != prev_Analog_Input_2 || (DateTime.Now - lastSentAnalog_Input).TotalSeconds > 5.0d) // force send every 5 sec
            {
                //Tracer.Trace("Analog Inputs=" + Analog_Input_1 + "   " + Analog_Input_2);
                prev_Analog_Input_1 = Analog_Input_1;
                prev_Analog_Input_2 = Analog_Input_2;

                if (onValueReceived_AnalogInputs != null)
                {
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1    = Analog_Input_1;
                    ev.value2    = Analog_Input_2;
                    onValueReceived_AnalogInputs(this, ev);
                }
            }
        }
        void rqQueryHeatsinkTemperature_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryHeatsinkTemperature query  = (RQQueryHeatsinkTemperature)sender;
            double Heatsink_Temperature_Left  = query.doubleValues[0];                                                                                                                                             // "Heatsink_Temperature_Left"
            double Heatsink_Temperature_Right = query.doubleValues[1];                                                                                                                                             // "Heatsink_Temperature_Right"

            if (Heatsink_Temperature_Left != prev_Heatsink_Temperature_Left || Heatsink_Temperature_Right != prev_Heatsink_Temperature_Right || (DateTime.Now - lastSentHeatsink_Temperature).TotalSeconds > 5.0d) // force send every 5 sec
            {
                //Tracer.Trace("Heatsink Temperature=" + Heatsink_Temperature_Left + "   " + Heatsink_Temperature_Right);
                prev_Heatsink_Temperature_Left  = Heatsink_Temperature_Left;
                prev_Heatsink_Temperature_Right = Heatsink_Temperature_Right;

                if (onValueReceived_HeatsinkTemperature != null)
                {
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1    = Heatsink_Temperature_Left;
                    ev.value2    = Heatsink_Temperature_Right;
                    onValueReceived_HeatsinkTemperature(this, ev);
                }
            }
        }
        void rqQueryVoltage_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryVoltage query = (RQQueryVoltage)sender;
            double         Main_Battery_Voltage = query.doubleValues[0];                                                                                                // "Main_Battery_Voltage"
            double         Internal_Voltage     = query.doubleValues[1];                                                                                                // "Internal_Voltage"

            if (Main_Battery_Voltage != prev_Main_Battery_Voltage || Internal_Voltage != prev_Internal_Voltage || (DateTime.Now - lastSentVoltage).TotalSeconds > 5.0d) // force send every 5 sec
            {
                // Tracer.Trace("Voltage=" + Main_Battery_Voltage + "   " + Internal_Voltage);
                prev_Main_Battery_Voltage = Main_Battery_Voltage;
                prev_Internal_Voltage     = Internal_Voltage;

                if (onValueReceived_Voltage != null)
                {
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1    = Main_Battery_Voltage;
                    ev.value2    = Internal_Voltage;
                    onValueReceived_Voltage(this, ev);
                }
            }
        }
        void rqQueryEncoderSpeed_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryEncoderSpeed query = (RQQueryEncoderSpeed)sender;
            double leftSpeed          = query.doubleValues[0];
            double rightSpeed         = query.doubleValues[1];

            if (leftSpeed != prev_encoderLeftSpeed || rightSpeed != prev_encoderRightSpeed)
            {
                //Tracer.Trace("ControllerRQAX2850: EncoderSpeed changed   L: " + prev_encoderLeftSpeed + " -> " + leftSpeed + "    R: " + prev_encoderRightSpeed + " -> " + rightSpeed);
                prev_encoderLeftSpeed  = leftSpeed;
                prev_encoderRightSpeed = rightSpeed;

                if (onValueReceived_EncoderSpeed != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1    = leftSpeed;
                    ev.value2    = rightSpeed;
                    onValueReceived_EncoderSpeed(this, ev);
                }
            }
        }
 void _brickConnection_onValueReceived_AnalogInputs(object sender, MeasuredValuesEventArgs e)
 {
     _state.PowerControllerState.Analog_Input_1 = e.value1;
     _state.PowerControllerState.Analog_Input_2 = e.value2;
     _state.TimeStamp = DateTime.Now;
 }
        void rqueryMotorPower_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryMotorPower query = (RQQueryMotorPower)sender;
            double Motor_Power_Left = query.doubleValues[0];     // "Motor_Power_Left"
            double Motor_Power_Right = query.doubleValues[1];    // "Motor_Power_Right"

            if (Motor_Power_Left != prev_Motor_Power_Left || Motor_Power_Right != prev_Motor_Power_Right || (DateTime.Now - lastSentMotor_Power).TotalSeconds > 5.0d)     // force send every 5 sec
            {
                //Tracer.Trace("MotorPower=" + Motor_Power_Left + "   " + Motor_Power_Right);
                prev_Motor_Power_Left = Motor_Power_Left;
                prev_Motor_Power_Right = Motor_Power_Right;

                if (onValueReceived_MotorPower != null)
                {
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1 = Motor_Power_Left;
                    ev.value2 = Motor_Power_Right;
                    onValueReceived_MotorPower(this, ev);
                }
            }
        }
        void rqQueryVoltage_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryVoltage query = (RQQueryVoltage)sender;
            double Main_Battery_Voltage = query.doubleValues[0];    // "Main_Battery_Voltage"
            double Internal_Voltage = query.doubleValues[1];        // "Internal_Voltage"

            if (Main_Battery_Voltage != prev_Main_Battery_Voltage || Internal_Voltage != prev_Internal_Voltage || (DateTime.Now - lastSentVoltage).TotalSeconds > 5.0d)     // force send every 5 sec
            {
                // Tracer.Trace("Voltage=" + Main_Battery_Voltage + "   " + Internal_Voltage);
                prev_Main_Battery_Voltage = Main_Battery_Voltage;
                prev_Internal_Voltage = Internal_Voltage;

                if (onValueReceived_Voltage != null)
                {
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1 = Main_Battery_Voltage;
                    ev.value2 = Internal_Voltage;
                    onValueReceived_Voltage(this, ev);
                }
            }
        }
		private void _brickConnection_onValueReceived_EncoderRightAbsolute(object sender, MeasuredValuesEventArgs ev)
		{
#if TRACEDEBUGTICKS
            LogInfo("TrackRoamerBrickPowerService : received EncoderRightAbsolute : " + ev.value1);
#endif // TRACEDEBUGTICKS

            UpdateMotorEncoder ume = new UpdateMotorEncoder();
			ume.Body.Timestamp = new DateTime(ev.timestamp);
			ume.Body.RightDistance = ev.value1;
			ume.Body.HardwareIdentifier = 2;    // 2 = Right

            _state.MotorEncoder.RightDistance = ume.Body.RightDistance;
            _state.MotorEncoder.Timestamp = ume.Body.Timestamp;
            _state.TimeStamp = DateTime.Now;

			base.SendNotification <UpdateMotorEncoder>(_subMgrPort, ume);
		}
 void _brickConnection_onValueReceived_HeatsinkTemperature(object sender, MeasuredValuesEventArgs e)
 {
     _state.PowerControllerState.Heatsink_Temperature_Left = e.value1;
     _state.PowerControllerState.Heatsink_Temperature_Right = e.value2;
     _state.TimeStamp = DateTime.Now;
 }
 void _brickConnection_onValueReceived_MotorPower(object sender, MeasuredValuesEventArgs e)
 {
     _state.PowerControllerState.Motor_Power_Left = e.value1;
     _state.PowerControllerState.Motor_Power_Right = e.value2;
     _state.TimeStamp = DateTime.Now;
 }
        void rqQueryAnalogInputs_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryAnalogInputs query = (RQQueryAnalogInputs)sender;
            double Analog_Input_1 = query.doubleValues[0];     // "Analog_Input_1"
            double Analog_Input_2 = query.doubleValues[1];     // "Analog_Input_2"

            if (Analog_Input_1 != prev_Analog_Input_1 || Analog_Input_2 != prev_Analog_Input_2 || (DateTime.Now - lastSentAnalog_Input).TotalSeconds > 5.0d)     // force send every 5 sec
            {
                //Tracer.Trace("Analog Inputs=" + Analog_Input_1 + "   " + Analog_Input_2);
                prev_Analog_Input_1 = Analog_Input_1;
                prev_Analog_Input_2 = Analog_Input_2;

                if (onValueReceived_AnalogInputs != null)
                {
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1 = Analog_Input_1;
                    ev.value2 = Analog_Input_2;
                    onValueReceived_AnalogInputs(this, ev);
                }
            }
        }
 void _brickConnection_onValueReceived_DigitalInputE(object sender, MeasuredValuesEventArgs e)
 {
     _state.PowerControllerState.Digital_Input_E = e.value1;
     _state.TimeStamp = DateTime.Now;
 }
 void _brickConnection_onValueReceived_Voltage(object sender, MeasuredValuesEventArgs e)
 {
     // the voltages come in as hex bytes 0...255, and then converted by formula:
     //      Measured Main Battery Volts = 55 * Read Value / 256
     //      Measured Internal Volts = 28.5 * Read Value / 256
     // there isn't much precision here, so rounding it to 2 digits seems adequate.
     _state.PowerControllerState.Main_Battery_Voltage = e.value1.HasValue ? Math.Round(e.value1.Value, 2) : (double?)null;
     _state.PowerControllerState.Internal_Voltage = e.value2.HasValue ? Math.Round(e.value2.Value, 2) : (double?)null;
     _state.TimeStamp = DateTime.Now;
 }
        void rqQueryDigitalInputs_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryDigitalInputs query = (RQQueryDigitalInputs)sender;
            double digitalInputE = query.doubleValues[0];
            double digitalInputF = query.doubleValues[1];
            double digitalInputEmerg = query.doubleValues[2];

            if(digitalInputE != prev_digitalInputE)
            {
                Tracer.Trace("ControllerRQAX2850: DigitalInputE changed : " + prev_digitalInputE + " -> " + digitalInputE);
                prev_digitalInputE = digitalInputE;

                if (onValueReceived_DigitalInputE != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1 = digitalInputE;
                    onValueReceived_DigitalInputE(this, ev);
                }
            }

            if (digitalInputF != prev_digitalInputF)      // WhiskerLeft
            {
                //Tracer.Trace("ControllerRQAX2850: DigitalInputF changed : " + prev_digitalInputF + " -> " + digitalInputF);
                prev_digitalInputF = digitalInputF;

                if (onValueReceived_DigitalInputF != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1 = digitalInputF;
                    onValueReceived_DigitalInputF(this, ev);
                }
            }

            if (digitalInputEmerg != prev_digitalInputEmerg)    // WhiskerRight
            {
                //Tracer.Trace("ControllerRQAX2850: DigitalInputEmerg changed : " + prev_digitalInputEmerg + " -> " + digitalInputEmerg);
                prev_digitalInputEmerg = digitalInputEmerg;

                if (onValueReceived_DigitalInputEmerg != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1 = digitalInputEmerg;
                    onValueReceived_DigitalInputEmerg(this, ev);
                }
            }
        }
 void _brickConnection_onValueReceived_MotorAmps(object sender, MeasuredValuesEventArgs e)
 {
     // note: Amps behave almost like integers, no precision here and low current will read as 0
     _state.PowerControllerState.Motor_Amps_Left = e.value1;
     _state.PowerControllerState.Motor_Amps_Right = e.value2;
     _state.TimeStamp = DateTime.Now;
 }
        void rqQueryEncoderRightAbsolute_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryEncoderRightAbsolute query = (RQQueryEncoderRightAbsolute)sender;
            double encoderRightAbsolute = query.doubleValues[0];

            if (encoderRightAbsolute != prev_encoderRightAbsolute)    // note: nonnull value is not equal to a null value
            {
                //Tracer.Trace("ControllerRQAX2850: EncoderRightAbsolute changed : " + prev_encoderRightAbsolute + " -> " + encoderRightAbsolute);
                prev_encoderRightAbsolute = encoderRightAbsolute;

                if (onValueReceived_EncoderRightAbsolute != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1 = encoderRightAbsolute;
                    onValueReceived_EncoderRightAbsolute(this, ev);
                }
            }
        }
        void _brickConnection_onValueReceived_EncoderSpeed(object sender, MeasuredValuesEventArgs ev)
		{
#if TRACEDEBUGTICKS
            LogInfo("TrackRoamerBrickPowerService : received EncoderSpeed : left=" + ev.value1 + "   right=" + ev.value2);
#endif // TRACEDEBUGTICKS

            UpdateMotorEncoderSpeed ume = new UpdateMotorEncoderSpeed();
			ume.Body.Timestamp = new DateTime(ev.timestamp);
			ume.Body.LeftSpeed = ev.value1;
			ume.Body.RightSpeed = ev.value2;

            _state.MotorEncoderSpeed.LeftSpeed = ume.Body.LeftSpeed;
            _state.MotorEncoderSpeed.RightSpeed = ume.Body.RightSpeed;
            _state.MotorEncoderSpeed.Timestamp = ume.Body.Timestamp;
            _state.TimeStamp = DateTime.Now;

            base.SendNotification<UpdateMotorEncoderSpeed>(_subMgrPort, ume);
		}
        void rqQueryEncoderSpeed_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryEncoderSpeed query = (RQQueryEncoderSpeed)sender;
            double leftSpeed = query.doubleValues[0];
            double rightSpeed = query.doubleValues[1];

            if (leftSpeed != prev_encoderLeftSpeed || rightSpeed != prev_encoderRightSpeed)
            {
                //Tracer.Trace("ControllerRQAX2850: EncoderSpeed changed   L: " + prev_encoderLeftSpeed + " -> " + leftSpeed + "    R: " + prev_encoderRightSpeed + " -> " + rightSpeed);
                prev_encoderLeftSpeed = leftSpeed;
                prev_encoderRightSpeed = rightSpeed;

                if (onValueReceived_EncoderSpeed != null)
                {
                    //ev = new MeasuredValuesEventArgs();
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1 = leftSpeed;
                    ev.value2 = rightSpeed;
                    onValueReceived_EncoderSpeed(this, ev);
                }
            }
        }
		private void onWhiskerRight(object sender, MeasuredValuesEventArgs ev)
		{
            LogInfo("TrackRoamerBrickPowerService : WhiskerRight : " + ev.value1);

            if (ev.value1 > 0 && (!_state.Whiskers.FrontWhiskerRight.HasValue || !_state.Whiskers.FrontWhiskerRight.Value))
            {
                // if this is a "whisker pressed" event, do emergency stop.
                stopMotorsNow();
                // Note: UpdateMotorSpeedHandler() will not set positive speed if whiskers are pressed.
            }

            _state.Whiskers.Timestamp = new DateTime(ev.timestamp);
            _state.Whiskers.FrontWhiskerRight = ev.value1 > 0;
            _state.TimeStamp = DateTime.Now;

            UpdateWhiskers uw = new UpdateWhiskers();
			uw.Body.Timestamp = _state.Whiskers.Timestamp;
			uw.Body.FrontWhiskerRight = ev.value1 > 0;

			base.SendNotification<UpdateWhiskers>(_subMgrPort, uw);
		}
        void rqQueryHeatsinkTemperature_onValueReceived(object sender, MeasuredValuesEventArgs ev)
        {
            RQQueryHeatsinkTemperature query = (RQQueryHeatsinkTemperature)sender;
            double Heatsink_Temperature_Left = query.doubleValues[0];     // "Heatsink_Temperature_Left"
            double Heatsink_Temperature_Right = query.doubleValues[1];    // "Heatsink_Temperature_Right"

            if (Heatsink_Temperature_Left != prev_Heatsink_Temperature_Left || Heatsink_Temperature_Right != prev_Heatsink_Temperature_Right || (DateTime.Now - lastSentHeatsink_Temperature).TotalSeconds > 5.0d)     // force send every 5 sec
            {
                //Tracer.Trace("Heatsink Temperature=" + Heatsink_Temperature_Left + "   " + Heatsink_Temperature_Right);
                prev_Heatsink_Temperature_Left = Heatsink_Temperature_Left;
                prev_Heatsink_Temperature_Right = Heatsink_Temperature_Right;

                if (onValueReceived_HeatsinkTemperature != null)
                {
                    ev.timestamp = query.whenReceivedTicks;
                    ev.value1 = Heatsink_Temperature_Left;
                    ev.value2 = Heatsink_Temperature_Right;
                    onValueReceived_HeatsinkTemperature(this, ev);
                }
            }
        }