87 lines
2.5 KiB
Rust
87 lines
2.5 KiB
Rust
use crate::serial_core::multi_dim_force::PztProcessor;
|
|
|
|
pub const FINGER_SAMPLE_COUNT: usize = 84;
|
|
pub const HAND_FINGERTIP_COUNT: usize = 5;
|
|
const MIN_TANGENTIAL_MAGNITUDE: f32 = 0.02;
|
|
|
|
#[derive(Debug, Clone, Copy)]
|
|
pub struct HudSpatialForce {
|
|
pub angle_deg: f32,
|
|
pub magnitude: f32,
|
|
}
|
|
|
|
pub struct ForceEstimatorState {
|
|
pzt_processor: PztProcessor,
|
|
fingertip_processors: [PztProcessor; HAND_FINGERTIP_COUNT],
|
|
}
|
|
|
|
impl ForceEstimatorState {
|
|
pub fn new() -> Self {
|
|
Self {
|
|
pzt_processor: PztProcessor::new(),
|
|
fingertip_processors: std::array::from_fn(|_| PztProcessor::new()),
|
|
}
|
|
}
|
|
|
|
pub fn reset(&mut self) {
|
|
self.pzt_processor.reset_baseline();
|
|
self.fingertip_processors
|
|
.iter_mut()
|
|
.for_each(PztProcessor::reset_baseline);
|
|
}
|
|
|
|
pub fn analyze(&mut self, values: &[u32]) -> Option<HudSpatialForce> {
|
|
if values.len() != FINGER_SAMPLE_COUNT {
|
|
return None;
|
|
}
|
|
|
|
let pzt_values = values.iter().map(|value| *value as f32).collect::<Vec<_>>();
|
|
|
|
self.pzt_processor
|
|
.get_pzt_analysis(&pzt_values)
|
|
.ok()
|
|
.filter(|analysis| analysis.magnitude > MIN_TANGENTIAL_MAGNITUDE)
|
|
.map(|analysis| HudSpatialForce {
|
|
angle_deg: analysis.angle_deg,
|
|
magnitude: analysis.magnitude,
|
|
})
|
|
}
|
|
|
|
pub fn analyze_fingertips(
|
|
&mut self,
|
|
values: &[u32],
|
|
) -> [Option<HudSpatialForce>; HAND_FINGERTIP_COUNT] {
|
|
let mut forces = [None; HAND_FINGERTIP_COUNT];
|
|
|
|
for (tip_index, processor) in self.fingertip_processors.iter_mut().enumerate() {
|
|
let start = tip_index * FINGER_SAMPLE_COUNT;
|
|
let end = start + FINGER_SAMPLE_COUNT;
|
|
let Some(segment) = values.get(start..end) else {
|
|
break;
|
|
};
|
|
|
|
let pzt_values = segment
|
|
.iter()
|
|
.map(|value| *value as f32)
|
|
.collect::<Vec<_>>();
|
|
|
|
forces[tip_index] = processor
|
|
.get_pzt_analysis(&pzt_values)
|
|
.ok()
|
|
.filter(|analysis| analysis.magnitude > MIN_TANGENTIAL_MAGNITUDE)
|
|
.map(|analysis| HudSpatialForce {
|
|
angle_deg: analysis.angle_deg,
|
|
magnitude: analysis.magnitude,
|
|
});
|
|
}
|
|
|
|
forces
|
|
}
|
|
}
|
|
|
|
impl Default for ForceEstimatorState {
|
|
fn default() -> Self {
|
|
Self::new()
|
|
}
|
|
}
|