Move command and control to VaM plugin

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<PositionUpdateEventArgs> PositionUpdate;
+        public EventHandler<CommandEventArgs> 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<float> Params;
+
+        public Command()
+        {
+            Type = -1;
+            Device = DEVICE_ALL;
+            Motor = MOTOR_ALL;
+            Params = new List<float>();
+        }
+
+        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;
         }
+
     }
 }

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
+src/MotionSources/PatternSource.cs
+src/MotionSources/ManualSource.cs

VAMLaunch/VAMLaunch.csproj

@@ -92,6 +92,7 @@
     <Compile Include="Properties\AssemblyInfo.cs" />
     <Compile Include="src\LaunchUtils.cs" />
     <Compile Include="src\MotionSources\IMotionSource.cs" />
+    <Compile Include="src\MotionSources\ManualSource.cs" />
     <Compile Include="src\MotionSources\OscillateSource.cs" />
     <Compile Include="src\MotionSources\PatternSource.cs" />
     <Compile Include="src\MotionSources\ZoneSource.cs" />

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<JSONStorableFloat> _storableCommands = new List<JSONStorableFloat>();
+        List<UIDynamicSlider> _uiCommands = new List<UIDynamicSlider>();
+        VAMLaunch _plugin;
+
+        Dictionary<string, Command> _flattenedCommandQueue = new Dictionary<string, Command>();
+        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<JSONStorableFloat>();
+
+            _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<string, Command>();
+            }
+
+            _uiCommands = new List<UIDynamicSlider>();
+            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<UIDynamicSlider>();
+
+            foreach(var s in _storableCommands)
+            {
+                plugin.RemoveSlider(s);
+            }
+            _storableCommands = new List<JSONStorableFloat>();
+
+            _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<Command>();
+            lock(_flattenedCommandQueue)
+            {
+                commands = _flattenedCommandQueue.Values.ToList();
+                _flattenedCommandQueue = new Dictionary<string, Command>();
+            }
+
+            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
+        }
+    }
+}

VAMLaunch/src/VAMLaunch.cs

@@ -34,14 +34,16 @@ public class VAMLaunch : MVRScript
         {
             "Oscillate",
             "Pattern",
-            "Zone"
+            "Zone",
+            "Manual"
         };
 
         private List<IMotionSource> _motionSources = new List<IMotionSource>
         {
             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;
             }