Added control library call in IK mode + camera focus on discover

Assets/IK/InverseKinematicsController.cs

@@ -198,7 +198,7 @@ public class InverseKinematicsController : MonoBehaviour
                 }
 
                 joint.currentAngle = desiredAngle;
-                joint.servo.SetAngle(desiredAngle);
+                joint.servo.SetAngleAndSendControlLibrary(desiredAngle, 1f);
                 
                 endEffectorPos = endEffector.position;
                 

Assets/Module/DiscoveryButton.cs

@@ -3,6 +3,8 @@ using UnityEngine;
 public class DiscoveryButton : MonoBehaviour
 {
     public TopologyBuilder topologyBuilder;
+    [Tooltip("If set, camera focuses on GeneratedTopology after Discover. If null, will FindObjectOfType.")]
+    public UserCameraControl cameraController;
 
     void Start()
     {
@@ -21,6 +23,7 @@ public class DiscoveryButton : MonoBehaviour
         {
             Debug.Log("Discovery button pressed.");
             topologyBuilder.BuildTopologyFromJson();
+            FocusCameraOnTopology();
         }
         else
         {
@@ -28,6 +31,13 @@ public class DiscoveryButton : MonoBehaviour
         }
     }
 
+    void FocusCameraOnTopology()
+    {
+        UserCameraControl cam = cameraController != null ? cameraController : FindObjectOfType<UserCameraControl>();
+        if (cam != null)
+            cam.FindModuleStructure();
+    }
+
     void OnDestroy()
     {
         Debug.Log("Cleaning up native resources");

Assets/Module/TopologyBuilder.cs

@@ -4,59 +4,122 @@ using System;
 using System.Linq;
 using Frontend;
 
-
+[System.Serializable]
+public class JsonTopologyGraph { public List<JsonModuleData> Modules; public List<JsonConnection> Connections; }
+[System.Serializable]
+public class JsonModuleData { public string Id; public string Type; public float Degree; }
+[System.Serializable]
+public class JsonConnection { public string FromModuleId; public string ToModuleId; public string FromSocket; public string ToSocket; public int Orientation; }
 
 public class TopologyBuilder : MonoBehaviour
 {
     public ModuleSpawner spawner;
 
+    [Header("Topology Selection")]
+    [Tooltip("Set to true to load from JSON file (for local testing). Set to false to use ControlLibrary.")]
+    public bool useJsonFile = false;
+    [Tooltip("When true, angle commands are not sent to native ControlLibrary - use only for local testing without hardware.")]
+    public bool skipControlLibraryCalls = false;
+    [Tooltip("JSON file name (without .json) in Resources folder. e.g. 'mockData' or 'mockDataSimple'")]
+    public string jsonFileName = "mockData";
+
     public static Dictionary<string, GameObject> idToInstance = new();
+    public static bool _skipControlLibraryCalls = false;
+
+    public static bool SkipControlLibraryCalls => _skipControlLibraryCalls;
 
     public void BuildTopologyFromJson()
     {
-
-        // todo: this is some really bad temporary code
         TopologyGraph graph = new TopologyGraph();
-        RobotConfiguration config = ControlLibrary.getRobotConfiguration();
-
         Dictionary<int, ModuleType> idToType = new();
-        int moduleCount = config.ModulesLength;
-        for (int i = 0; i < moduleCount; i++)
+
+        if (useJsonFile)
         {
-            var n_module = config.Modules(i);
-            if (n_module != null)
+            TextAsset jsonAsset = Resources.Load<TextAsset>(jsonFileName);
+            if (jsonAsset == null) { Debug.LogError($"JSON not found: Resources/{jsonFileName}.json"); return; }
+            JsonTopologyGraph jsonGraph = JsonUtility.FromJson<JsonTopologyGraph>(jsonAsset.text);
+            if (jsonGraph?.Modules == null || jsonGraph.Modules.Count == 0) { Debug.LogError("JSON has no modules"); return; }
+
+            Dictionary<string, int> stringToIntId = new Dictionary<string, int>();
+            int nextIntId = 1;
+            foreach (var m in jsonGraph.Modules)
             {
-                var module = n_module.Value;
-                Debug.Log("Adding module " + module.Id);
-                graph.Modules.Add(new ModuleData
-                {
-                    Id = module.Id,
-                    Type = module.ModuleType.ToString(),
-                    Degree = module.ConfigurationAsMotorState().Angle
-                });
-                idToType.Add(module.Id, module.ModuleType);
+                if (!stringToIntId.ContainsKey(m.Id)) stringToIntId[m.Id] = nextIntId++;
+                int intId = stringToIntId[m.Id];
+                ModuleType mt = ParseModuleType(m.Type);
+                idToType[intId] = mt;
+                graph.Modules.Add(new ModuleData { Id = intId, Type = mt.ToString(), Degree = m.Degree });
             }
+            if (jsonGraph.Connections != null)
+                foreach (var c in jsonGraph.Connections)
+                {
+                    if (!stringToIntId.TryGetValue(c.FromModuleId, out int fromId) || !stringToIntId.TryGetValue(c.ToModuleId, out int toId)) continue;
+                    Orientation o = c.Orientation == 90 ? Orientation.Deg90 : c.Orientation == 180 ? Orientation.Deg180 : c.Orientation == 270 ? Orientation.Deg270 : Orientation.Deg0;
+                    graph.Connections.Add(new Connection { FromModuleId = fromId, ToModuleId = toId, FromSocket = c.FromSocket ?? "MaleSocket", ToSocket = c.ToSocket ?? "FemaleSocket", Orientation = o });
+                }
+            Debug.Log($"[TopologyBuilder] Built from JSON: {jsonFileName}");
+            _skipControlLibraryCalls = skipControlLibraryCalls;
+        }
+        else
+        {
+            RobotConfiguration config = ControlLibrary.getRobotConfiguration();
+            int moduleCount = config.ModulesLength;
+            for (int i = 0; i < moduleCount; i++)
+            {
+                var n_module = config.Modules(i);
+                if (n_module != null)
+                {
+                    var module = n_module.Value;
+                    Debug.Log("Adding module " + module.Id);
+                    graph.Modules.Add(new ModuleData
+                    {
+                        Id = module.Id,
+                        Type = module.ModuleType.ToString(),
+                        Degree = module.ConfigurationAsMotorState().Angle
+                    });
+                    idToType.Add(module.Id, module.ModuleType);
+                }
+            }
+            int connectionCount = config.ConnectionsLength;
+            for (int i = 0; i < connectionCount; i++)
+            {
+                var n_connection = config.Connections(i);
+                if (n_connection != null)
+                {
+                    var connection = n_connection.Value;
+                    Debug.Log("orientation: " + connection.Orientation);
+                    graph.Connections.Add(new Connection
+                    {
+                        FromModuleId = connection.FromModuleId,
+                        FromSocket = idToType[connection.FromModuleId] == ModuleType.SPLITTER ? "MaleSocket" + (connection.FromSocket == 0 ? "" : connection.FromSocket) : "MaleSocket",
+                        ToModuleId = connection.ToModuleId,
+                        ToSocket = "FemaleSocket",
+                        Orientation = connection.Orientation
+                    });
+                }
+            }
+            _skipControlLibraryCalls = false;
         }
 
-        int connectionCount = config.ConnectionsLength;
-        for (int i = 0; i < connectionCount; i++)
-        {
-            var n_connection = config.Connections(i);
-            if (n_connection != null)
-            {
-                var connection = n_connection.Value;
-                Debug.Log("orientation: " + connection.Orientation);
-                graph.Connections.Add(new Connection
-                {
-                    FromModuleId = connection.FromModuleId,
-                    FromSocket = idToType[connection.FromModuleId] == ModuleType.SPLITTER ? "MaleSocket" + (connection.FromSocket == 0 ? "" : connection.FromSocket) : "MaleSocket",
-                    ToModuleId = connection.ToModuleId,
-                    ToSocket = "FemaleSocket",
-                    Orientation = connection.Orientation
-                });
-            }
-        }
+        BuildTopologyFromGraph(graph);
+    }
 
+    private static ModuleType ParseModuleType(string typeString)
+    {
+        if (string.IsNullOrEmpty(typeString)) return ModuleType.SPLITTER;
+        switch (typeString)
+        {
+            case "Servo1": return ModuleType.SERVO_1;
+            case "Servo2": return ModuleType.SERVO_2;
+            case "DC": return ModuleType.DC_MOTOR;
+            case "Hub": return ModuleType.SPLITTER;
+            case "Battery": return ModuleType.BATTERY;
+            default: return ModuleType.SPLITTER;
+        }
+    }
+
+    private void BuildTopologyFromGraph(TopologyGraph graph)
+    {
         // Destroy previous topology root if it exists
         GameObject oldRoot = GameObject.Find("GeneratedTopology");
         if (oldRoot != null)
@@ -101,10 +164,11 @@ public class TopologyBuilder : MonoBehaviour
             if (baseScript != null)
             {
                 baseScript.moduleID = module.Id.ToString();
-                if (module.Type.Contains("Servo") && module.Degree != null)
+                ModuleType parsedType = Enum.Parse<ModuleType>(module.Type);
+                if ((parsedType == ModuleType.SERVO_1 || parsedType == ModuleType.SERVO_2))
                 {
                     ServoMotorModule servo;
-                    if (module.Type == "Servo1")
+                    if (parsedType == ModuleType.SERVO_1)
                     {
                         servo = instance.GetComponent<ServoBendModule>();
                     }
@@ -112,8 +176,6 @@ public class TopologyBuilder : MonoBehaviour
                     {
                         servo = instance.GetComponent<ServoStraightModule>();
                     }
-                    //ServoBendModule servo = instance.GetComponent<ServoBendModule>();
-                    // servo.SetInitialAngle(module.Degree);
                     servo.InitialSetAngle(module.Degree);
                 }
             }

Assets/ModuleBase.cs

@@ -37,6 +37,10 @@ public abstract class ModuleBase : MonoBehaviour
             });
         }
         Debug.Log($"[ControlLibrary] Sending command: {json}");
+        if (TopologyBuilder.SkipControlLibraryCalls)
+        {
+            return;
+        }
         if (0 != ControlLibrary.send_angle_control(Int32.Parse(moduleID), (int)currentAngle))
         {
             Debug.Log("Control library exited with error");

Assets/Resources/mockData.json

@@ -1,80 +1 @@
-{
-    "Modules": [
-      { "Id": "servo1", "Type": "Servo1", "Degree": 34 },
-      { "Id": "servo2", "Type": "Servo1", "Degree": 130 },
-      { "Id": "dc1", "Type": "DC", "Degree": null },
-      { "Id": "dc2", "Type": "DC", "Degree": null },
-      { "Id": "hub1", "Type": "Hub", "Degree": null },
-      { "Id": "hub2", "Type": "Hub", "Degree": null },
-      { "Id": "battery1", "Type": "Battery", "Degree": null },
-      { "Id": "battery2", "Type": "Battery", "Degree": null },
-      { "Id": "servo3", "Type": "Servo2", "Degree": 32 },
-      { "Id": "battery3", "Type": "Battery", "Degree": null }
-    ],
-    "Connections": [
-      {
-        "FromModuleId": "servo1",
-        "ToModuleId": "servo2",
-        "FromSocket": "MaleSocket",
-        "ToSocket": "FemaleSocket",
-        "Orientation": 90
-      },
-      {
-        "FromModuleId": "servo2",
-        "ToModuleId": "dc1",
-        "FromSocket": "MaleSocket",
-        "ToSocket": "FemaleSocket",
-        "Orientation": 180
-      },
-      {
-        "FromModuleId": "hub1",
-        "ToModuleId": "battery1",
-        "FromSocket": "MaleSocket2",
-        "ToSocket": "FemaleSocket",
-        "Orientation": 0
-      },
-      {
-        "FromModuleId": "hub1",
-        "ToModuleId": "battery2",
-        "FromSocket": "MaleSocket3",
-        "ToSocket": "FemaleSocket",
-        "Orientation": 90
-      },
-      {
-        "FromModuleId": "hub1",
-        "ToModuleId": "dc2",
-        "FromSocket": "MaleSocket1",
-        "ToSocket": "FemaleSocket",
-        "Orientation": 0
-      },
-      {
-        "FromModuleId": "servo1",
-        "ToModuleId": "battery1",
-        "FromSocket": "BodySocket",
-        "ToSocket": "MaleSocket",
-        "Orientation": 0
-      },
-      {
-        "FromModuleId": "servo3",
-        "ToModuleId": "battery3",
-        "FromSocket": "MaleSocket",
-        "ToSocket": "FemaleSocket",
-        "Orientation": 90
-      },
-      {
-        "FromModuleId": "hub1",
-        "ToModuleId": "battery3",
-        "FromSocket": "FemaleSocket",
-        "ToSocket": "MaleSocket",
-        "Orientation": 0
-      },
-      {
-        "FromModuleId": "servo3",
-        "ToModuleId": "hub2",
-        "FromSocket": "FemaleSocket",
-        "ToSocket": "MaleSocket1",
-        "Orientation": 90
-      }
-    ]
-  }
-  
+{"Modules":[{"Id":"battery1","Type":"Battery","Degree":null},{"Id":"servo1","Type":"Servo1","Degree":45},{"Id":"servo2","Type":"Servo2","Degree":60},{"Id":"servo3","Type":"Servo1","Degree":90},{"Id":"servo4","Type":"Servo2","Degree":45},{"Id":"servo5","Type":"Servo1","Degree":120},{"Id":"servo6","Type":"Servo2","Degree":30},{"Id":"endModule","Type":"DC","Degree":null}],"Connections":[{"FromModuleId":"battery1","ToModuleId":"servo1","FromSocket":"MaleSocket","ToSocket":"BodySocket","Orientation":0},{"FromModuleId":"servo1","ToModuleId":"servo2","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo2","ToModuleId":"servo3","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0},{"FromModuleId":"servo3","ToModuleId":"servo4","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo4","ToModuleId":"servo5","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0},{"FromModuleId":"servo5","ToModuleId":"servo6","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo6","ToModuleId":"endModule","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0}]}

Assets/Resources/mockDataSimple.json

@@ -0,0 +1 @@
+{"Modules":[{"Id":"battery1","Type":"Battery","Degree":null},{"Id":"servo1","Type":"Servo1","Degree":45},{"Id":"servo2","Type":"Servo2","Degree":60},{"Id":"servo3","Type":"Servo1","Degree":90},{"Id":"servo4","Type":"Servo2","Degree":45},{"Id":"endModule","Type":"DC","Degree":null}],"Connections":[{"FromModuleId":"battery1","ToModuleId":"servo1","FromSocket":"MaleSocket","ToSocket":"BodySocket","Orientation":0},{"FromModuleId":"servo1","ToModuleId":"servo2","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo2","ToModuleId":"servo3","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0},{"FromModuleId":"servo3","ToModuleId":"servo4","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo4","ToModuleId":"endModule","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0}]}

Assets/Resources/mockDataSimple.json.meta

@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: 415eb2ad073c149198677d12765af959
+TextScriptImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Scenes/SampleScene.unity

@@ -942,6 +942,9 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   spawner: {fileID: 2021810911}
+  useJsonFile: 0
+  skipControlLibraryCalls: 0
+  jsonFileName: mockData
 --- !u!4 &387671720
 Transform:
   m_ObjectHideFlags: 0

Assets/ServoMotorModule.cs

@@ -25,10 +25,10 @@ public abstract class ServoMotorModule : ModuleBase
 
     public abstract void InitialSetAngle(float angle);
 
-    public void SetAngleAndSendControlLibrary(float angle)
+    public void SetAngleAndSendControlLibrary(float angle, float minChangeDegrees = 0.1f)
     {
         SetAngle(angle);
-        if (Mathf.Abs(currentAngle - lastSentAngle) > 0.1f)
+        if (Mathf.Abs(currentAngle - lastSentAngle) > minChangeDegrees)
         {
             SendToControlLibrary(servoType, currentAngle);
             lastSentAngle = currentAngle;