Skip to content

Commit e475c95

Browse files
author
Marcus Sonestedt
committed
ServoPID: Fixes to com protocol + others
1 parent b386db2 commit e475c95

File tree

4 files changed

+130
-78
lines changed

4 files changed

+130
-78
lines changed

‎ServoPID/ServoPIDControl/ArduinoCom.cs‎

Lines changed: 85 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
using System.IO.Ports;
66
using System.Linq;
77
using System.Text;
8+
using System.Windows;
89
using System.Windows.Threading;
910

1011
namespace ServoPIDControl
@@ -44,14 +45,16 @@ public ArduinoCom()
4445
{
4546
_timer.Tick += TimerOnTick;
4647
_timer.Start();
48+
49+
MessageReceived += (s, a) => Debug.WriteLine($"Received: {a.Message}");
4750
}
4851

4952
private void TimerOnTick(object sender, EventArgs e)
5053
{
5154
if ((!_port?.IsOpen ?? false) || Model == null)
5255
return;
5356

54-
SendCommand(Command.GetServoData);
57+
SendCommand(Command.GetServoData, (byte)Model.Servos.Count);
5558
}
5659

5760
public class StringEventArgs : EventArgs
@@ -70,10 +73,10 @@ private void PortOnDataReceived(object sender, SerialDataReceivedEventArgs e)
7073

7174
while (_readBuf.ToString().Contains('\n'))
7275
{
73-
Debug.WriteLine(_readBuf.ToString());
76+
//Debug.WriteLine(_readBuf.ToString());
7477

7578
var lines = _readBuf.ToString().Split('\n');
76-
foreach (var line in lines)
79+
foreach (var line in lines.Select(l => l.Trim()).Where(l => !string.IsNullOrWhiteSpace(l)))
7780
LineReceived(line);
7881

7982
_readBuf.Clear();
@@ -82,47 +85,65 @@ private void PortOnDataReceived(object sender, SerialDataReceivedEventArgs e)
8285

8386
private void LineReceived(string line)
8487
{
85-
if (line.StartsWith("NS "))
88+
if (line.StartsWith("DT "))
8689
{
87-
Model.Servos.Clear();
88-
89-
var numServos = int.Parse(line.Substring(3));
90-
for (var i = 0; i < numServos; ++i)
91-
Model.Servos.Add(new ServoPidModel(Model.Servos.Count));
92-
90+
if (float.TryParse(line.Substring(3), out var dt))
91+
Model.DeltaTime = dt;
92+
else
93+
Debug.WriteLine("Bad DT received: " + line);
94+
9395
return;
9496
}
9597

96-
if (line.StartsWith("SP "))
98+
if (line.StartsWith("NS "))
9799
{
98-
var parts = line.Split('\n');
99-
var servoId = int.Parse(parts[1]);
100-
var servo = Model.Servos[servoId];
101-
servo.P = float.Parse(parts[2]);
102-
servo.I = float.Parse(parts[3]);
103-
servo.D = float.Parse(parts[4]);
104-
servo.DLambda = float.Parse(parts[5]);
105-
servo.SetPoint = float.Parse(parts[6]);
106-
return;
107-
}
100+
Application.Current.Dispatcher.Invoke(() =>
101+
{
102+
Model.Servos.Clear();
108103

109-
if (line.StartsWith("SD "))
104+
if (int.TryParse(line.Substring(3), out var numServos))
105+
for (var i = 0; i < numServos; ++i)
106+
Model.Servos.Add(new ServoPidModel(Model.Servos.Count));
107+
});
108+
109+
SendCommand(Command.GetServoParams);
110+
}
111+
else if (line.StartsWith("SP "))
110112
{
111-
var parts = line.Split('\n');
112-
var servoId = int.Parse(parts[1]);
113-
var servo = Model.Servos[servoId];
114-
servo.Input = float.Parse(parts[2]);
115-
servo.Output = float.Parse(parts[3]);
116-
servo.Integrator = float.Parse(parts[4]);
117-
servo.DFiltered = float.Parse(parts[5]);
118-
return;
113+
var parts = line.Split(' ');
114+
try
115+
{
116+
var servoId = int.Parse(parts[1]);
117+
var servo = Model.Servos[servoId];
118+
servo.P = float.Parse(parts[2]);
119+
servo.I = float.Parse(parts[3]);
120+
servo.D = float.Parse(parts[4]);
121+
servo.DLambda = float.Parse(parts[5]);
122+
servo.SetPoint = float.Parse(parts[6]);
123+
}
124+
catch (Exception e)
125+
{
126+
Debug.WriteLine("Bad servo data: " + line + " - " + e.Message);
127+
return;
128+
}
119129
}
120-
121-
if (line.StartsWith("DT "))
130+
else if (line.StartsWith("SD "))
122131
{
123-
var dt = float.Parse(line.Substring(3));
124-
Model.DeltaTime = dt;
125-
return;
132+
var parts = line.Split(' ');
133+
try
134+
{
135+
var servoId = int.Parse(parts[1]);
136+
var servo = Model.Servos[servoId];
137+
servo.Input = float.Parse(parts[2]);
138+
servo.Output = float.Parse(parts[3]);
139+
servo.Integrator = float.Parse(parts[4]);
140+
servo.DFiltered = float.Parse(parts[5]);
141+
}
142+
catch (Exception e)
143+
{
144+
Debug.WriteLine("Bad servo data: " + line + " - " + e.Message);
145+
return;
146+
}
126147
}
127148

128149
MessageReceived?.Invoke(this, new StringEventArgs {Message = line});
@@ -191,27 +212,31 @@ private void ServoOnPropertyChanged(object sender, PropertyChangedEventArgs e)
191212

192213
private void ServosOnCollectionChanged(object sender, NotifyCollectionChangedEventArgs e)
193214
{
194-
foreach (var servo in e.OldItems.Cast<ServoPidModel>())
195-
servo.PropertyChanged -= ServoOnPropertyChanged;
215+
if (e.OldItems != null)
216+
foreach (var servo in e.OldItems.Cast<ServoPidModel>())
217+
servo.PropertyChanged -= ServoOnPropertyChanged;
196218

197-
foreach (var servo in e.NewItems.Cast<ServoPidModel>())
198-
servo.PropertyChanged += ServoOnPropertyChanged;
219+
if (e.NewItems != null)
220+
foreach (var servo in e.NewItems.Cast<ServoPidModel>())
221+
servo.PropertyChanged += ServoOnPropertyChanged;
199222

200223
if (e.Action == NotifyCollectionChangedAction.Reset)
201-
throw new NotImplementedException("Servo collection was reset");
224+
{
225+
foreach (var servo in Model.Servos)
226+
servo.PropertyChanged -= ServoOnPropertyChanged;
227+
}
202228
}
203229

204230
private void ModelOnPropertyChanged(object sender, PropertyChangedEventArgs e)
205231
{
206232
if (e.PropertyName == null || e.PropertyName == nameof(Model.Enabled))
207-
if (_port.IsOpen)
208-
SendCommand(Command.EnableRegulator, (byte) (Model.Enabled ? 1 : 0));
209-
210-
if (e.PropertyName == null || e.PropertyName == nameof(Model.ComPorts))
211-
ConnectPort();
233+
SendCommand(Command.EnableRegulator, (byte) (Model.Enabled ? 1 : 0));
234+
235+
if ((e.PropertyName == null || e.PropertyName == nameof(Model.PortName)))
236+
ConnectPort();
212237

213238
if (e.PropertyName == null || e.PropertyName == nameof(Model.PollPidData))
214-
_timer.IsEnabled = Model.PollPidData;
239+
_timer.IsEnabled = Model.PollPidData;
215240
}
216241

217242
private void SendCommand(Command cmd, params byte[] data)
@@ -220,6 +245,10 @@ private void SendCommand(Command cmd, params byte[] data)
220245

221246
lock (_portLock)
222247
{
248+
if (_port == null || !_port.IsOpen)
249+
return;
250+
251+
Debug.WriteLine($"Sending {cmdData.Length}: {BitConverter.ToString(cmdData)}");
223252
_port.Write(cmdData, 0, cmdData.Length);
224253
}
225254
}
@@ -246,20 +275,25 @@ private void ConnectPort()
246275
}
247276
finally
248277
{
249-
250278
_port.Dispose();
251279
_port = null;
252280
Model.Connected = false;
253281
}
254282
}
255283

