Skip to content

Commit 0f6e90a

Browse files
author
Marcus Sonestedt
committed
ServoPIDControl: Fix some comm issues & speed it up
1 parent 0cac078 commit 0f6e90a

File tree

10 files changed

+288
-69
lines changed

10 files changed

+288
-69
lines changed

‎ServoPIDControl/App.xaml.cs‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
using System;
2+
using System.Linq;
23
using System.Windows;
34
using System.Windows.Threading;
45

@@ -13,8 +14,13 @@ public App()
1314
{
1415
DispatcherUnhandledException += OnDispatcherUnhandledException;
1516
AppDomain.CurrentDomain.UnhandledException += CurrentDomainOnUnhandledException;
17+
18+
MockMode = Environment.GetCommandLineArgs().Contains("--mock");
19+
1620
}
1721

22+
public bool MockMode { get; set; }
23+
1824
private static void CurrentDomainOnUnhandledException(object sender, UnhandledExceptionEventArgs e)
1925
{
2026
var ex = (Exception) e.ExceptionObject;

‎ServoPIDControl/ArduinoCom.cs‎

Lines changed: 63 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -39,17 +39,16 @@ public class ArduinoCom : IDisposable
3939
private static readonly Logger Log = LogManager.GetCurrentClassLogger();
4040

4141
private ISerialPort _port;
42-
private readonly StringBuilder _readBuf = new StringBuilder();
4342
private ViewModel _model;
44-
private readonly DispatcherTimer _timer = new DispatcherTimer {Interval = TimeSpan.FromMilliseconds(250)};
43+
private readonly StringBuilder _readBuf = new StringBuilder();
44+
private readonly DispatcherTimer _timer = new DispatcherTimer {Interval = TimeSpan.FromMilliseconds(50)};
4545
private readonly object _portLock = new object();
46+
private static readonly char[] Separators = {'\n', '\r'};
4647

4748
public ArduinoCom()
4849
{
4950
_timer.Tick += TimerOnTick;
5051
_timer.Start();
51-
52-
MessageReceived += (s, a) => Log.Info($"Received: {a.Message}");
5352
}
5453

5554
private void TimerOnTick(object sender, EventArgs e)
@@ -60,27 +59,29 @@ private void TimerOnTick(object sender, EventArgs e)
6059
SendCommand(Command.GetServoData, (byte) Model.Servos.Count);
6160
}
6261

63-
public class StringEventArgs : EventArgs
64-
{
65-
public string Message;
66-
}
67-
68-
public event EventHandler<StringEventArgs> MessageReceived;
69-
7062
private void PortOnDataReceived(object sender, SerialDataReceivedEventArgs e)
7163
{
7264
lock (_portLock)
7365
{
7466
_readBuf.Append(_port.ReadExisting());
75-
}
7667

77-
string str;
78-
while ((str = _readBuf.ToString()).IndexOfAny(new [] {'\n', '\r'}) >= 0)
79-
{
80-
var lines = str.Split(new[] {'\n','\r'}, StringSplitOptions.RemoveEmptyEntries);
81-
Application.Current.Dispatcher.Invoke(() => LineReceived(lines.First().Trim()));
82-
_readBuf.Clear();
83-
_readBuf.Append(string.Join("\n", lines.Skip(1)));
68+
while (_readBuf[0] == '\n')
69+
_readBuf.Remove(0, 1);
70+
71+
string str;
72+
while ((str = _readBuf.ToString()).IndexOfAny(Separators) > 0)
73+
{
74+
var lines = str.Split(Separators, StringSplitOptions.RemoveEmptyEntries);
75+
76+
var line = lines.First().Trim();
77+
Application.Current.Dispatcher.InvokeAsync(() => LineReceived(line));
78+
79+
_readBuf.Clear();
80+
_readBuf.Append(string.Join("\n", lines.Skip(1)));
81+
82+
if (str.LastOrDefault() == '\n')
83+
_readBuf.Append('\n');
84+
}
8485
}
8586
}
8687

@@ -97,7 +98,7 @@ private void LineReceived(string line)
9798
}
9899
catch (Exception e)
99100
{
100-
Log.Error($"Bad DT received: {line} - {e.Message}");
101+
Log.Error($"Bad DT received: {line} - {e.Message}");
101102
}
102103

