Skip to content

Commit dc1072d

Browse files
blackspherefollowerqdot
blackspherefollower
authored and
qdot
committed
Adding support for the new generic commands to the current device drivers
1 parent 27dc5f9 commit dc1072d

13 files changed

Lines changed: 538 additions & 36 deletions

File tree

‎Buttplug.Server.Managers.ErosTek/ET312Device.cs‎

Lines changed: 38 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,12 @@
11
using System;
2+
using System.Collections.Generic;
23
using System.IO.Ports;
34
using System.Text;
45
using System.Threading.Tasks;
56
using System.Timers;
67
using Buttplug.Core;
78
using Buttplug.Core.Messages;
9+
using Buttplug.Server.Util;
810
using static Buttplug.Server.Managers.ETSerialManager.ET312Protocol;
911

1012
namespace Buttplug.Server.Managers.ETSerialManager
@@ -69,8 +71,9 @@ public ET312Device(SerialPort port, IButtplugLogManager aLogManager, string name
6971
}
7072

7173
// We're now ready to receive events
72-
MsgFuncs.Add(typeof(FleshlightLaunchFW12Cmd), HandleFleshlightLaunchFW12Cmd);
73-
MsgFuncs.Add(typeof(StopDeviceCmd), HandleStopDeviceCmd);
74+
MsgFuncs.Add(typeof(FleshlightLaunchFW12Cmd), new ButtplugDeviceWrapper(HandleFleshlightLaunchFW12Cmd));
75+
MsgFuncs.Add(typeof(LinearCmd), new ButtplugDeviceWrapper(HandleLinearCmd, new MessageAttributes() { FeatureCount = 1 }));
76+
MsgFuncs.Add(typeof(StopDeviceCmd), new ButtplugDeviceWrapper(HandleStopDeviceCmd));
7477

7578
// Start update timer
7679
_updateInterval = 20; // <- Change this value to adjust box update frequency in ms
@@ -209,12 +212,41 @@ private async Task<ButtplugMessage> HandleStopDeviceCmd(ButtplugDeviceMessage aM
209212
}
210213
}
211214

