From 09e6a3704307940b7e3ae61eb110e629d84bba0d Mon Sep 17 00:00:00 2001 From: Paul Quinn Date: Thu, 27 Nov 2025 00:40:42 -0800 Subject: [PATCH] FM-695-orb_ui_position_feedback --- ui/src/engine/animations/mod.rs | 1 + ui/src/engine/animations/position_feedback.rs | 325 ++++++++++++++++++ ui/src/engine/diamond.rs | 135 ++++++++ ui/src/engine/mod.rs | 5 + 4 files changed, 466 insertions(+) create mode 100644 ui/src/engine/animations/position_feedback.rs diff --git a/ui/src/engine/animations/mod.rs b/ui/src/engine/animations/mod.rs index 0527e1d1d..05ea704c1 100644 --- a/ui/src/engine/animations/mod.rs +++ b/ui/src/engine/animations/mod.rs @@ -7,6 +7,7 @@ mod fake_progress; pub mod fake_progress_v2; pub mod idle; pub mod milky_way; +pub mod position_feedback; pub mod progress; pub mod progress_with_notch; mod segmented; diff --git a/ui/src/engine/animations/position_feedback.rs b/ui/src/engine/animations/position_feedback.rs new file mode 100644 index 000000000..d2de8e6e9 --- /dev/null +++ b/ui/src/engine/animations/position_feedback.rs @@ -0,0 +1,325 @@ +use crate::engine::{Animation, AnimationState, RingFrame}; +use orb_rgb::Argb; +use std::{any::Any, f64::consts::PI}; + +/// Real-time position feedback animation that lights LEDs based on user position +/// The animation creates an arc of LEDs pointing toward the user's x,y position +/// with arc length determined by distance from optimal position (closer = larger arc) +/// +/// Coordinate System (based on logged calibration data): +/// - X: ~375-379 (optimal ~377.5) - horizontal position +/// - Y: ~-10 to -18 (optimal ~-17.5) - vertical position +/// - Z: ~80-82 (optimal ~81.0) - depth/distance from camera +pub struct PositionFeedback { + /// Current user position in pixel/physical coordinates + position_x: f64, + position_y: f64, + position_z: f64, + + /// Optimal position (based on logged "good" position values) + optimal_x: f64, // ~377.5 (center of logged x values) + optimal_y: f64, // ~-17.5 (center of logged y values) + optimal_z: f64, // ~81.0 (center of logged z values) + + /// Animation parameters + color: Argb, + max_arc_angle: f64, // Maximum arc angle in radians (when at center) + min_arc_angle: f64, // Minimum arc angle in radians (when at edge) + center_threshold: f64, // Distance from center considered "centered" (in pixels) + edge_threshold: f64, // Distance from center considered "at edge" (in pixels) + _z_tolerance: f64, // Z-axis tolerance for good positioning (unused for now) + + /// Animation state + pulse_phase: f64, // Phase for pulsing effect + pulse_frequency: f64, // Pulsing frequency in Hz + target_intensity: f64, // Target LED intensity + current_intensity: f64, // Current LED intensity (smoothed) + frame_count: u32, // Frame counter for debug logging +} + +impl PositionFeedback { + pub fn new(color: Argb) -> Self { + tracing::info!("Creating position feedback animation with color: {:?}", color); + Self { + position_x: 300.0, // Start at optimal position + position_y: -20.0, + position_z: 70.0, + + // Optimal position based on logged "good" values + optimal_x: 280.0, // Center moved left (was 300.0) + optimal_y: -20.0, // Center of y range -10 to -18 + optimal_z: 70.0, // Center of z range 80-82 + + color, + max_arc_angle: PI * 0.95, // 171 degrees when perfectly centered (almost full ring) + min_arc_angle: PI * 0.08, // 14 degrees when far off (narrow guidance) + center_threshold: 10.0, // Within 10 pixels considered "centered" + edge_threshold: 40.0, // Beyond 40 pixels considered "far" + _z_tolerance: 5.0, // Within 5 units of optimal Z + pulse_phase: 0.0, + pulse_frequency: 1.5, // 1.5 Hz pulse for more responsiveness + target_intensity: 1.0, // Start with full intensity for debugging + current_intensity: 1.0, // Start with full intensity for debugging + frame_count: 0, // Initialize frame counter + } + } + + /// Update the user position (x, y, z in pixel/physical coordinates) + pub fn update_position(&mut self, x: f64, y: f64, z: f64) { + self.position_x = x; + self.position_y = y; + self.position_z = z; + + // Calculate some key metrics for debugging + let offset_x = x - self.optimal_x; + let offset_y = y - self.optimal_y; + let _xy_distance = (offset_x * offset_x + offset_y * offset_y).sqrt(); // For future use + let _z_distance = (z - self.optimal_z).abs(); // For future use + + tracing::info!( + "Position update - pos:({:.1},{:.1},{:.1}) optimal:({:.1},{:.1},{:.1}) offset_x:{:.1} offset_y:{:.1} guidance_angle:{:.1}° arc:{:.1}°", + x, y, z, + self.optimal_x, self.optimal_y, self.optimal_z, + offset_x, + offset_y, + self.calculate_guidance_angle() * 180.0 / PI, + self.calculate_arc_length() * 180.0 / PI + ); + } + + /// Calculate the guidance angle - where LEDs should light to guide user toward optimal position + fn calculate_guidance_angle(&self) -> f64 { + // Calculate offset from optimal position + let offset_x = self.position_x - self.optimal_x; + let offset_y = self.position_y - self.optimal_y; + + // Debug logging to understand coordinate mapping + if self.frame_count % 30 == 0 { // Log every second + tracing::info!( + "Position: ({:.1}, {:.1}) | Optimal: ({:.1}, {:.1}) | Offset: ({:.1}, {:.1})", + self.position_x, self.position_y, + self.optimal_x, self.optimal_y, + offset_x, offset_y + ); + } + + // Calculate the direction the user should move (opposite of their offset) + // If user is too far right (+X), they should move left (-X) → light LEFT LEDs + // If user is too far down (+Y), they should move up (-Y) → light UP LEDs + // Since current behavior matches position, flip to show guidance direction + let guidance_x = offset_x; // Flip to show opposite guidance + let guidance_y = offset_y; // Flip to show opposite guidance + + // Convert guidance direction to LED ring angle + // atan2 gives angle from positive X-axis (3 o'clock), but LED 0 is at top (12 o'clock) + // So we need to rotate by -90° to align: LED 0 = 12 o'clock = 0° + let angle = guidance_x.atan2(guidance_y) - PI / 2.0; + + // Normalize to [0, 2π] where 0 is top of ring (LED 0) + let normalized_angle = if angle < 0.0 { + angle + 2.0 * PI + } else { + angle + }; + + // Debug logging for angle calculation + if self.frame_count % 30 == 0 { // Log every second + tracing::info!( + "Guidance: ({:.1}, {:.1}) | Raw angle: {:.2} rad ({:.1}°) | Normalized: {:.2} rad ({:.1}°)", + guidance_x, guidance_y, + angle, angle.to_degrees(), + normalized_angle, normalized_angle.to_degrees() + ); + } + + normalized_angle + } + + /// Calculate arc length based on how far off-center the user is + /// Further from center = smaller, more precise arc for guidance + fn calculate_arc_length(&self) -> f64 { + // Calculate individual axis offsets + let offset_x = (self.position_x - self.optimal_x).abs(); + let offset_y = (self.position_y - self.optimal_y).abs(); + + // Use the maximum offset (most off-center direction) to determine arc size + let max_offset = offset_x.max(offset_y); + + if max_offset <= self.center_threshold { + // Very close to optimal - large arc (almost full ring) + self.max_arc_angle + } else if max_offset >= self.edge_threshold { + // Far from optimal - small, precise arc for guidance + self.min_arc_angle + } else { + // Interpolate: closer to center = larger arc, further = smaller arc + let t = (max_offset - self.center_threshold) / (self.edge_threshold - self.center_threshold); + self.max_arc_angle - t * (self.max_arc_angle - self.min_arc_angle) + } + } + + /// Calculate intensity based on how far off-center the user is + fn calculate_intensity(&self) -> f64 { + // Calculate offset magnitudes + let offset_x = (self.position_x - self.optimal_x).abs(); + let offset_y = (self.position_y - self.optimal_y).abs(); + let max_offset = offset_x.max(offset_y); + + // Base intensity: further from center = brighter guidance + let base_intensity = if max_offset <= self.center_threshold { + 0.3 // Low intensity when well-positioned + } else if max_offset >= self.edge_threshold { + 1.0 // High intensity when far off + } else { + // Interpolate: further from center = brighter + let t = (max_offset - self.center_threshold) / (self.edge_threshold - self.center_threshold); + 0.3 + t * 0.7 // Scale from 0.3 to 1.0 + }; + + // Add pulsing effect - more pulsing when further off-center for attention + let pulse_strength = (max_offset / self.edge_threshold).clamp(0.0, 1.0); + let pulse_multiplier = (1.0 - pulse_strength * 0.4) + pulse_strength * 0.4 * (self.pulse_phase.sin() * 0.5 + 0.5); + + base_intensity * pulse_multiplier + } + + /// Check if a given LED index should be lit based on guidance direction + fn should_light_led(&self, led_index: usize) -> bool { + let guidance_angle = self.calculate_guidance_angle(); + let arc_length = self.calculate_arc_length(); + let half_arc = arc_length * 0.5; + + // Calculate this LED's angle + let led_angle = (led_index as f64 / N as f64) * 2.0 * PI; + + // Calculate angular distance from guidance angle + let mut angle_diff = (led_angle - guidance_angle).abs(); + if angle_diff > PI { + angle_diff = 2.0 * PI - angle_diff; + } + + // Light LED if within the guidance arc + angle_diff <= half_arc + } +} + +impl Animation for PositionFeedback { + type Frame = RingFrame; + + fn as_any(&self) -> &dyn Any { + self + } + + fn as_any_mut(&mut self) -> &mut dyn Any { + self + } + + fn animate(&mut self, frame: &mut RingFrame, dt: f64, idle: bool) -> AnimationState { + if idle { + return AnimationState::Running; + } + + // Add a simple log to verify animate is being called + if self.frame_count == 0 { + tracing::info!("PositionFeedback animate() called for first time!"); + } + + // Update pulse phase + self.pulse_phase += dt * self.pulse_frequency * 2.0 * PI; + self.pulse_phase %= 2.0 * PI; + + // Update target intensity + self.target_intensity = self.calculate_intensity(); + + // Smooth intensity transitions + let smooth_factor = 5.0; // How fast intensity changes + self.current_intensity += (self.target_intensity - self.current_intensity) * dt * smooth_factor; + + // Debug output every few frames + self.frame_count += 1; + if self.frame_count % 30 == 0 { // Every second at 30fps + tracing::info!( + "Animation frame - intensity: {:.2}, target: {:.2}, pos: ({:.1}, {:.1}, {:.1})", + self.current_intensity, self.target_intensity, + self.position_x, self.position_y, self.position_z + ); + } + + // Calculate offsets from optimal position + let offset_x = self.position_x - self.optimal_x; + let offset_y = self.position_y - self.optimal_y; + let max_offset = offset_x.abs().max(offset_y.abs()); + + let mut leds_lit = 0; + let mut max_brightness = 0.0f64; + + if max_offset <= self.center_threshold { + // Very close to optimal position - light full ring with low intensity + tracing::info!("CENTERED MODE - max_offset: {:.1}, intensity: {:.2}", max_offset, self.current_intensity); + for led in frame.iter_mut() { + *led = self.color * self.current_intensity; + leds_lit += 1; + max_brightness = max_brightness.max(self.current_intensity); + } + } else if max_offset <= self.edge_threshold * 2.0 { + // Show directional guidance arc + let guidance_angle = self.calculate_guidance_angle(); + let arc_length = self.calculate_arc_length(); + + tracing::info!( + "GUIDANCE MODE - offset_x: {:.1}, offset_y: {:.1}, guidance_angle: {:.1}°, arc: {:.1}°, intensity: {:.2}", + offset_x, + offset_y, + guidance_angle * 180.0 / PI, + arc_length * 180.0 / PI, + self.current_intensity + ); + + // Debug: Show which LEDs should be lit for orientation testing + let mut lit_leds = Vec::new(); + for i in 0..N { + if self.should_light_led(i) { + lit_leds.push(i); + } + } + if self.frame_count % 30 == 0 && !lit_leds.is_empty() { + let led_count = lit_leds.len(); + let display_leds = if led_count <= 10 { + format!("{:?}", lit_leds) + } else { + format!("{:?}..{:?}", &lit_leds[0..5], &lit_leds[led_count-5..]) + }; + tracing::info!("LEDs to light: {} (total: {})", display_leds, led_count); + } + + for (i, led) in frame.iter_mut().enumerate() { + if self.should_light_led(i) { + *led = self.color * self.current_intensity; + leds_lit += 1; + max_brightness = max_brightness.max(self.current_intensity); + } else { + *led = Argb::OFF; + } + } + } else { + // Very far from optimal position - turn off all LEDs (user out of frame) + tracing::info!("OUT OF FRAME - max_offset: {:.1}, turning off all LEDs", max_offset); + for led in frame.iter_mut() { + *led = Argb::OFF; + } + } + + if self.frame_count % 30 == 0 { + tracing::info!( + "LED Result - {} LEDs lit, max brightness: {:.2}, color: {:?}", + leds_lit, max_brightness, self.color + ); + } + + AnimationState::Running + } + + fn stop(&mut self, _transition: crate::engine::Transition) -> eyre::Result<()> { + Ok(()) + } +} diff --git a/ui/src/engine/diamond.rs b/ui/src/engine/diamond.rs index 720ecf379..de303e71a 100644 --- a/ui/src/engine/diamond.rs +++ b/ui/src/engine/diamond.rs @@ -30,6 +30,9 @@ use super::animations::composites::biometric_flow::{ }; use super::CriticalState; +// Position feedback animation level - higher than LEVEL_NOTICE to ensure it takes priority +const LEVEL_POSITION_FEEDBACK: u8 = 30; + struct WrappedCenterMessage(Message); struct WrappedRingMessage(Message); @@ -236,6 +239,114 @@ impl Runner { self.center_animations_stack.stop(level, transition); } + fn start_position_feedback(&mut self) { + tracing::info!( + "Starting position feedback animation with color: {:?}", + Argb::DIAMOND_RING_USER_CAPTURE + ); + // Use higher priority than LEVEL_NOTICE to ensure our animation runs + self.set_ring( + LEVEL_POSITION_FEEDBACK, + animations::position_feedback::PositionFeedback::::new( + Argb::DIAMOND_RING_USER_CAPTURE + ), + ); + } + + fn stop_position_feedback(&mut self) { + // Only stop if the current animation is actually position feedback + let is_position_feedback = self + .ring_animations_stack + .stack + .get(&LEVEL_POSITION_FEEDBACK) + .and_then(|RunningAnimation { animation, .. }| { + animation + .as_any() + .downcast_ref::>() + }) + .is_some(); + + if is_position_feedback { + self.stop_ring(LEVEL_POSITION_FEEDBACK, Transition::ForceStop); + } + } + + fn update_position_feedback(&mut self, x: f64, y: f64, z: f64) { + tracing::info!( + "Update position feedback called: ({:.1}, {:.1}, {:.1})", + x, + y, + z + ); + + // Check what's currently running at all levels + tracing::info!("=== Animation Stack Status ==="); + for (&level, animation) in &self.ring_animations_stack.stack { + tracing::info!("Level {}: {}", level, animation.animation.name()); + } + if self.ring_animations_stack.stack.is_empty() { + tracing::info!("Ring animation stack is EMPTY"); + } + tracing::info!("==============================="); + + // Check what's currently running at LEVEL_FOREGROUND + if let Some(animation) = self.ring_animations_stack.stack.get(&LEVEL_FOREGROUND) + { + tracing::info!( + "Animation exists at LEVEL_FOREGROUND: {}", + animation.animation.name() + ); + } else { + tracing::info!("No animation at LEVEL_FOREGROUND"); + } + + // Check if the current LEVEL_POSITION_FEEDBACK animation is a PositionFeedback + let is_position_feedback_active = self + .ring_animations_stack + .stack + .get(&LEVEL_POSITION_FEEDBACK) + .and_then(|RunningAnimation { animation, .. }| { + animation + .as_any() + .downcast_ref::>() + }) + .is_some(); + + if !is_position_feedback_active { + // No position feedback animation or wrong animation type - start/replace with position feedback + tracing::info!("Position feedback not active, starting new one"); + self.start_position_feedback(); + tracing::info!("Position feedback started"); + } else { + tracing::info!("Position feedback animation already active"); + } + + // Now update the position (we know it exists) + if let Some(position_feedback) = self + .ring_animations_stack + .stack + .get_mut(&LEVEL_POSITION_FEEDBACK) + .and_then(|RunningAnimation { animation, .. }| { + animation + .as_any_mut() + .downcast_mut::>() + }) + { + tracing::info!("Update position feedback: {:?}, {:?}, {:?}", x, y, z); + position_feedback.update_position(x, y, z); + } else { + tracing::error!( + "Failed to update position feedback animation after ensuring it exists" + ); + } + } + fn biometric_capture_success(&mut self) -> Result<()> { // fade out duration + sound delay // delaying the sound allows resynchronizing in case another @@ -323,6 +434,9 @@ impl EventHandler for Runner { fn event(&mut self, event: &Event) -> Result<()> { match event { Event::Bootup => { + // Stop position feedback during bootup + self.stop_position_feedback(); + self.stop_ring(LEVEL_NOTICE, Transition::ForceStop); self.stop_center(LEVEL_NOTICE, Transition::ForceStop); self.set_ring( @@ -379,6 +493,9 @@ impl EventHandler for Runner { ); } Event::Shutdown { requested: _ } => { + // Stop position feedback during shutdown + self.stop_position_feedback(); + self.sound.queue( sound::Type::Melody(sound::Melody::PoweringDown), Duration::ZERO, @@ -647,6 +764,11 @@ impl EventHandler for Runner { sound::Type::Melody(sound::Melody::UserStartCapture), Duration::ZERO, )?; + + // Start position feedback animation for real-time user positioning + tracing::info!("SignupStart event - about to start position feedback"); + self.start_position_feedback(); + tracing::info!("SignupStart event - position feedback started"); } Event::BiometricCaptureHalfObjectivesCompleted => { // do nothing @@ -901,6 +1023,10 @@ impl EventHandler for Runner { positioning.set_in_range(*in_range); } } + Event::BiometricCapturePosition { x, y, z } => { + // Update real-time position feedback animation (ensures it's the sole animation running) + self.update_position_feedback(*x, *y, *z); + } Event::BiometricFlowStart { timeout, min_fast_forward_duration, @@ -1040,6 +1166,9 @@ impl EventHandler for Runner { )?; } Event::SignupFail { reason } => { + // Stop position feedback when signup fails + self.stop_position_feedback(); + match reason { SignupFailReason::Timeout => { self.play_signup_fail_ux(Some(sound::Type::Voice( @@ -1077,6 +1206,9 @@ impl EventHandler for Runner { self.operator_signup_phase.failure(); } Event::SignupSuccess => { + // Stop position feedback when signup succeeds + self.stop_position_feedback(); + self.operator_signup_phase.signup_successful(); self.set_ring( LEVEL_BACKGROUND, @@ -1085,6 +1217,9 @@ impl EventHandler for Runner { self.stop_ring(LEVEL_FOREGROUND, Transition::ForceStop); } Event::Idle => { + // Stop position feedback when going to idle + self.stop_position_feedback(); + self.stop_ring(LEVEL_FOREGROUND, Transition::ForceStop); self.stop_center(LEVEL_FOREGROUND, Transition::ForceStop); self.stop_ring(LEVEL_NOTICE, Transition::ForceStop); diff --git a/ui/src/engine/mod.rs b/ui/src/engine/mod.rs index 3e4b9ae85..dade6326c 100644 --- a/ui/src/engine/mod.rs +++ b/ui/src/engine/mod.rs @@ -314,6 +314,11 @@ event_enum! { BiometricCaptureDistance { in_range: bool }, + /// User position in frame + #[event_enum(method = biometric_capture_position)] + BiometricCapturePosition { + x: f64, y: f64, z: f64, + }, /// Biometric capture succeeded. #[event_enum(method = biometric_capture_success)] BiometricCaptureSuccess,