103104
return;
@@ -107,23 +108,25 @@ private void LineReceived(string line)
107108
{
108109
Application.Current.Dispatcher.Invoke(() =>
109110
{
110-
if (!int.TryParse(line.Substring(3), out var numServos))
111+
if (!int.TryParse(line.Substring(3), out var numServos))
111112
return;
112113

113-
if (numServos >= 33)
114+
if (numServos > 16) // u crazy?
114115
{
115116
Log.Error("Too many servos: " + numServos);
116117
return;
117118
}
118-
119+
119120
Model.Servos.Clear();
120121
for (var i = 0; i < numServos; ++i)
121122
Model.Servos.Add(new ServoPidModel(Model.Servos.Count));
122123
});
123124

124125
SendCommand(Command.GetServoParams);
126+
return;
125127
}
126-
else if (line.StartsWith("SP "))
128+
129+
if (line.StartsWith("SP "))
127130
{
128131
var parts = line.Split(' ');
129132
try
@@ -135,14 +138,19 @@ private void LineReceived(string line)
135138
servo.D = float.Parse(parts[4]);
136139
servo.DLambda = float.Parse(parts[5]);
137140
servo.SetPoint = float.Parse(parts[6]);
141+
142+
if (servoId == Model.Servos.Count - 1)
143+
SendCommand(Command.GetServoData, 0x80);
138144
}
139145
catch (Exception e)
140146
{
141-
Log.Error("Bad servo data: " + line + " - " + e.Message);
142-
return;
147+
Log.Error($"Bad servo data: {line} - {e.Message}");
143148
}
149+
150+
return;
144151
}
145-
else if (line.StartsWith("SD "))
152+
153+
if (line.StartsWith("SD "))
146154
{
147155
var parts = line.Split(' ');
148156
try
@@ -159,23 +167,30 @@ private void LineReceived(string line)
159167
catch (Exception e)
160168
{
161169
Log.Error($"Bad servo data: {line} - {e.Message}");
162-
return;
163170
}
171+
172+
return;
164173
}
165-
else if (line.StartsWith("ERR: "))
174+
175+
if (line.StartsWith("ERR: "))
166176
{
177+
Log.Error($"Received: {line}");
178+
167179
lock (_portLock)
168180
{
169181
_port.WriteLine("RST");
170182
}
183+
184+
return;
171185
}
172-
else
186+
187+
if (line == "RST ACK")
173188
{
174-
Log.Warn($"Ignored: {line}");
189+
Log.Debug("Received: " + line);
175190
return;
176191
}
177192

178-
MessageReceived?.Invoke(this, new StringEventArgs {Message = line});
193+
Log.Warn($"Unknown message: {line}");
179194
}
180195

181196
public ViewModel Model
@@ -304,7 +319,9 @@ private void ConnectPort()
304319
{
305320
_port.Dispose();
306321
_port = null;
307-
Model.Connected = false;
322+
323+
if (Model is ViewModel model)
324+
model.Connected = false;
308325
}
309326
}
310327

@@ -313,11 +330,14 @@ private void ConnectPort()
313330

314331
try
315332
{
316-
_port = new SerialPort(Model.ConnectedPort)
317-
{
318-
BaudRate = 115200,
319-
NewLine = "\n"
320-
};
333+
_port = Model.ConnectedPort == "Mock"
334+
? CreateMockPort()
335+
: new SerialPort(Model.ConnectedPort)
336+
{
337+
BaudRate = 115200,
338+
NewLine = "\n"
339+
};
340+
321341
_port.Open();
322342

323343
Log.Info("Sending: RST");
@@ -339,6 +359,11 @@ private void ConnectPort()
339359
//SendCommand(Command.EnableRegulator, (byte) (Model.Enabled ? 1 : 0));
340360
}
341361

