Pinpoint
Pinpoint copied to clipboard
VirtualBrainLab
Add drive timer
private void ComputeAndSetDriveTime(float depthDriveBaseSpeed, Action callback = null)
{
// Update drive past distance and return to surface button text
_exitButtonText.text = "Return to Surface (" + SpeedToString(_exitDriveBaseSpeed) + ")";
// Compute drive distance and duration
CommunicationManager.Instance.GetPos(_manipulatorId, position =>
{
// Remember dura depth
// _duraDepth = position.w;
// Calibrate target insertion depth based on surface position
var targetInsertion = new ProbeInsertion(
InsertionSelectionPanelHandler.ManipulatorIDToSelectedTargetProbeManager[
_manipulatorId].ProbeController.Insertion);
var targetPositionWorldT = targetInsertion.PositionWorldT();
var relativePositionWorldT =
ProbeManager.ProbeController.Insertion.PositionWorldT() - targetPositionWorldT;
var probeTipTUp = ProbeManager.ProbeController.ProbeTipT.up;
var offsetAdjustedRelativeTargetPositionWorldT =
Vector3.ProjectOnPlane(relativePositionWorldT, probeTipTUp);
var offsetAdjustedTargetPositionWorldT =
targetPositionWorldT + offsetAdjustedRelativeTargetPositionWorldT;
// Converting worldT back to APMLDV (position transformed)
targetInsertion.apmldv =
targetInsertion.CoordinateTransform.Space2TransformAxisChange(
targetInsertion.CoordinateSpace.World2Space(offsetAdjustedTargetPositionWorldT));
// Compute return exit position (500 dv above surface)
var exitInsertion = new ProbeInsertion(0, 0, 0.5f, 0, 0, 0, targetInsertion.CoordinateSpace,
targetInsertion.CoordinateTransform, false);
var exitPositionWorldT = exitInsertion.PositionWorldT();
var exitPlane = new Plane(Vector3.down, exitPositionWorldT);
var direction = new Ray(ProbeManager.ProbeController.Insertion.PositionWorldT(), probeTipTUp);
var offsetAdjustedSurfacePositionWorldT = Vector3.zero;
if (exitPlane.Raycast(direction, out var distanceToSurface))
offsetAdjustedSurfacePositionWorldT = direction.GetPoint(distanceToSurface);
// Converting worldT back to APMLDV (position transformed)
var offsetAdjustedSurfacePosition =
exitInsertion.CoordinateTransform.Space2TransformAxisChange(
exitInsertion.CoordinateSpace.World2Space(offsetAdjustedSurfacePositionWorldT));
// Compute drive distances
var targetDriveDistance =
Vector3.Distance(targetInsertion.apmldv, ProbeManager.ProbeController.Insertion.apmldv);
var exitDriveDistance = Vector3.Distance(offsetAdjustedSurfacePosition,
ProbeManager.ProbeController.Insertion.apmldv);
// Set target and exit depths
// _targetDepth = _duraDepth + targetDriveDistance;
_exitDepth = _duraDepth - exitDriveDistance;
// Warn if target depth is out of bounds
if (!_acknowledgeOutOfBounds &&
(_targetDepth > ProbeManager.ManipulatorBehaviorController.Dimensions.z || _targetDepth < 0))
{
QuestionDialogue.Instance.NewQuestion(
"Target depth is out of bounds. Are you sure you want to continue?");
QuestionDialogue.Instance.YesCallback = () => _acknowledgeOutOfBounds = true;
}
// Set drive speeds (base + x mm/s / 1 mm of depth)
// _targetDriveSpeed = _driveBaseSpeed + targetDriveDistance * _per1000Speed;
/*
* Compute target drive duration
* 1. Drive down towards target until at near target distance at target drive speed
* 2. Drive past target by drive past distance at near target speed
* 3. Drive back to target at near target speed
* 4. Settle for 1 minute per 1 mm of target drive distance with a minimum of 2 minutes
*/
_targetDriveDuration =
Mathf.Max(0, targetDriveDistance - NEAR_TARGET_DISTANCE) / _targetDriveSpeed +
(Mathf.Min(NEAR_TARGET_DISTANCE, targetDriveDistance) + 2 * _drivePastTargetDistance) /
(_targetDriveSpeed * NEAR_TARGET_SPEED_MULTIPLIER) +
Mathf.Max(120, 60 * (targetDriveDistance + _drivePastTargetDistance));
/*
* Compute exit drive duration
* 1. Drive out by near target distance at near target speed
* 2. Drive out to dura at exit speed
* 3. Drive out by dura margin distance at exit speed
* 4. Drive out to surface at outside speed
*/
_exitDriveDuration =
Mathf.Max(0, NEAR_TARGET_DISTANCE - targetDriveDistance) /
(_exitDriveBaseSpeed * NEAR_TARGET_SPEED_MULTIPLIER) +
Mathf.Max(0, targetDriveDistance - NEAR_TARGET_DISTANCE) / _exitDriveBaseSpeed +
_duraMarginDriveDuration +
(exitDriveDistance - targetDriveDistance - DURA_MARGIN_DISTANCE) /
_outsideDriveSpeed;
// Set timer text
_timerText.text = TimeSpan.FromSeconds(_targetDriveDuration).ToString(@"mm\:ss");
// Run callback (if any)
callback?.Invoke();
});
}
private IEnumerator CountDownTimer(float seconds, DriveState driveState)
{
// Set timer text
_timerText.text = TimeSpan.FromSeconds(seconds).ToString(@"mm\:ss");
// Wait for 1 second
yield return new WaitForSeconds(1);
switch (seconds)
{
// Check if timer is done
case > 0 when
_driveState == driveState:
StartCoroutine(CountDownTimer(seconds - 1, driveState));
break;
case <= 0:
{
// Set status to complete
_timerText.text = "";
if (_driveState == DriveState.DrivingToTarget)
{
// Completed driving to target (finished settling)
CompleteDrive();
}
else
{
// Completed returning to surface
_statusText.text = "Ready to Drive";
_stopButton.SetActive(false);
_driveGroup.SetActive(true);
_driveState = DriveState.Ready;
}
break;
}
default:
_timerText.text = "";
break;
}
}
I'm going through issues and making sure the final to-dos for SfN are all doable and I saw the code you pasted here. Two quick thoughts:
- This code could hit the recursion limit in Javascript when we get copilot working there (the limit is large but not that large). Probably better to implement it as a while loop inside the coroutine, or use Update()
- I'm not 100% sure but I think Coroutines are evaluated during the frame logic, where the only accurate estimate of time elapsed is Time.deltaTime (since last frame) or Time.realTimeSinceStartup. So I think if you're running at a low framerate you'll accumulate up to a frame's worth of delay each second with the code you've written here. In case it's not clear why: the coroutine will evaluate at an unknown time within each frame, then wait for the next frame when >= 1 s has passed and repeat. We could test this by setting up a coroutine with repeated 1s waits and seeing if the realTimeSinceStartup occurs at 1s intervals... I'll do that today
Can confirm, you can't use recursive coroutines like this:
private void Start()
{
StartCoroutine(Test());
}
private IEnumerator Test()
{
yield return new WaitForSeconds(5f);
float start = Time.realtimeSinceStartup;
for (int i = 0; i < 10; i++)
{
yield return new WaitForSeconds(1f);
Debug.Log($"Accumulated delay: {Time.realtimeSinceStartup - start - (i+1)*1f}");
}
}
Oh! Good to know. I pasted this code because I had deleted it in my last push. I was considering using the Update function just for simplicity and since it'll be a bit more reactive (since I'd calculate how much time there was left on each update cycle and display that). The computation logic was really what I was trying to save here.
We can chat about this in our next meeting, would be good to set it up in a way that is easy to maintain to maintain in the future!