Skip to content

Commit 28e2fb7

Browse files
tthierytthiery
committed
Split Motor Control in various Levels
- Added Notification Infrastructure to device properties. - Added interface to IPoweredUpProtocol to allow routing layer in hubs Closes #1
1 parent bcc46cd commit 28e2fb7

File tree

10 files changed

+326
-104
lines changed

10 files changed

+326
-104
lines changed

‎examples/message-trace/Program.cs‎

Lines changed: 29 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,8 @@ static async Task Main(string[] args)
5252

5353
await kernel.ConnectAsync();
5454

55+
await protocol.SetupUpstreamObservableAsync();
56+
5557
await protocol.ReceiveMessageAsync(message =>
5658
{
5759
try
@@ -73,18 +75,18 @@ await protocol.ReceiveMessageAsync(message =>
7375
PortInformationForPossibleModeCombinationsMessage msg => $"Port Information (Combinations) - Port {msg.PortId} Combinations: {string.Join(",", msg.ModeCombinations.Select(x => x.ToString("X")))}",
7476
PortValueSingleMessage msg => "Port Values - " + string.Join(";", msg.Data.Select(d => d switch
7577
{
76-
PortValueData<sbyte> dd => $"Port {dd.PortId}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
77-
PortValueData<short> dd => $"Port {dd.PortId}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
78-
PortValueData<int> dd => $"Port {dd.PortId}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
79-
PortValueData<float> dd => $"Port {dd.PortId}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
78+
PortValueData<sbyte> dd => $"Port {dd.PortId}/{dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
79+
PortValueData<short> dd => $"Port {dd.PortId}/{dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
80+
PortValueData<int> dd => $"Port {dd.PortId}/{dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
81+
PortValueData<float> dd => $"Port {dd.PortId}/{dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
8082
_ => "Undefined Data Type",
8183
})),
8284
PortValueCombinedModeMessage msg => $"Port Value (Combined Mode) - Port {msg.PortId} " + string.Join(";", msg.Data.Select(d => d switch
8385
{
84-
PortValueData<sbyte> dd => $"Mode {dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
85-
PortValueData<short> dd => $"Mode {dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
86-
PortValueData<int> dd => $"Mode {dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
87-
PortValueData<float> dd => $"Mode {dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
86+
PortValueData<sbyte> dd => $"Mode {dd.ModeIndex}/{dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
87+
PortValueData<short> dd => $"Mode {dd.ModeIndex}/{dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
88+
PortValueData<int> dd => $"Mode {dd.ModeIndex}/{dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
89+
PortValueData<float> dd => $"Mode {dd.ModeIndex}/{dd.ModeIndex}: {string.Join(",", dd.InputValues)} ({dd.DataType})",
8890
_ => "Undefined Data Type",
8991
})),
9092
PortInputFormatSingleMessage msg => $"Port Input Format (Single) - Port {msg.PortId}, Mode {msg.Mode}, Threshold {msg.DeltaInterval}, Notification {msg.NotificationEnabled}",
@@ -157,7 +159,7 @@ await protocol.ReceiveMessageAsync(message =>
157159
await rgbLight.SetRgbColorsAsync(0x00, 0xff, 0x00);
158160
//await rgbLight.SetRgbColorsAsync(0xFF, 0x00, 0x00);
159161

160-
var motor = new TechnicXLargeLinearMotor(protocol, 0);
162+
var motor = new TechnicXLargeLinearMotor(protocol, 0, 0);
161163
// await motor.SetAccelerationTime(3000);
162164
// await motor.SetDeccelerationTime(1000);
163165
// await motor.StartSpeedForTimeAsync(6000, 90, 100, PortOutputCommandSpecialSpeed.Hold, PortOutputCommandSpeedProfile.AccelerationProfile | PortOutputCommandSpeedProfile.DeccelerationProfile);
@@ -166,11 +168,27 @@ await protocol.ReceiveMessageAsync(message =>
166168

167169
//await motor.StartSpeedForDegrees(180, -10, 100, PortOutputCommandSpecialSpeed.Brake, PortOutputCommandSpeedProfile.None);
168170

169-
await motor.GotoAbsolutePosition(45, 10, 100, PortOutputCommandSpecialSpeed.Brake, PortOutputCommandSpeedProfile.None);
171+
await motor.GotoAbsolutePositionAsync(45, 10, 100, PortOutputCommandSpecialSpeed.Brake, PortOutputCommandSpeedProfile.None);
172+
await Task.Delay(2000);
173+
await motor.GotoAbsolutePositionAsync(-45, 10, 100, PortOutputCommandSpecialSpeed.Brake, PortOutputCommandSpeedProfile.None);
174+
175+
await Task.Delay(2000);
176+
177+
await motor.SetupNotificationAsync(motor.ModeIndexAbsolutePosition, true);
178+
179+
await motor.StartPowerAsync(80);
180+
181+
logger.LogWarning($"XXXXXXXXXXXXXXXXXXXXXXXXXXXXX: {motor.AbsolutePosition}");
182+
183+
await Task.Delay(2000);
184+
185+
logger.LogWarning($"XXXXXXXXXXXXXXXXXXXXXXXXXXXXX: {motor.AbsolutePosition}");
186+
170187
await Task.Delay(2000);
171-
await motor.GotoAbsolutePosition(-45, 10, 100, PortOutputCommandSpecialSpeed.Brake, PortOutputCommandSpeedProfile.None);
172188

189+
await motor.StartPowerAsync(0);
173190

191+
logger.LogWarning($"XXXXXXXXXXXXXXXXXXXXXXXXXXXXX: {motor.AbsolutePosition}");
174192
// await motor.StartSpeedAsync(100, 90, PortOutputCommandSpeedProfile.None);
175193

176194
// await Task.Delay(2000);

‎src/SharpBrick.PoweredUp/Devices/AbsoluteMotor.cs‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
using System.Threading.Tasks;
2+
using SharpBrick.PoweredUp.Protocol;
3+
using SharpBrick.PoweredUp.Protocol.Messages;
4+
5+
namespace SharpBrick.PoweredUp.Devices
6+
{
7+
public abstract class AbsoluteMotor : TachoMotor
8+
{
9+
public byte ModeIndexAbsolutePosition { get; protected set; } = 3;
10+
public short AbsolutePosition { get; private set; } // ModeIndex = 3
11+
public AbsoluteMotor()
12+
{ }
13+
protected AbsoluteMotor(PoweredUpProtocol protocol, byte hubId, byte portId)
14+
: base(protocol, hubId, portId)
15+
{ }
16+
17+
protected override bool OnPortValueChange(PortValueData data)
18+
=> data.ModeIndex switch
19+
{
20+
var mode when (mode == ModeIndexAbsolutePosition) => OnAbsolutePositionChange(data as PortValueData<short>),
21+
_ => base.OnPortValueChange(data),
22+
};
23+
24+
private bool OnAbsolutePositionChange(PortValueData<short> data)
25+
{
26+
AbsolutePosition = data.InputValues[0];
27+
28+
return true;
29+
}
30+
31+
public async Task GotoAbsolutePositionAsync(int absolutePosition, sbyte speed, byte maxPower, PortOutputCommandSpecialSpeed endState, PortOutputCommandSpeedProfile profile)
32+
{
33+
await _protocol.SendMessageAsync(new PortOutputCommandGotoAbsolutePositionMessage()
34+
{
35+
PortId = _portId,
36+
StartupInformation = PortOutputCommandStartupInformation.ExecuteImmediately,
37+
CompletionInformation = PortOutputCommandCompletionInformation.CommandFeedback,
38+
AbsolutePosition = absolutePosition,
39+
Speed = speed,
40+
MaxPower = maxPower,
41+
EndState = endState,
42+
Profile = profile,
43+
});
44+
}
45+
}
46+
}

‎src/SharpBrick.PoweredUp/Devices/BasicMotor.cs‎

Lines changed: 17 additions & 87 deletions
Original file line numberDiff line numberDiff line change
@@ -1,113 +1,43 @@
1-
using System;
21
using System.Threading.Tasks;
32
using SharpBrick.PoweredUp.Protocol;
43
using SharpBrick.PoweredUp.Protocol.Messages;
54

65
namespace SharpBrick.PoweredUp.Devices
76
{
8-
public abstract class BasicMotor
7+
public abstract class BasicMotor : Device
98
{
10-
protected readonly PoweredUpProtocol _protocol;
11-
protected readonly byte _portId;
9+
public byte ModeIndexPower { get; protected set; } = 0;
10+
public sbyte Power { get; private set; } = 0;
1211

1312
public BasicMotor()
1413
{ }
15-
public BasicMotor(PoweredUpProtocol protocol, byte portId)
16-
{
17-
_protocol = protocol ?? throw new ArgumentNullException(nameof(protocol));
18-
_portId = portId;
19-
}
20-
21-
public async Task StartPowerAsync(sbyte power)
22-
{
23-
await _protocol.SendMessageAsync(new PortOutputCommandStartPowerMessage()
24-
{
25-
PortId = _portId,
26-
StartupInformation = PortOutputCommandStartupInformation.ExecuteImmediately,
27-
CompletionInformation = PortOutputCommandCompletionInformation.CommandFeedback,
28-
Power = power,
29-
});
30-
}
3114

32-
public async Task SetAccelerationTime(ushort timeInMs, PortOutputCommandSpeedProfile profileNumber = PortOutputCommandSpeedProfile.AccelerationProfile)
33-
{
34-
await _protocol.SendMessageAsync(new PortOutputCommandSetAccTimeMessage()
35-
{
36-
PortId = _portId,
37-
StartupInformation = PortOutputCommandStartupInformation.ExecuteImmediately,
38-
CompletionInformation = PortOutputCommandCompletionInformation.CommandFeedback,
39-
Time = timeInMs,
40-
Profile = profileNumber,
41-
});
42-
}
43-
44-
public async Task SetDeccelerationTime(ushort timeInMs, PortOutputCommandSpeedProfile profileNumber = PortOutputCommandSpeedProfile.DeccelerationProfile)
45-
{
46-
await _protocol.SendMessageAsync(new PortOutputCommandSetDecTimeMessage()
47-
{
48-
PortId = _portId,
49-
StartupInformation = PortOutputCommandStartupInformation.ExecuteImmediately,
50-
CompletionInformation = PortOutputCommandCompletionInformation.CommandFeedback,
51-
Time = timeInMs,
52-
Profile = profileNumber,
53-
});
54-
}
15+
public BasicMotor(IPoweredUpProtocol protocol, byte hubId, byte portId)
16+
: base(protocol, hubId, portId)
17+
{ }
5518

56-
public async Task StartSpeedAsync(sbyte speed, byte maxPower, PortOutputCommandSpeedProfile profile)
57-
{
58-
await _protocol.SendMessageAsync(new PortOutputCommandStartSpeedMessage()
19+
protected override bool OnPortValueChange(PortValueData data)
20+
=> data.ModeIndex switch
5921
{
60-
PortId = _portId,
61-
StartupInformation = PortOutputCommandStartupInformation.ExecuteImmediately,
62-
CompletionInformation = PortOutputCommandCompletionInformation.CommandFeedback,
63-
Speed = speed,
64-
MaxPower = maxPower,
65-
Profile = profile,
66-
});
67-
}
22+
var mode when (mode == ModeIndexPower) => OnPowerChange(data as PortValueData<sbyte>),
23+
_ => base.OnPortValueChange(data),
24+
};
6825

69-
public async Task StartSpeedForTimeAsync(ushort time, sbyte speed, byte maxPower, PortOutputCommandSpecialSpeed endState, PortOutputCommandSpeedProfile profile)
26+
private bool OnPowerChange(PortValueData<sbyte> data)
7027
{
71-
await _protocol.SendMessageAsync(new PortOutputCommandStartSpeedForTimeMessage()
72-
{
73-
PortId = _portId,
74-
StartupInformation = PortOutputCommandStartupInformation.ExecuteImmediately,
75-
CompletionInformation = PortOutputCommandCompletionInformation.CommandFeedback,
76-
Time = time,
77-
Speed = speed,
78-
MaxPower = maxPower,
79-
EndState = endState,
80-
Profile = profile,
81-
});
82-
}
28+
Power = data.InputValues[0];
8329

84-
public async Task StartSpeedForDegrees(uint degrees, sbyte speed, byte maxPower, PortOutputCommandSpecialSpeed endState, PortOutputCommandSpeedProfile profile)
85-
{
86-
await _protocol.SendMessageAsync(new PortOutputCommandStartSpeedForDegreesMessage()
87-
{
88-
PortId = _portId,
89-
StartupInformation = PortOutputCommandStartupInformation.ExecuteImmediately,
90-
CompletionInformation = PortOutputCommandCompletionInformation.CommandFeedback,
91-
Degrees = degrees,
92-
Speed = speed,
93-
MaxPower = maxPower,
94-
EndState = endState,
95-
Profile = profile,
96-
});
30+
return true;
9731
}
9832

99-
public async Task GotoAbsolutePosition(int absolutePosition, sbyte speed, byte maxPower, PortOutputCommandSpecialSpeed endState, PortOutputCommandSpeedProfile profile)
33+
public async Task StartPowerAsync(sbyte power)
10034
{
101-
await _protocol.SendMessageAsync(new PortOutputCommandGotoAbsolutePositionMessage()
35+
await _protocol.SendMessageAsync(new PortOutputCommandStartPowerMessage()
10236
{
10337
PortId = _portId,
10438
StartupInformation = PortOutputCommandStartupInformation.ExecuteImmediately,
10539
CompletionInformation = PortOutputCommandCompletionInformation.CommandFeedback,
106-
AbsolutePosition = absolutePosition,
107-
Speed = speed,
108-
MaxPower = maxPower,
109-
EndState = endState,
110-
Profile = profile,
40+
Power = power,
11141
});
11242
}
11343
}

‎src/SharpBrick.PoweredUp/Devices/Device.cs‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
using System;
2+
using System.Reactive.Linq;
3+
using System.Threading.Tasks;
4+
using SharpBrick.PoweredUp.Protocol;
5+
using SharpBrick.PoweredUp.Protocol.Messages;
6+
7+
namespace SharpBrick.PoweredUp.Devices
8+
{
9+
public abstract class Device : IDisposable
10+
{
11+
protected readonly IPoweredUpProtocol _protocol;
12+
private readonly byte _hubId;
13+
protected readonly byte _portId;
14+
private readonly IDisposable _receivingDisposable;
15+
16+
public Device()
17+
{ }
18+
19+
public Device(IPoweredUpProtocol protocol, byte hubId, byte portId)
20+
{
21+
_protocol = protocol ?? throw new ArgumentNullException(nameof(protocol));
22+
_hubId = hubId;
23+
_portId = portId;
24+
25+
_receivingDisposable = _protocol.UpstreamMessages
26+
.Where(msg => msg.HubId == _hubId)
27+
.SelectMany(msg => msg switch
28+
{
29+
PortValueSingleMessage pvsm => pvsm.Data,
30+
PortValueCombinedModeMessage pvcmm => pvcmm.Data,
31+
_ => Array.Empty<PortValueData>(),
32+
})
33+
.Where(pvd => pvd.PortId == _portId)
34+
.Subscribe(pvd =>
35+
{
36+
OnPortValueChange(pvd);
37+
});
38+
}
39+
40+
public async Task SetupNotificationAsync(byte modeIndex, bool enabled, uint deltaInterval = 5)
41+
{
42+
await _protocol.SendMessageAsync(new PortInputFormatSetupSingleMessage()
43+
{
44+
PortId = _portId,
45+
Mode = modeIndex,
46+
DeltaInterval = deltaInterval,
47+
NotificationEnabled = enabled,
48+
});
49+
}
50+
51+
protected virtual bool OnPortValueChange(PortValueData portValue)
52+
=> false;
53+
54+
#region Disposable Pattern
55+
private bool disposedValue;
56+
protected virtual void Dispose(bool disposing)
57+
{
58+
if (!disposedValue)
59+
{
60+
if (disposing)
61+
{
62+
_receivingDisposable?.Dispose();
63+
}
64+
65+
// TODO: Nicht verwaltete Ressourcen (nicht verwaltete Objekte) freigeben und Finalizer überschreiben
66+
// TODO: Große Felder auf NULL setzen
67+
disposedValue = true;
68+
}
69+
}
70+
71+
// // TODO: Finalizer nur überschreiben, wenn "Dispose(bool disposing)" Code für die Freigabe nicht verwalteter Ressourcen enthält
72+
// ~Device()
73+
// {
74+
// // Ändern Sie diesen Code nicht. Fügen Sie Bereinigungscode in der Methode "Dispose(bool disposing)" ein.
75+
// Dispose(disposing: false);
76+
// }
77+
78+
public void Dispose()
79+
{
80+
// Ändern Sie diesen Code nicht. Fügen Sie Bereinigungscode in der Methode "Dispose(bool disposing)" ein.
81+
Dispose(disposing: true);
82+
GC.SuppressFinalize(this);
83+
}
84+
85+
#endregion
86+
}
87+
}

0 commit comments

Comments
 (0)