215+
private async Task<ButtplugMessage> HandleLinearCmd(ButtplugDeviceMessage aMsg)
216+
{
217+
var cmdMsg = aMsg as LinearCmd;
218+
if (cmdMsg is null)
219+
{
220+
return BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler");
221+
}
222+
223+
foreach (var v in cmdMsg.Vectors)
224+
{
225+
if (v.Index != 0)
226+
{
227+
continue;
228+
}
229+
230+
return await HandleFleshlightLaunchFW12Cmd(new FleshlightLaunchFW12Cmd(cmdMsg.DeviceIndex,
231+
Convert.ToUInt32(FleshlightHelper.GetSpeed(Math.Abs((_position / 100) - v.Position), v.Duration) * 99),
232+
Convert.ToUInt32(v.Position * 99), cmdMsg.Id));
233+
}
234+
235+
return new Ok(aMsg.Id);
236+
}
237+
212238
private async Task<ButtplugMessage> HandleFleshlightLaunchFW12Cmd(ButtplugDeviceMessage aMsg)
213239
{
240+
var cmdMsg = aMsg as FleshlightLaunchFW12Cmd;
241+
if (cmdMsg is null)
242+
{
243+
return BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler");
244+
}
245+
214246
lock (_movementLock)
215247
{
216-
_speed = (aMsg as FleshlightLaunchFW12Cmd).Speed;
217-
_position = (aMsg as FleshlightLaunchFW12Cmd).Position;
248+
_speed = (Convert.ToDouble(cmdMsg.Speed) / 99) * 100;
249+
_position = (Convert.ToDouble(cmdMsg.Position) / 99) * 100;
218250

219251
_position = _position < 0 ? 0 : _position;
220252
_position = _position > 100 ? 100 : _position;
@@ -223,9 +255,8 @@ private async Task<ButtplugMessage> HandleFleshlightLaunchFW12Cmd(ButtplugDevice
223255

224256
// This is @funjack's algorithm for converting Fleshlight Launch
225257
// commands into absolute distance (percent) / duration (millisecond) values
226-
double distance = Math.Abs(_position - _currentPosition);
227-
double mil = Math.Pow(_speed / 25000, -0.95);
228-
double duration = mil / (90 / distance);
258+
var distance = Math.Abs(_position - _currentPosition);
259+
var duration = FleshlightHelper.GetDuration(distance / 100, _speed / 100);
229260

230261
// We convert those into "position" increments for our OnUpdate() timer event.
231262
_increment = 1.5 * (distance / (duration / _updateInterval));

‎Buttplug.Server.Managers.XInputGamepadManager/XInputGamepadDevice.cs‎

Lines changed: 37 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
using System;
2+
using System.Collections.Generic;
23
using System.Threading.Tasks;
34
using Buttplug.Core;
45
using Buttplug.Core.Messages;
@@ -9,13 +10,15 @@ namespace Buttplug.Server.Managers.XInputGamepadManager
910
internal class XInputGamepadDevice : ButtplugDevice
1011
{
1112
private Controller _device;
13+
private double[] _vibratorSpeeds = { 0, 0 };
1214

1315
public XInputGamepadDevice(IButtplugLogManager aLogManager, Controller aDevice)
1416
: base(aLogManager, "XBox Compatible Gamepad (XInput)", aDevice.UserIndex.ToString())
1517
{
1618
_device = aDevice;
17-
MsgFuncs.Add(typeof(SingleMotorVibrateCmd), HandleSingleMotorVibrateCmd);
18-
MsgFuncs.Add(typeof(StopDeviceCmd), HandleStopDeviceCmd);
19+
MsgFuncs.Add(typeof(SingleMotorVibrateCmd), new ButtplugDeviceWrapper(HandleSingleMotorVibrateCmd));
20+
MsgFuncs.Add(typeof(VibrateCmd), new ButtplugDeviceWrapper(HandleVibrateCmd, new MessageAttributes() { FeatureCount = 2 }));
21+
MsgFuncs.Add(typeof(StopDeviceCmd), new ButtplugDeviceWrapper(HandleStopDeviceCmd));
1922
}
2023

2124
private Task<ButtplugMessage> HandleStopDeviceCmd(ButtplugDeviceMessage aMsg)
@@ -27,15 +30,45 @@ private Task<ButtplugMessage> HandleStopDeviceCmd(ButtplugDeviceMessage aMsg)
2730
private Task<ButtplugMessage> HandleSingleMotorVibrateCmd(ButtplugDeviceMessage aMsg)
2831
{
2932
var cmdMsg = aMsg as SingleMotorVibrateCmd;
33+
34+
if (cmdMsg is null)
35+
{
36+
return Task.FromResult<ButtplugMessage>(BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler"));
37+
}
38+
39+
var speeds = new List<VibrateCmd.VibrateIndex>();
40+
for (uint i = 0; i < 2; i++)
41+
{
42+
speeds.Add(new VibrateCmd.VibrateIndex(i, cmdMsg.Speed));
43+
}
44+
45+
return HandleVibrateCmd(new VibrateCmd(cmdMsg.DeviceIndex, speeds, cmdMsg.Id));
46+
}
47+
48+
private Task<ButtplugMessage> HandleVibrateCmd(ButtplugDeviceMessage aMsg)
49+
{
50+
var cmdMsg = aMsg as VibrateCmd;
3051
if (cmdMsg is null)
3152
{
3253
return Task.FromResult<ButtplugMessage>(BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler"));
3354
}
3455

56+
foreach (var vi in cmdMsg.Speeds)
57+
{
58+
if (vi.Index < 0 || vi.Index >= 2)
59+
{
60+
continue;
61+
}
62+
63+
_vibratorSpeeds[vi.Index] = _vibratorSpeeds[vi.Index] < 0 ? 0
64+
: _vibratorSpeeds[vi.Index] > 1 ? 1
65+
: vi.Speed;
66+
}
67+
3568
var v = new Vibration
3669
{
37-
LeftMotorSpeed = (ushort)(cmdMsg.Speed * ushort.MaxValue),
38-
RightMotorSpeed = (ushort)(cmdMsg.Speed * ushort.MaxValue),
70+
LeftMotorSpeed = (ushort)(_vibratorSpeeds[0] * ushort.MaxValue),
71+
RightMotorSpeed = (ushort)(_vibratorSpeeds[1] * ushort.MaxValue),
3972
};
4073

4174
try

‎Buttplug.Server.Test/Buttplug.Server.Test.csproj‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@
6161
<Compile Include="ButtplugDeviceTests.cs" />
6262
<Compile Include="ButtplugJsonMessageParserTests.cs" />
6363
<Compile Include="ButtplugServerTests.cs" />
64+
<Compile Include="FleshlightHelperTests.cs" />
6465
<Compile Include="TestBluetoothSubtypeManager.cs" />
6566
<Compile Include="TestDevice.cs" />
6667
<Compile Include="TestDeviceSubtypeManager.cs" />

‎Buttplug.Server.Test/FleshlightHelperTests.cs‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
using System;
2+
using Buttplug.Server.Util;
3+
using Xunit;
4+
5+
namespace Buttplug.Server.Test
6+
{
7+
public class FleshlightHelperTests
8+
{
9+
[Fact]
10+
public void TestRoundTrip()
11+
{
12+
var delta = FleshlightHelper.GetDistance(100, 0.5);
13+
var speed = FleshlightHelper.GetSpeed(delta, 100);
14+
var time = FleshlightHelper.GetDuration(delta, 0.5);
15+
Assert.True(Math.Abs(0.5 - speed) < 0.01);
16+
Assert.True(Math.Abs(100 - time) < 0.01);
17+
18+
var speed2 = FleshlightHelper.GetSpeed(0.5, 500);
19+
var delta2 = FleshlightHelper.GetDistance(500, speed2);
20+
var time2 = FleshlightHelper.GetDuration(0.5, speed2);
21+
Assert.True(Math.Abs(0.5 - delta2) < 0.01);
22+
Assert.True(Math.Abs(500 - time2) < 10);
23+
24+
var time3 = Convert.ToUInt32(FleshlightHelper.GetDuration(0.5, 0.5));
25+
var speed3 = FleshlightHelper.GetSpeed(0.5, time3);
26+
var delta3 = FleshlightHelper.GetDistance(time3, 0.5);
27+
Assert.True(Math.Abs(0.5 - delta3) < 0.01);
28+
Assert.True(Math.Abs(0.5 - speed3) < 0.01);
29+
}
30+
}
31+
}

‎Buttplug.Server/Bluetooth/Devices/FleshlightLaunch.cs‎

Lines changed: 32 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,10 @@
11
using System;
2+
using System.Collections.Generic;
23
using System.Threading.Tasks;
34
using Buttplug.Core;
45
using Buttplug.Core.Messages;
56
using JetBrains.Annotations;
7+
using Buttplug.Server.Util;
68

79
namespace Buttplug.Server.Bluetooth.Devices
810
{
@@ -42,6 +44,8 @@ public IButtplugDevice CreateDevice(
4244

4345
internal class FleshlightLaunch : ButtplugBluetoothDevice
4446
{
47+
private double _lastPosition = 0;
48+
4549
public FleshlightLaunch([NotNull] IButtplugLogManager aLogManager,
4650
[NotNull] IBluetoothDeviceInterface aInterface,
4751
[NotNull] IBluetoothDeviceInfo aInfo)
@@ -51,8 +55,9 @@ public FleshlightLaunch([NotNull] IButtplugLogManager aLogManager,
5155
aInfo)
5256
{
5357
// Setup message function array
54-
MsgFuncs.Add(typeof(FleshlightLaunchFW12Cmd), HandleFleshlightLaunchRawCmd);
55-
MsgFuncs.Add(typeof(StopDeviceCmd), HandleStopDeviceCmd);
58+
MsgFuncs.Add(typeof(FleshlightLaunchFW12Cmd), new ButtplugDeviceWrapper(HandleFleshlightLaunchRawCmd));
59+
MsgFuncs.Add(typeof(LinearCmd), new ButtplugDeviceWrapper(HandleLinearCmd, new MessageAttributes() { FeatureCount = 1 }));
60+
MsgFuncs.Add(typeof(StopDeviceCmd), new ButtplugDeviceWrapper(HandleStopDeviceCmd));
5661
}
5762

5863
public override async Task<ButtplugMessage> Initialize()
@@ -75,6 +80,29 @@ private Task<ButtplugMessage> HandleStopDeviceCmd(ButtplugDeviceMessage aMsg)
7580
return Task.FromResult<ButtplugMessage>(new Ok(aMsg.Id));
7681
}
7782

83+
private async Task<ButtplugMessage> HandleLinearCmd(ButtplugDeviceMessage aMsg)
84+
{
85+
var cmdMsg = aMsg as LinearCmd;
86+
if (cmdMsg is null)
87+
{
88+
return BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler");
89+
}
90+
91+
foreach (var v in cmdMsg.Vectors)
92+
{
93+
if (v.Index != 0)
94+
{
95+
continue;
96+
}
97+
98+
return await HandleFleshlightLaunchRawCmd(new FleshlightLaunchFW12Cmd(cmdMsg.DeviceIndex,
99+
Convert.ToUInt32(FleshlightHelper.GetSpeed(Math.Abs(_lastPosition - v.Position), v.Duration) * 99),
100+
Convert.ToUInt32(v.Position * 99), cmdMsg.Id));
101+
}
102+
103+
return new Ok(aMsg.Id);
104+
}
105+
78106
private async Task<ButtplugMessage> HandleFleshlightLaunchRawCmd(ButtplugDeviceMessage aMsg)
79107
{
80108
// TODO: Split into Command message and Control message? (Issue #17)
@@ -84,6 +112,8 @@ private async Task<ButtplugMessage> HandleFleshlightLaunchRawCmd(ButtplugDeviceM
84112
return BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler");
85113
}
86114

115+
_lastPosition = cmdMsg.Position / 99;
116+
87117
return await Interface.WriteValue(aMsg.Id,
88118
Info.Characteristics[(uint)FleshlightLaunchBluetoothInfo.Chrs.Tx],
89119
new[] { (byte)cmdMsg.Position, (byte)cmdMsg.Speed });

‎Buttplug.Server/Bluetooth/Devices/Kiiroo.cs‎

Lines changed: 50 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
using Buttplug.Core;
55
using Buttplug.Core.Messages;
66
using JetBrains.Annotations;
7+
using System.Collections.Generic;
78

89
namespace Buttplug.Server.Bluetooth.Devices
910
{
@@ -37,6 +38,8 @@ public IButtplugDevice CreateDevice(IButtplugLogManager aLogManager,
3738

3839
internal class Kiiroo : ButtplugBluetoothDevice
3940
{
41+
private double _vibratorSpeed = 0;
42+
4043
public Kiiroo([NotNull] IButtplugLogManager aLogManager,
4144
[NotNull] IBluetoothDeviceInterface aInterface,
4245
[NotNull] IBluetoothDeviceInfo aInfo)
@@ -45,8 +48,14 @@ public Kiiroo([NotNull] IButtplugLogManager aLogManager,
4548
aInterface,
4649
aInfo)
4750
{
48-
MsgFuncs.Add(typeof(KiirooCmd), HandleKiirooRawCmd);
49-
MsgFuncs.Add(typeof(StopDeviceCmd), HandleStopDeviceCmd);
51+
MsgFuncs.Add(typeof(KiirooCmd), new ButtplugDeviceWrapper(HandleKiirooRawCmd));
52+
MsgFuncs.Add(typeof(StopDeviceCmd), new ButtplugDeviceWrapper(HandleStopDeviceCmd));
53+
54+
if (aInterface.Name == "PEARL")
55+
{
56+
MsgFuncs.Add(typeof(VibrateCmd), new ButtplugDeviceWrapper(HandleVibrateCmd, new MessageAttributes() { FeatureCount = 1 }));
57+
MsgFuncs.Add(typeof(SingleMotorVibrateCmd), new ButtplugDeviceWrapper(HandleSingleMotorVibrateCmd));
58+
}
5059
}
5160

5261
private Task<ButtplugMessage> HandleStopDeviceCmd([NotNull] ButtplugDeviceMessage aMsg)
@@ -70,5 +79,44 @@ private async Task<ButtplugMessage> HandleKiirooRawCmd([NotNull] ButtplugDeviceM
7079
Info.Characteristics[(uint)KiirooBluetoothInfo.Chrs.Tx],
7180
Encoding.ASCII.GetBytes($"{cmdMsg.Position},\n"));
7281
}
82+
83+
private async Task<ButtplugMessage> HandleSingleMotorVibrateCmd([NotNull] ButtplugDeviceMessage aMsg)
84+
{
85+
var cmdMsg = aMsg as SingleMotorVibrateCmd;
86+
if (cmdMsg is null)
87+
{
88+
return BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler");
89+
}
90+
91+
if (_vibratorSpeed == cmdMsg.Speed)
92+
{
93+
return new Ok(cmdMsg.Id);
94+
}
95+
96+
_vibratorSpeed = cmdMsg.Speed;
97+
98+
return await HandleVibrateCmd(new VibrateCmd(cmdMsg.DeviceIndex,
99+
new List<VibrateCmd.VibrateIndex>() { new VibrateCmd.VibrateIndex(0, cmdMsg.Speed) },
100+
cmdMsg.Id));
101+
}
102+
103+
private async Task<ButtplugMessage> HandleVibrateCmd([NotNull] ButtplugDeviceMessage aMsg)
104+
{
105+
var cmdMsg = aMsg as VibrateCmd;
106+
if (cmdMsg is null)
107+
{
108+
return BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler");
109+
}
110+
111+
foreach (var vi in cmdMsg.Speeds)
112+
{
113+
if (vi.Index == 0)
114+
{
115+
_vibratorSpeed = vi.Speed;
116+
}
117+
}
118+
119+
return await HandleKiirooRawCmd(new KiirooCmd(aMsg.DeviceIndex, Convert.ToUInt16(_vibratorSpeed * 3), aMsg.Id));
120+
}
73121
}
74122
}

0 commit comments

Comments
 (0)