256-
if (Model == null)
284+
if (Model?.PortName == null)
257285
return;
258286

259287
try
260288
{
261-
_port = new SerialPort(Model.PortName);
289+
_port = new SerialPort(Model.PortName)
290+
{
291+
BaudRate = 115200
292+
};
262293
_port.Open();
294+
295+
Debug.WriteLine("Sending: RST");
296+
_port.WriteLine("RST");
263297
_port.DataReceived += PortOnDataReceived;
264298

265299
Model.Connected = true;
@@ -268,15 +302,13 @@ private void ConnectPort()
268302
{
269303
_port?.Dispose();
270304
_port = null;
271-
throw;
305+
//throw;
272306
}
273307
}
274308

275309
SendCommand(Command.GetNumServos);
276-
SendCommand(Command.GetServoParams);
277-
SendCommand(Command.GetServoData);
278310

279-
SendCommand(Command.EnableRegulator, (byte)(Model.Enabled ? 1 : 0));
311+
//SendCommand(Command.EnableRegulator, (byte) (Model.Enabled ? 1 : 0));
280312
}
281313

282314
public void Dispose()

‎ServoPID/ServoPIDControl/ArduinoModel.cs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ public ArduinoModel()
2828
Servos.Add(new ServoPidModel(i));
2929

3030
_timer = new DispatcherTimer {Interval = TimeSpan.FromSeconds(1), IsEnabled = true};
31-
_timer.Tick += (s, a) => ComPorts = SerialPort.GetPortNames();
31+
_timer.Tick += (s, a) => ComPorts = SerialPort.GetPortNames().Distinct().ToArray();
3232
}
3333

3434
public string PortName

‎ServoPID/ServoPIDControl/MainWindow.xaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@
3232
<CheckBox x:Name="PollPidDataCheckBox" IsChecked="{Binding PollPidData}" Content="Poll PID data"
3333
IsEnabled="{Binding Connected}" />
3434
<TextBlock Text="Delta Time:" />
35-
<TextBox x:Name="DeltaTimeTextBox" Text="{Binding DeltaTime, Mode=OneWay, StringFormat=F3}" IsReadOnly="true"
35+
<TextBox x:Name="DeltaTimeTextBox" Text="{Binding DeltaTime, Mode=OneWay, StringFormat=F6}" IsReadOnly="true"
3636
IsEnabled="{Binding Connected}" TextAlignment="Right" />
3737
</StackPanel>
3838
<DataGrid Grid.Row="0" Grid.Column="1"

0 commit comments

Comments
 (0)