diff --git a/app/src/main/java/com/example/lzwcai_terminal_temi/MainActivity.kt b/app/src/main/java/com/example/lzwcai_terminal_temi/MainActivity.kt index 86a9f36..0208ac0 100644 --- a/app/src/main/java/com/example/lzwcai_terminal_temi/MainActivity.kt +++ b/app/src/main/java/com/example/lzwcai_terminal_temi/MainActivity.kt @@ -97,6 +97,8 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG private var blinkJob: Job? = null private var networkErrorJob: Job? = null private var autoRechargeJob: Job? = null + private var latestYaw: Float? = null + private var receptionAnchorYaw: Float? = null private lateinit var telemetryManager: TelemetryManager private lateinit var taskController: TaskController @@ -322,6 +324,7 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG } override fun onCurrentPositionChanged(position: Position) { + latestYaw = position.yaw telemetryManager.onCurrentPositionChanged(position) } @@ -355,6 +358,14 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG if (taskController.currentTask == "patrol") { taskController.handlePatrolArrival(location) } + if (taskController.currentTask == "reception" && + location.equals(taskController.getReceptionLocation(), ignoreCase = true) + ) { + captureReceptionAnchorYawIfNeeded() + } + if (taskController.currentTask != "reception") { + receptionAnchorYaw = null + } if (taskController.handleNotificationArrival(location)) { return } @@ -406,6 +417,13 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG private fun handleStableDetectionStateChanged(state: Int) { Log.i("MainActivity", "Stable detection state: $state") liveKitManager?.setDetectionActive(state == DETECTED) + if (taskController.currentTask == "reception") { + if (state == DETECTED) { + captureReceptionAnchorYawIfNeeded() + } else if (state == IDLE) { + recoverReceptionFacingDirection() + } + } if (taskController.handleDetectionStateChanged(state)) { Log.i("MainActivity", "what the f**k") return @@ -490,6 +508,9 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG if (task.trim().isNotEmpty()) { cancelAutoRecharge("task_set:$task") } + if (!task.equals("reception", ignoreCase = true)) { + receptionAnchorYaw = null + } taskController.setCurrentTask(task) } @@ -762,6 +783,48 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG autoRechargeJob = null } + private fun captureReceptionAnchorYawIfNeeded() { + if (receptionAnchorYaw != null) { + return + } + val yaw = latestYaw ?: return + receptionAnchorYaw = yaw + Log.i("MainActivity", "Reception anchor yaw captured: $yaw") + } + + private fun recoverReceptionFacingDirection() { + if (taskController.currentTask != "reception") { + return + } + val atReceptionLocation = lastArrivalLocation?.equals(taskController.getReceptionLocation(), ignoreCase = true) == true + if (!atReceptionLocation) { + return + } + val anchorYaw = receptionAnchorYaw ?: return + val currentYaw = latestYaw ?: return + val delta = normalizeAngle(anchorYaw - currentYaw) + // Ignore tiny drift to avoid jitter. + if (kotlin.math.abs(delta) < 8f) { + return + } + val turn = delta.toInt().coerceIn(-45, 45) + if (turn == 0) { + return + } + navCon.turnBy(turn, 0.35f) + Log.i("MainActivity", "Reception facing recovered by $turn degrees (delta=$delta).") + } + + private fun normalizeAngle(angle: Float): Float { + var normalized = angle % 360f + if (normalized > 180f) { + normalized -= 360f + } else if (normalized < -180f) { + normalized += 360f + } + return normalized + } + private fun updateConnectionIndicator() { val colorRes = when { !isLiveKitConnected && !isMqttConnected -> android.R.color.holo_red_dark