diff --git a/crates/buttplug_server/src/device/protocol_impl/lovense/lovense_stroker.rs b/crates/buttplug_server/src/device/protocol_impl/lovense/lovense_stroker.rs index 992a388f6..b2f9bf358 100644 --- a/crates/buttplug_server/src/device/protocol_impl/lovense/lovense_stroker.rs +++ b/crates/buttplug_server/src/device/protocol_impl/lovense/lovense_stroker.rs @@ -18,22 +18,24 @@ use buttplug_server_device_config::Endpoint; use futures::future::BoxFuture; use std::{ sync::{ - Arc, - atomic::{AtomicU32, Ordering}, - }, - time::Duration, + Arc, RwLock + }, time::Duration }; use uuid::{Uuid, uuid}; +use instant::Instant; + const LOVENSE_STROKER_PROTOCOL_UUID: Uuid = uuid!("a97fc354-5561-459a-bc62-110d7c2868ac"); +const LINEAR_STEP_INTERVAL: Duration = Duration::from_millis(100); + pub struct LovenseStroker { - linear_info: Arc<(AtomicU32, AtomicU32)>, + linear_info: Arc>, } impl LovenseStroker { pub fn new(hardware: Arc) -> Self { - let linear_info = Arc::new((AtomicU32::new(0), AtomicU32::new(0))); + let linear_info = Arc::new(RwLock::new((0, 0, Instant::now()))); async_manager::spawn(update_linear_movement( hardware.clone(), linear_info.clone(), @@ -54,8 +56,7 @@ impl ProtocolHandler for LovenseStroker { position: u32, duration: u32, ) -> Result, ButtplugDeviceError> { - self.linear_info.0.store(position, Ordering::Relaxed); - self.linear_info.1.store(duration, Ordering::Relaxed); + *self.linear_info.write().unwrap() = (position, duration, Instant::now()); Ok(vec![]) } @@ -70,39 +71,69 @@ impl ProtocolHandler for LovenseStroker { } } -async fn update_linear_movement(device: Arc, linear_info: Arc<(AtomicU32, AtomicU32)>) { - let mut last_goal_position = 0i32; - let mut current_move_amount = 0i32; - let mut current_position = 0i32; +async fn update_linear_movement(device: Arc, linear_info: Arc>) { + let mut current_position = 0u32; + let mut start_position = 0u32; + let mut last_goal_position = 0u32; + let mut last_start_time = Instant::now(); loop { - // See if we've updated our goal position - let goal_position = linear_info.0.load(Ordering::Relaxed) as i32; - // If we have and it's not the same, recalculate based on current status. - if last_goal_position != goal_position { + let (goal_position, goal_duration, start_time) = { *linear_info.read().unwrap() }; + let current_time = Instant::now(); + let end_time = start_time + Duration::from_millis(goal_duration.try_into().unwrap()); + + // Sleep, accounting for time passed during loop (mostly from bt call time) + let fn_sleep = async || { + let elapsed = Instant::now() - current_time; + if elapsed < LINEAR_STEP_INTERVAL { + sleep(LINEAR_STEP_INTERVAL - elapsed).await + }; + }; + + //debug!("lovense: goal data {:?}/{:?}/{:?}", goal_position, goal_duration, start_time); + + // At rest + if current_position == goal_position { + fn_sleep().await; + continue; + } + + // If parameters changed, re-capture the current position as the new starting position. + if last_start_time != start_time || last_goal_position != goal_position { + start_position = current_position; + last_start_time = start_time; last_goal_position = goal_position; - // We move every 100ms, so divide the movement into that many chunks. - // If we're moving so fast it'd be under our 100ms boundary, just move in 1 step. - let move_steps = (linear_info.1.load(Ordering::Relaxed) / 100).max(1); - current_move_amount = (goal_position - current_position) / move_steps as i32; } - // If we aren't going anywhere, just pause then restart - if current_position == last_goal_position { - sleep(Duration::from_millis(100)).await; + // Determine where in the motion we should be + assert!(current_time >= start_time); + let step_position = if current_time < end_time { + let movement_range = goal_position as f64 - start_position as f64; + let time_elapsed_ms = (current_time - start_time).as_millis(); + + let step_percentage = (time_elapsed_ms as f64) / (goal_duration as f64); + let step_position_dbl = step_percentage * movement_range + (start_position as f64); + let step_position = step_position_dbl.round() as u32; + + //debug!("lovense: calculating step for time {:?} with start of {:?} and end of {:?}. Pct movement is {:?} from {:?} to {:?}, result {:?}", + // current_time, start_time, end_time, step_percentage, start_position, goal_position, step_position); + + step_position + } else { + goal_position + }; + + // No movement over this window + if current_position == step_position { + fn_sleep().await; continue; } - // Update our position, make sure we don't overshoot - current_position += current_move_amount; - if current_move_amount < 0 { - if current_position < last_goal_position { - current_position = last_goal_position; - } - } else if current_position > last_goal_position { - current_position = last_goal_position; - } + //debug!("lovense: moving to position {:?} from {:?}, goal {:?}", step_position, current_position, goal_position); + + current_position = step_position; - let lovense_cmd = format!("FSetSite:{current_position};"); + //let lovense_cmd = format!("FSetSite:{current_position};"); + let lovense_cmd = format!("SetPoint:{current_position};"); let hardware_cmd: HardwareWriteCmd = HardwareWriteCmd::new( &[LOVENSE_STROKER_PROTOCOL_UUID], @@ -113,6 +144,7 @@ async fn update_linear_movement(device: Arc, linear_info: Arc<(AtomicU if device.write_value(&hardware_cmd).await.is_err() { return; } - sleep(Duration::from_millis(100)).await; + + fn_sleep().await; } }