diff --git a/LaunchServerLib/VAMLaunchServer.cs b/LaunchServerLib/VAMLaunchServer.cs index faad148..741ef52 100644 --- a/LaunchServerLib/VAMLaunchServer.cs +++ b/LaunchServerLib/VAMLaunchServer.cs @@ -1,22 +1,18 @@ using System; +using System.Collections.Generic; using System.Threading; namespace VAMLaunch { - public class PositionUpdateEventArgs : EventArgs + public class CommandEventArgs : EventArgs { - // 0-99, in Launch FW 1.2 message units - public uint Position; - // 0-99, in Launch FW 1.2 message units - public uint Speed; - // time in decimal seconds - public float Duration; + public Command Command; } public class VAMLaunchServer { - public EventHandler PositionUpdate; + public EventHandler CommandUpdate; private const string SERVER_IP = "127.0.0.1"; private const int SERVER_LISTEN_PORT = 15601; private const int SERVER_SEND_PORT = 15600; @@ -32,10 +28,7 @@ public class VAMLaunchServer private bool _running; - private byte _latestLaunchPos; - private byte _latestLaunchSpeed; - private float _latestLaunchDuration; - private bool _hasNewLaunchSnapshot; + private bool _hasNewCommands; private DateTime _timeOfLastLaunchUpdate; public void Run() @@ -98,30 +91,101 @@ private void UpdateMovement() TimeSpan timeSinceLastUpdate = now - _timeOfLastLaunchUpdate; if (timeSinceLastUpdate.TotalSeconds > LAUNCH_UPDATE_INTERVAL) { - if (_hasNewLaunchSnapshot) + if (_hasNewCommands) { - PositionUpdate?.Invoke(this, new PositionUpdateEventArgs { Position = _latestLaunchPos, Speed = _latestLaunchSpeed, Duration = _latestLaunchDuration }); - _hasNewLaunchSnapshot = false; + foreach(var cmd in _latestCommands.Values) + { + CommandUpdate?.Invoke(this, new CommandEventArgs { Command = cmd }); + } + _latestCommands.Clear(); + _hasNewCommands = false; } _timeOfLastLaunchUpdate = now; } } + private Dictionary<(int, int, int), Command> _latestCommands = new Dictionary<(int, int, int), Command>(); private void ProcessNetworkMessages() { - byte[] msg = _network.GetNextMessage(); - if (msg != null && msg.Length == 6) + var cmd = Command.Parse(_network.GetNextMessage()); + if(cmd != null) { - _latestLaunchPos = msg[0]; - _latestLaunchSpeed = msg[1]; - _latestLaunchDuration = BitConverter.ToSingle(msg, 2); + _latestCommands[(cmd.Type, cmd.Device, cmd.Motor)] = cmd; + _hasNewCommands = true; + } + } + } -// Console.WriteLine("Receiving: P:{0}, S:{1}, D:{2}", _latestLaunchPos, _latestLaunchSpeed, -// _latestLaunchDuration); - - _hasNewLaunchSnapshot = true; + public class Command + { + public const byte LINEAR_CMD = 0; + public const byte VIBRATE_CMD = 1; + public const byte ROTATE_CMD = 2; + + public const byte DEVICE_ALL = 0; + public const byte MOTOR_ALL = 0; + + public int Type; + public int Device; + public int Motor; + public List Params; + + public Command() + { + Type = -1; + Device = DEVICE_ALL; + Motor = MOTOR_ALL; + Params = new List(); + } + + public static Command Parse(byte[] data) + { + if(data == null || data.Length < 4) + { + return null; + } + + var cmd = new Command() + { + Device = data[2], + Motor = data[3] + }; + + switch(data[1]) + { + case LINEAR_CMD: + if(data.Length < 12) + { + return null; + } + cmd.Type = LINEAR_CMD; + cmd.Params.Add(BitConverter.ToSingle(data, 4)); // duration + cmd.Params.Add(BitConverter.ToSingle(data, 8)); // position + break; + case VIBRATE_CMD: + if(data.Length < 8) + { + return null; + } + cmd.Type = VIBRATE_CMD; + cmd.Params.Add(BitConverter.ToSingle(data, 4)); // speed + break; + case ROTATE_CMD: + if(data.Length < 9) + { + return null; + } + cmd.Type = ROTATE_CMD; + cmd.Params.Add(BitConverter.ToSingle(data, 4)); // speed + cmd.Params.Add((float)data[8]); // clockwise + break; + default: + return null; } + + return cmd; } + } } diff --git a/VAMLaunch/ADD_ME.cslist b/VAMLaunch/ADD_ME.cslist index add6168..ee4eb28 100644 --- a/VAMLaunch/ADD_ME.cslist +++ b/VAMLaunch/ADD_ME.cslist @@ -4,4 +4,5 @@ src/LaunchUtils.cs src/MotionSources/IMotionSource.cs src/MotionSources/ZoneSource.cs src/MotionSources/OscillateSource.cs -src/MotionSources/PatternSource.cs \ No newline at end of file +src/MotionSources/PatternSource.cs +src/MotionSources/ManualSource.cs \ No newline at end of file diff --git a/VAMLaunch/VAMLaunch.csproj b/VAMLaunch/VAMLaunch.csproj index 775fe09..69150f4 100644 --- a/VAMLaunch/VAMLaunch.csproj +++ b/VAMLaunch/VAMLaunch.csproj @@ -92,6 +92,7 @@ + diff --git a/VAMLaunch/src/MotionSources/ManualSource.cs b/VAMLaunch/src/MotionSources/ManualSource.cs new file mode 100644 index 0000000..365fa12 --- /dev/null +++ b/VAMLaunch/src/MotionSources/ManualSource.cs @@ -0,0 +1,123 @@ +using System.Collections.Generic; +using UnityEngine; + +namespace VAMLaunchPlugin.MotionSources +{ + + class Command + { + public string Method; + public int Device; + public int Motor; + public float Percent; + } + + public class ManualSource : IMotionSource + { + + List _storableCommands = new List(); + List _uiCommands = new List(); + VAMLaunch _plugin; + + Dictionary _flattenedCommandQueue = new Dictionary(); + private void EnqueueVibration(int device, int motor, float percent) + { + var key = $"vibration-{device}-{motor}"; + //SuperController.LogMessage($"enqueue {key}"); + lock(_flattenedCommandQueue) + { + _flattenedCommandQueue[key] = new Command + { + Method = "vibration", + Device = device, + Motor = motor, + Percent = percent + }; + } + } + + public void OnInitStorables(VAMLaunch plugin) + { + _plugin = plugin; + _storableCommands = new List(); + + _storableCommands.Add(new JSONStorableFloat("Vibrate All", 0, (float x) => { EnqueueVibration(0, 0, x); }, 0, 1, constrain: true, interactable: true)); + for (var i = 0; i < 3; i++) + { + var device = i + 1; + _storableCommands.Add(new JSONStorableFloat($"Vibrate Dev {i + 1} : All", 0, (float x) => { EnqueueVibration(device, 0, x); }, 0, 1, constrain: true, interactable: true)); + _storableCommands.Add(new JSONStorableFloat($"Vibrate Dev {i + 1} : Motor 1", 0, (float x) => { EnqueueVibration(device, 1, x); }, 0, 1, constrain: true, interactable: true)); + _storableCommands.Add(new JSONStorableFloat($"Vibrate Dev {i + 1} : Motor 2", 0, (float x) => { EnqueueVibration(device, 2, x); }, 0, 1, constrain: true, interactable: true)); + } + + foreach(var s in _storableCommands) + { + plugin.RegisterFloat(s); + } + } + + public void OnInit(VAMLaunch plugin) + { + lock(_flattenedCommandQueue) + { + _flattenedCommandQueue = new Dictionary(); + } + + _uiCommands = new List(); + foreach(var s in _storableCommands) + { + _uiCommands.Add(plugin.CreateSlider(s, rightSide: true)); + } + + } + + public void OnDestroy(VAMLaunch plugin) + { + foreach(var ui in _uiCommands) + { + plugin.RemoveSlider(ui); + } + _uiCommands = new List(); + + foreach(var s in _storableCommands) + { + plugin.RemoveSlider(s); + } + _storableCommands = new List(); + + _plugin = null; + } + + + + public void OnSimulatorUpdate(float prevPos, float newPos, float deltaTime) + { + } + + public bool OnUpdate(ref byte outPos, ref byte outSpeed) + { + if(_plugin == null) + { + return false; // always return false since this class handles sending the commands + } + + var commands = new List(); + lock(_flattenedCommandQueue) + { + commands = _flattenedCommandQueue.Values.ToList(); + _flattenedCommandQueue = new Dictionary(); + } + + foreach(var cmd in commands) + { + if(cmd.Method.Equals("vibration")) + { + //SuperController.LogMessage($"Vibrate {cmd.Device} {cmd.Motor} {cmd.Percent}"); + _plugin.SetVibration(cmd.Device, cmd.Motor, cmd.Percent); + } + } + + return false; // always return false since this class handles sending the commands + } + } +} diff --git a/VAMLaunch/src/VAMLaunch.cs b/VAMLaunch/src/VAMLaunch.cs index 3a7f8f6..7aa98d8 100644 --- a/VAMLaunch/src/VAMLaunch.cs +++ b/VAMLaunch/src/VAMLaunch.cs @@ -34,14 +34,16 @@ public class VAMLaunch : MVRScript { "Oscillate", "Pattern", - "Zone" + "Zone", + "Manual" }; private List _motionSources = new List { new OscillateSource(), new PatternSource(), - new ZoneSource() + new ZoneSource(), + new ManualSource() }; public override void Init() @@ -234,8 +236,23 @@ private void ReceiveNetworkMessages() } } - - private static byte[] _launchData = new byte[6]; + public void SetVibration(int device, int motor, float percent) + { + if(_network == null) + { + return; + } + + if(_pauseLaunchMessages.val) + { + return; + } + + percent = Mathf.Clamp01(percent); + + _network.SendVibrateCmd(device, motor, percent * 100); + } + private void SendLaunchPosition(byte pos, byte speed) { SetSimulatorTarget(pos, speed); @@ -247,21 +264,19 @@ private void SendLaunchPosition(byte pos, byte speed) if (!_pauseLaunchMessages.val) { - _launchData[0] = pos; - _launchData[1] = speed; - float dist = Mathf.Abs(pos - _lastSentLaunchPos); float duration = LaunchUtils.PredictMoveDuration(dist, speed); - - var durationData = BitConverter.GetBytes(duration); - _launchData[2] = durationData[0]; - _launchData[3] = durationData[1]; - _launchData[4] = durationData[2]; - _launchData[5] = durationData[3]; - - //SuperController.LogMessage(string.Format("Sending: P:{0}, S:{1}, D:{2}", pos, speed, duration)); - - _network.Send(_launchData, _launchData.Length); + + _network.SendLinearCmd(duration, pos); + + if(speed <= 20) + { + _network.SendVibrateCmd(0); + } + else + { + _network.SendVibrateCmd((float)(pos * (speed / 100.0))); + } _lastSentLaunchPos = pos; } diff --git a/VAMLaunch/src/VAMLaunchNetwork.cs b/VAMLaunch/src/VAMLaunchNetwork.cs index 53ab509..81361d3 100644 --- a/VAMLaunch/src/VAMLaunchNetwork.cs +++ b/VAMLaunch/src/VAMLaunchNetwork.cs @@ -113,5 +113,85 @@ public byte[] GetNextMessage() { return _recvQueue.Count > 0 ? _recvQueue.Dequeue() : null; } + + private const byte LINEAR_CMD = 0; + private const byte VIBRATE_CMD = 1; + private const byte ROTATE_CMD = 2; + private const byte DEVICE_ALL = 0; + private const byte MOTOR_ALL = 0; + + public void SendLinearCmd(int device, int motor, float duration, float position) + { + byte[] data = new byte[4 + 4 + 4]; + data[0] = (byte)data.Length; + data[1] = LINEAR_CMD; + data[2] = (byte)device; + data[3] = (byte)motor; + + var durationData = BitConverter.GetBytes(duration); + data[4] = durationData[0]; + data[5] = durationData[1]; + data[6] = durationData[2]; + data[7] = durationData[3]; + + var positionData = BitConverter.GetBytes(position); + data[8] = positionData[0]; + data[9] = positionData[1]; + data[10] = positionData[2]; + data[11] = positionData[3]; + + Send(data, data.Length); + } + + public void SendLinearCmd(float duration, float position) + { + SendLinearCmd(DEVICE_ALL, MOTOR_ALL, duration, position); + } + + public void SendVibrateCmd(int device, int motor, float speed) + { + byte[] data = new byte[4 + 4]; + data[0] = (byte)data.Length; + data[1] = VIBRATE_CMD; + data[2] = (byte)device; + data[3] = (byte)motor; + + var speedData = BitConverter.GetBytes(speed); + data[4] = speedData[0]; + data[5] = speedData[1]; + data[6] = speedData[2]; + data[7] = speedData[3]; + + Send(data, data.Length); + } + + public void SendVibrateCmd(float speed) + { + SendVibrateCmd(DEVICE_ALL, MOTOR_ALL, speed); + } + + public void SendRotateCmd(int device, int motor, float speed, bool clockwise) + { + byte[] data = new byte[4 + 4 + 1]; + data[0] = (byte)data.Length; + data[1] = ROTATE_CMD; + data[2] = (byte)device; + data[3] = (byte)motor; + + var speedData = BitConverter.GetBytes(speed); + data[4] = speedData[0]; + data[5] = speedData[1]; + data[6] = speedData[2]; + data[7] = speedData[3]; + + data[8] = clockwise ? (byte)1 : (byte)0; + + Send(data, data.Length); + } + + public void SendRotateCmd(float speed, bool clockwise) + { + SendRotateCmd(DEVICE_ALL, MOTOR_ALL, speed, clockwise); + } } } \ No newline at end of file diff --git a/VaMLaunchGUI/IntifaceControl.xaml.cs b/VaMLaunchGUI/IntifaceControl.xaml.cs index 2874440..53e0938 100644 --- a/VaMLaunchGUI/IntifaceControl.xaml.cs +++ b/VaMLaunchGUI/IntifaceControl.xaml.cs @@ -2,11 +2,13 @@ using System.Collections.Generic; using System.Collections.ObjectModel; using System.Diagnostics; +using System.Linq; using System.Net; using System.Threading.Tasks; using System.Windows; using System.Windows.Controls; using Buttplug; +using VAMLaunch; namespace VaMLaunchGUI { @@ -273,32 +275,97 @@ public void OnDeviceRemoved(object aObj, DeviceRemovedEventArgs aArgs) }); } - public async Task Vibrate(double aSpeed) + public async Task Vibrate(int device, int motor, double aSpeed) { - foreach (var deviceItem in DevicesList) + var devices = DevicesList; + //Console.WriteLine("---------"); + //Console.WriteLine($"Vibrate {device} {motor} {aSpeed}"); + if (device != Command.DEVICE_ALL) { + var singleDevice = DevicesList.FirstOrDefault(d => d.Device.Index == device - 1); + if(singleDevice != null) + { + //Console.WriteLine($"found device {device}"); + devices = new ObservableCollection { singleDevice }; + } + else + { + //Console.WriteLine($"no found device {device}"); + devices = new ObservableCollection(); + } + } + + foreach (var deviceItem in devices) { - if (deviceItem.IsChecked && deviceItem.Device.AllowedMessages.ContainsKey(ServerMessage.Types.MessageAttributeType.VibrateCmd)) + if(!deviceItem.IsChecked) { - await deviceItem.Device.SendVibrateCmd(aSpeed); + continue; + } + + if(deviceItem.Device.AllowedMessages.ContainsKey(ServerMessage.Types.MessageAttributeType.VibrateCmd)) + { + if(motor == Command.MOTOR_ALL) + { + await deviceItem.Device.SendVibrateCmd(aSpeed); + } + else + { + await deviceItem.Device.SendVibrateCmd(new Dictionary() + { + { (uint)motor-1, aSpeed } + }); + } + } } } - public async Task Linear(uint aDuration, double aPosition) + public async Task Linear(int device, int motor, uint aDuration, double aPosition) { - foreach (var deviceItem in DevicesList) + var devices = DevicesList; + //Console.WriteLine("---------"); + //Console.WriteLine($"Linear {device} {motor} {aDuration} {aPosition}"); + if (device != Command.DEVICE_ALL) { + var singleDevice = DevicesList.FirstOrDefault(d => d.Device.Index == device - 1); + if(singleDevice != null) + { + //Console.WriteLine($"found device {device}"); + devices = new ObservableCollection { singleDevice }; + } + else + { + //Console.WriteLine($"no found device {device}"); + devices = new ObservableCollection(); + } + } + + foreach (var deviceItem in devices) { - // If the duration is 0, just drop the message. - if (deviceItem.IsChecked && deviceItem.Device.AllowedMessages.ContainsKey(ServerMessage.Types.MessageAttributeType.LinearCmd) && aDuration > 0) + if(!deviceItem.IsChecked) { - await deviceItem.Device.SendLinearCmd(aDuration, aPosition); + continue; + } + + if(deviceItem.Device.AllowedMessages.ContainsKey(ServerMessage.Types.MessageAttributeType.LinearCmd) && aDuration > 0) + { + if(motor == Command.MOTOR_ALL) + { + await deviceItem.Device.SendLinearCmd(aDuration, aPosition); + } + else + { + await deviceItem.Device.SendLinearCmd(new Dictionary() + { + { (uint)motor-1, (aDuration, aPosition) } + }); + } + } } } public async Task StopVibration() { - await Vibrate(0); + await Vibrate(Command.DEVICE_ALL, Command.MOTOR_ALL, 0); } private void DisposeClient() diff --git a/VaMLaunchGUI/MainWindow.xaml.cs b/VaMLaunchGUI/MainWindow.xaml.cs index 7764395..4e1f3b0 100644 --- a/VaMLaunchGUI/MainWindow.xaml.cs +++ b/VaMLaunchGUI/MainWindow.xaml.cs @@ -44,28 +44,30 @@ public MainWindow() _serverTask = new Task (() => server.UpdateThread()); _serverTask.Start(); // This is an event handler that will be executed on an outside thread, so remember to use dispatcher. - server.PositionUpdate += OnPositionUpdate; + server.CommandUpdate += OnCommandEvent; } - protected void OnPositionUpdate(object aObj, PositionUpdateEventArgs e) + protected void OnCommandEvent(object aObj, CommandEventArgs e) { Dispatcher.Invoke(async () => { - if (!_positionReceived) - { - ConnectionStatus.Content = "Connected to VaM"; - } + if (!_positionReceived) + { + ConnectionStatus.Content = "Connected to VaM"; + } - if (e.Speed <= 20) - { - await _intifaceTab.StopVibration(); - } - else - { - await _intifaceTab.Vibrate((double)(e.Position / 100.0 * (e.Speed / 100.0))); + switch (e.Command.Type) + { + case Command.LINEAR_CMD: + await _intifaceTab.Linear(e.Command.Device, e.Command.Motor, (uint)(e.Command.Params[0] * 1000), e.Command.Params[1] / 100.0); + break; + case Command.VIBRATE_CMD: + await _intifaceTab.Vibrate(e.Command.Device, e.Command.Motor, e.Command.Params[0] / 100.0); + break; + case Command.ROTATE_CMD: + // TODO: implement + break; } - - await _intifaceTab.Linear((uint)(e.Duration * 1000), (double)(e.Position / 100.0)); }); } } diff --git a/VaMLaunchGUI/VaMLaunchGUI.csproj b/VaMLaunchGUI/VaMLaunchGUI.csproj index 874b065..535a7e2 100644 --- a/VaMLaunchGUI/VaMLaunchGUI.csproj +++ b/VaMLaunchGUI/VaMLaunchGUI.csproj @@ -39,8 +39,8 @@ false - - ..\packages\Buttplug.2.0.2\lib\net47\Buttplug.dll + + ..\packages\Buttplug.2.0.3\lib\net47\Buttplug.dll ..\packages\Google.Protobuf.3.17.0\lib\net45\Google.Protobuf.dll diff --git a/VaMLaunchGUI/packages.config b/VaMLaunchGUI/packages.config index 6224a00..1fcabf4 100644 --- a/VaMLaunchGUI/packages.config +++ b/VaMLaunchGUI/packages.config @@ -1,6 +1,6 @@  - +