362+
private ISerialPort CreateMockPort()
363+
{
364+
return new MockSerialPort();
365+
}
366+
342367
public void Dispose()
343368
{
344369
_port?.Dispose();

‎ServoPIDControl/Helper/WpfRichTextBoxTarget.cs‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -272,10 +272,11 @@ private void SendTheMessageToRichTextBox(string logMessage, WpfRichTextBoxRowCol
272272
if (this.MaxLines > 0)
273273
{
274274
this.lineCount++;
275+
275276
if (this.lineCount > MaxLines)
276277
{
277-
tr = new TextRange(rtbx.Document.ContentStart, rtbx.Document.ContentEnd);
278-
tr.Text.Remove(0, tr.Text.IndexOf('\n'));
278+
var b = rtbx.Document.Blocks.FirstBlock;
279+
rtbx.Document.Blocks.Remove(b);
279280
this.lineCount--;
280281
}
281282
}

‎ServoPIDControl/ISerialPort.cs‎

Lines changed: 128 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,8 @@
11
using System;
2+
using System.Diagnostics;
3+
using System.Reflection;
4+
using System.Text;
5+
using NLog;
26
using Ports = System.IO.Ports;
37

48
namespace ServoPIDControl
@@ -15,7 +19,7 @@ public interface ISerialPort : IDisposable
1519
string ReadExisting();
1620
void WriteLine(string s);
1721
void Write(byte[] data, int i, int cmdDataLength);
18-
22+
1923
event Ports.SerialDataReceivedEventHandler DataReceived;
2024
}
2125

@@ -61,4 +65,127 @@ public event Ports.SerialDataReceivedEventHandler DataReceived
6165
remove => _port.DataReceived -= value;
6266
}
6367
}
68+
69+
70+
public class MockSerialPort : ISerialPort
71+
{
72+
private static readonly Logger Log = LogManager.GetCurrentClassLogger();
73+
private static readonly Stopwatch StopwatchTotal = Stopwatch.StartNew();
74+
private static readonly Stopwatch StopwatchDelta = Stopwatch.StartNew();
75+
76+
private readonly StringBuilder _mockReadData = new StringBuilder();
77+
private readonly StringBuilder _mockWriteData = new StringBuilder();
78+
79+
public void Dispose()
80+
{
81+
IsOpen = false;
82+
}
83+
84+
public void Open()
85+
{
86+
IsOpen = true;
87+
}
88+
89+
public void Close()
90+
{
91+
IsOpen = false;
92+
}
93+
94+
public bool IsOpen { get; private set; }
95+
96+
public string ReadExisting()
97+
{
98+
var data = _mockReadData.ToString();
99+
_mockReadData.Clear();
100+
return data;
101+
}
102+
103+
public void WriteLine(string s)
104+
{
105+
_mockWriteData.Append(s);
106+
_mockWriteData.Append('\n');
107+
HandleWrittenData();
108+
}
109+
110+
public void Write(byte[] data, int i, int len)
111+
{
112+
_mockWriteData.Append(Encoding.ASCII.GetString(data, i, len));
113+
HandleWrittenData();
114+
}
115+
116+
public event Ports.SerialDataReceivedEventHandler DataReceived;
117+
118+
private void HandleWrittenData()
119+
{
120+
Log.Debug($"Received: '{BitConverter.ToString(Encoding.ASCII.GetBytes(_mockWriteData.ToString()))}'");
121+
122+
if (_mockWriteData.ToString().Contains("RST\n"))
123+
{
124+
Send("RST ACK\n");
125+
var d = _mockWriteData.ToString();
126+
_mockWriteData.Remove(0, d.IndexOf("RST\n", StringComparison.Ordinal) + 4);
127+
}
128+
129+
while (_mockWriteData.Length >= 2 && _mockWriteData.Length >= _mockWriteData[0])
130+
{
131+
var cmd = _mockWriteData.ToString().Substring(0, _mockWriteData[0]);
132+
133+
switch (cmd)
134+
{
135+
case "\u0002\u0003":
136+
Send("NS 4\n");
137+
break;
138+
case "\u0002\u0004":
139+
Send("SP 0 1 2 3 4 5\n");
140+
Send("SP 1 2 2 3 4 5\n");
141+
Send("SP 2 3 2 3 4 5\n");
142+
Send("SP 3 4 2 3 4 5\n");
143+
break;
144+
default:
145+
if (cmd[1] == '\u0001')
146+
{
147+
// ignore
148+
}
149+
else if (cmd.StartsWith("\u0003\u0005"))
150+
{
151+
var t = (float) StopwatchTotal.Elapsed.TotalSeconds * 10;
152+
var dt = (float) StopwatchDelta.Elapsed.TotalSeconds;
153+
StopwatchDelta.Restart();
154+
155+
Send($"DT {dt:F3} 0.2 0.3\n");
156+
Send($"SD 0 {Math.Sin(t) * 2:F3} 0 3 4\n");
157+
Send($"SD 1 {Math.Sin(t) * 4:F3} .5 3 4\n");
158+
Send($"SD 2 {Math.Sin(t) * 6:F3} 1 3 4\n");
159+
Send($"SD 3 {Math.Sin(t) * 8:F3} 1.5 3 4\n");
160+
}
161+
else
162+
{
163+
Log.Warn($"Unknown command: {BitConverter.ToString(Encoding.ASCII.GetBytes(cmd))}");
164+
}
165+
166+
break;
167+
}
168+
169+
_mockWriteData.Remove(0, cmd.Length);
170+
}
171+
}
172+
173+
private void Send(string data)
174+
{
175+
Log.Debug($"Sending: {data.Trim()}");
176+
_mockReadData.Append(data);
177+
DataReceived?.BeginInvoke(this, SerialCharsReceivedEventArgs, null, null);
178+
}
179+
180+
private static readonly ConstructorInfo SerialDataReceivedEventArgsConstructorInfo
181+
= typeof(Ports.SerialDataReceivedEventArgs).GetConstructor(
182+
BindingFlags.NonPublic | BindingFlags.Instance,
183+
null,
184+
new[] {typeof(Ports.SerialData)},
185+
null);
186+
187+
private static readonly Ports.SerialDataReceivedEventArgs SerialCharsReceivedEventArgs =
188+
(Ports.SerialDataReceivedEventArgs) SerialDataReceivedEventArgsConstructorInfo.Invoke(new object[]
189+
{Ports.SerialData.Chars});
190+
}
64191
}

‎ServoPIDControl/MainWindow.xaml‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,8 @@
6666
</DataGrid>
6767
<GridSplitter Grid.Row="1" Grid.Column="0" Grid.ColumnSpan="2"
6868
ResizeDirection="Rows" MinHeight="10" />
69-
<TabControl x:Name="Tab" Grid.Row="2" Grid.Column="1" Margin="5">
69+
<TabControl x:Name="Tab" Grid.Row="2" Grid.Column="1" Margin="5"
70+
SelectionChanged="Tab_OnSelectionChanged">
7071
<TabItem Header="Log">
7172
<RichTextBox x:Name="LogBox"
7273
MinHeight="100" IsReadOnly="True" BorderBrush="{x:Null}"

0 commit comments

Comments
 (0)