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