public RobotConnectionService(IStreamResource streamResource)
{
_streamResource = streamResource;
_watchdog = new Watchdog(1100); //this should be minimum 2x keepAlive sending period
_watchdog.TimeoutOccured += (sender, args) => TimeoutOccured?.Invoke(sender, args);
//TODO unifiy the error event args, so we can use one common event handler
var messageExtractor = new MessageExtractor();
messageExtractor.KeepAliveReceived += (sender, args) => _watchdog.ResetWatchdog();
messageExtractor.MessageLostOccured +=
(sender, args) => Logger.Fatal($"Lost message, total count: {args.TotalLostCount}");
messageExtractor.SpeedCurrentFeedbackReceived += (sender, args) => SpeedCurrentFeedbackReceived?.Invoke(sender, args);
messageExtractor.VoltageTemperatureFeedbackReceived += (sender, args) => VoltageTemperatureFeedbackReceived?.Invoke(sender, args);
messageExtractor.ParametersReceived += (sender, args) => ParametersReceived?.Invoke(sender, args);
_receiverTask = new ReceiverTask(_streamResource);
_receiverTask.ErrorOccurred += (sender, args) => Logger.Error($"Receiver task error: {args.GetException().Message}");
_receiverTask.DataReceived += (sender, args) => messageExtractor.TryGetMessage(args.Data);
_receiverTask.Start();
_senderQueue = new SendQueueWrapper();
_senderTask = new SenderTask(_streamResource, _senderQueue);
_senderTask.ErrorOccurred += (sender, args) => Logger.Error($"Sender task error: {args.GetException().Message}");
_senderTask.Start();
_watchdog.Start();
}
public void Parse(IReceiveMessage message)
{
try
{
var payload = (byte[])message.Payload;
switch (message.Command)
{
case EReceiverCommand.KeepAlive:
KeepAliveReceived?.Invoke(this, EventArgs.Empty);
Debug.WriteLine("Keep alive recieved");
break;
case EReceiverCommand.FeedbackSpeedCurrent:
{
var leftRawCurrent = BitConverter.ToInt16(payload, 2);
var leftRawVelocity = BitConverter.ToInt16(payload, 0);
var rightRawCurrent = BitConverter.ToInt16(payload, 6);
var rightRawVelocity = BitConverter.ToInt16(payload, 4);
var parsed = new MessageParsedEventArgs
{
LeftMotor = new SpeedCurrentFeedbackModel
{
Velocity = VelocityConverter.GetPhysical(leftRawVelocity),
RawVelocity = leftRawVelocity,
Current = CurrentConverter.GetPhysical(leftRawCurrent),
RawCurrent = leftRawCurrent
},
RightMotor = new SpeedCurrentFeedbackModel
{
Velocity = VelocityConverter.GetPhysical(rightRawVelocity),
RawVelocity = rightRawVelocity,
Current = CurrentConverter.GetPhysical(rightRawCurrent),
RawCurrent = rightRawCurrent
}
};
SpeedCurrentFeedbackReceived?.Invoke(this, parsed);
}
break;
case EReceiverCommand.FeedbackVoltageTemperature:
{
var parsed = new MessageParsedEventArgs
{
VoltageTemperatureFeedbackModel = new VoltageTemperatureFeedbackModel
{
Voltage = VoltageConverter.GetPhysical(BitConverter.ToUInt16(payload, 0)),
Temperature = TemperatureConverter.GetPhysical(BitConverter.ToUInt16(payload, 2)),
}
};
VoltageTemperatureFeedbackReceived?.Invoke(this, parsed);
}
break;
#region Alarms
case EReceiverCommand.VoltageAlarm:
_parameters.Alarms.VoltageAlarm = VoltageConverter.GetPhysical(BitConverter.ToUInt16(payload, 0));
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.CriticalVoltageAlarm:
_parameters.Alarms.CriticalVoltageAlarm = VoltageConverter.GetPhysical(BitConverter.ToUInt16(payload, 0));
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.TemperatureAlarm:
_parameters.Alarms.TemperatureAlarm = TemperatureConverter.GetPhysical(BitConverter.ToUInt16(payload, 0));
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.CriticalTemperatureAlarm:
_parameters.Alarms.CriticalTemperatureAlarm = TemperatureConverter.GetPhysical(BitConverter.ToUInt16(payload, 0));
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.CurrentLeftAlarm:
_parameters.Alarms.LeftCurrentAlarm = CurrentConverter.GetPhysical(BitConverter.ToInt16(payload, 0));
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.CurrentRightAlarm:
_parameters.Alarms.RightCurrentAlarm = CurrentConverter.GetPhysical(BitConverter.ToInt16(payload, 0));
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
#endregion
#region PID parameters
case EReceiverCommand.PidKp_1:
_parameters.PidLeft.Kp = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidKi_1:
_parameters.PidLeft.Ki = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidKd_1:
_parameters.PidLeft.Kd = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidIntegralLimit_1:
_parameters.PidLeft.IntegralLimit = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidClamping_1:
_parameters.PidLeft.OutputLimit = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidDeadband_1:
_parameters.PidLeft.Deadband = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidPeriod_1:
_parameters.PidLeft.Period = BitConverter.ToUInt16(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidKp_2:
_parameters.PidRight.Kp = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidKi_2:
_parameters.PidRight.Ki = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidKd_2:
_parameters.PidRight.Kd = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidIntegralLimit_2:
_parameters.PidRight.IntegralLimit = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidClamping_2:
_parameters.PidRight.OutputLimit = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidDeadband_2:
_parameters.PidRight.Deadband = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.PidPeriod_2:
_parameters.PidRight.Period = BitConverter.ToUInt16(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
#endregion PID parameters
#region Fuzzy parameters
case EReceiverCommand.FuzzyKp_1:
_parameters.FuzzyLeft.Kp = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyKi_1:
_parameters.FuzzyLeft.Ki = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyKd_1:
_parameters.FuzzyLeft.Kd = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyIntegralLimit_1:
_parameters.FuzzyLeft.IntegralLimit = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyClamping_1:
_parameters.FuzzyLeft.OutputLimit = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyDeadband_1:
_parameters.FuzzyLeft.Deadband = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyPeriod_1:
_parameters.FuzzyLeft.Period = BitConverter.ToUInt16(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyKp_2:
_parameters.FuzzyRight.Kp = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyKi_2:
_parameters.FuzzyRight.Ki = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyKd_2:
_parameters.FuzzyRight.Kd = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyIntegralLimit_2:
_parameters.FuzzyRight.IntegralLimit = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyClamping_2:
_parameters.FuzzyRight.OutputLimit = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyDeadband_2:
_parameters.FuzzyRight.Deadband = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.FuzzyPeriod_2:
_parameters.FuzzyRight.Period = BitConverter.ToUInt16(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
#endregion Fuzzy parameters
#region Encoder parameters
case EReceiverCommand.EncoderFilterCoef_1:
_parameters.EncoderLeft.LowPassFilterCoeff = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.EncoderFilterCoef_2:
_parameters.EncoderRight.LowPassFilterCoeff = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.EncoderScaleCoef_1:
_parameters.EncoderLeft.ScaleCoeff = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.EncoderScaleCoef_2:
_parameters.EncoderRight.ScaleCoeff = BitConverter.ToSingle(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.EncoderIsReversed_1:
_parameters.EncoderLeft.IsReversed = BitConverter.ToBoolean(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.EncoderIsReversed_2:
_parameters.EncoderRight.IsReversed = BitConverter.ToBoolean(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.EncoderIsFilterEnabled_1:
_parameters.EncoderLeft.IsFilterEnabled = BitConverter.ToBoolean(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.EncoderIsFilterEnabled_2:
_parameters.EncoderRight.IsFilterEnabled = BitConverter.ToBoolean(payload, 0);
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
#endregion Encoder parameters
case EReceiverCommand.ControlType:
_parameters.ControlType = payload[0];
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.RegenerativeBreaking:
_parameters.UseRegenerativeBreaking = payload[0] != 0;
ParametersReceived?.Invoke(this, new MessageParsedEventArgs {
Parameters = _parameters
});
break;
case EReceiverCommand.EepromSaved:
Debug.WriteLine("Saved");
break;
case EReceiverCommand.TX_MovementEnabled:
Debug.WriteLine("Enabled");
break;
case EReceiverCommand.TX_MovementDisabled:
Debug.WriteLine("Disabled");
break;
default:
throw new NotImplementedException();
}
}
catch (Exception e)
{
Logger.Error(e.Message);
ParsingErrorOccured?.Invoke(this, new MessageParsingErrorEventArgs(e));
}
}
