Skip to content

Commit 1967638

Browse files
kjy5kjy5
committed
Demo loop
1 parent ebd894e commit 1967638

File tree

1 file changed

+176
-0
lines changed

1 file changed

+176
-0
lines changed

‎Assets/Scripts/Services/EphysLinkService.cs‎

Lines changed: 176 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,12 @@ public class EphysLinkService
5050

5151
#endregion
5252

53+
#region Demo Loop
54+
55+
private readonly Dictionary<string, bool> _runningDemoLoops = new();
56+
private const float DEMO_SPEED = 100f; // µm/s
57+
#endregion
58+
5359
public EphysLinkService(StoreService storeService)
5460
{
5561
// Register services.
@@ -69,6 +75,27 @@ private async void OnSceneStateChanged(SceneState sceneState)
6975
// Apply small delay to prevent overrunning updates (delay for roughly 60 FPS).
7076
await Task.Delay(10);
7177

78+
// Handle demo loop state changes.
79+
foreach (var manipulatorState in sceneState.Manipulators)
80+
{
81+
var isRunning =
82+
_runningDemoLoops.ContainsKey(manipulatorState.Id)
83+
&& _runningDemoLoops[manipulatorState.Id];
84+
85+
switch (manipulatorState.IsDemoRunning)
86+
{
87+
// Start demo loop if requested and not already running.
88+
case true when !isRunning:
89+
_runningDemoLoops[manipulatorState.Id] = true;
90+
_ = RunDemoLoop(manipulatorState.Id);
91+
break;
92+
// Stop demo loop if requested and currently running.
93+
case false when isRunning:
94+
await Stop(manipulatorState.Id);
95+
break;
96+
}
97+
}
98+
7299
// WARNING: this will create an infinite loop of state updates on purpose.
73100
// Update the position of visualization probes.
74101
await UpdateVisualizationProbePosition(sceneState);
@@ -738,6 +765,155 @@ public async Task SetManipulatorDemoTargetCoordinateToCurrentPosition(string man
738765
);
739766
}
740767

768+
/// <summary>
769+
/// Run the demo loop for a manipulator.
770+
/// Sequence: Home -> Target (X,Z only) -> Target (Y,W only) -> Back to Target (X,Z only) -> repeat indefinitely
771+
/// until IsDemoRunning is set to false or an error occurs.
772+
/// </summary>
773+
/// <param name="manipulatorId">ID of the manipulator to run demo loop for.</param>
774+
private async Task RunDemoLoop(string manipulatorId)
775+
{
776+
while (true)
777+
{
778+
// Get current manipulator state.
779+
var sceneState = _storeService.Store.GetState<SceneState>(SliceNames.SCENE_SLICE);
780+
var manipulatorState = sceneState.Manipulators.FirstOrDefault(m =>
781+
m.Id == manipulatorId
782+
);
783+
784+
// Exit if manipulator not found.
785+
if (manipulatorState == null)
786+
{
787+
_runningDemoLoops[manipulatorId] = false;
788+
return;
789+
}
790+
791+
// Exit if demo is no longer running.
792+
if (!manipulatorState.IsDemoRunning)
793+
{
794+
_runningDemoLoops[manipulatorId] = false;
795+
return;
796+
}
797+
798+
// Step 1: Move to home position.
799+
var homeRequest = new SetPositionRequest(
800+
manipulatorId,
801+
manipulatorState.DemoHomeCoordinate,
802+
DEMO_SPEED
803+
);
804+
var homeResponse = await SetPosition(homeRequest);
805+
806+
// Exit on error (including stop request).
807+
if (HasError(homeResponse.Error))
808+
{
809+
_runningDemoLoops[manipulatorId] = false;
810+
return;
811+
}
812+
813+
// Refresh state and check if demo is still running.
814+
sceneState = _storeService.Store.GetState<SceneState>(SliceNames.SCENE_SLICE);
815+
manipulatorState = sceneState.Manipulators.FirstOrDefault(m =>
816+
m.Id == manipulatorId
817+
);
818+
if (manipulatorState is not { IsDemoRunning: true })
819+
{
820+
_runningDemoLoops[manipulatorId] = false;
821+
return;
822+
}
823+
824+
// Step 2: Move to target position on X and Z axes only (keep Y and W from home).
825+
var intermediatePosition = new Vector4(
826+
manipulatorState.DemoTargetCoordinate.x,
827+
manipulatorState.DemoHomeCoordinate.y,
828+
manipulatorState.DemoTargetCoordinate.z,
829+
manipulatorState.DemoHomeCoordinate.w
830+
);
831+
var intermediateRequest = new SetPositionRequest(
832+
manipulatorId,
833+
intermediatePosition,
834+
DEMO_SPEED
835+
);
836+
var intermediateResponse = await SetPosition(intermediateRequest);
837+
838+
// Exit on error (including stop request).
839+
if (HasError(intermediateResponse.Error))
840+
{
841+
_runningDemoLoops[manipulatorId] = false;
842+
return;
843+
}
844+
845+
// Refresh state and check if demo is still running.
846+
sceneState = _storeService.Store.GetState<SceneState>(SliceNames.SCENE_SLICE);
847+
manipulatorState = sceneState.Manipulators.FirstOrDefault(m =>
848+
m.Id == manipulatorId
849+
);
850+
if (manipulatorState is not { IsDemoRunning: true })
851+
{
852+
_runningDemoLoops[manipulatorId] = false;
853+
return;
854+
}
855+
856+
// Step 3: Move to target position on Y and W axes only (complete movement to target).
857+
var targetRequest = new SetPositionRequest(
858+
manipulatorId,
859+
manipulatorState.DemoTargetCoordinate,
860+
DEMO_SPEED
861+
);
862+
var targetResponse = await SetPosition(targetRequest);
863+
864+
// Exit on error (including stop request).
865+
if (HasError(targetResponse.Error))
866+
{
867+
_runningDemoLoops[manipulatorId] = false;
868+
return;
869+
}
870+
871+
// Refresh state and check if demo is still running.
872+
sceneState = _storeService.Store.GetState<SceneState>(SliceNames.SCENE_SLICE);
873+
manipulatorState = sceneState.Manipulators.FirstOrDefault(m =>
874+
m.Id == manipulatorId
875+
);
876+
if (manipulatorState is not { IsDemoRunning: true })
877+
{
878+
_runningDemoLoops[manipulatorId] = false;
879+
return;
880+
}
881+
882+
// Step 4: Move back to intermediate position (X,Z only, keeping Y,W from home).
883+
var returnToIntermediatePosition = new Vector4(
884+
manipulatorState.DemoTargetCoordinate.x,
885+
manipulatorState.DemoHomeCoordinate.y,
886+
manipulatorState.DemoTargetCoordinate.z,
887+
manipulatorState.DemoHomeCoordinate.w
888+
);
889+
var returnToIntermediateRequest = new SetPositionRequest(
890+
manipulatorId,
891+
returnToIntermediatePosition,
892+
DEMO_SPEED
893+
);
894+
var returnToIntermediateResponse = await SetPosition(returnToIntermediateRequest);
895+
896+
// Exit on error (including stop request).
897+
if (HasError(returnToIntermediateResponse.Error))
898+
{
899+
_runningDemoLoops[manipulatorId] = false;
900+
return;
901+
}
902+
903+
// Refresh state and check if demo is still running before next iteration.
904+
sceneState = _storeService.Store.GetState<SceneState>(SliceNames.SCENE_SLICE);
905+
manipulatorState = sceneState.Manipulators.FirstOrDefault(m =>
906+
m.Id == manipulatorId
907+
);
908+
if (manipulatorState is { IsDemoRunning: true })
909+
continue;
910+
_runningDemoLoops[manipulatorId] = false;
911+
return;
912+
913+
// Loop continues to next iteration (back to home).
914+
}
915+
}
916+
741917
#endregion
742918
}
743919
}

0 commit comments

Comments
 (0)