buttplug.io
Buttplug sex toy control library
feat: Better timing for the Fredorch F21S
This uses a lookup table vs trying to come up with
a magic algorithm. It's not perfect, but it's worlds
better. Be aware that the chatty-ness of the protocol
still means updates aren't as immediate as you'd want.
authored by
blackspherefollower
and committed by
+48
-16
crates/buttplug_server/src/device/protocol_impl/fredorch.rs
+48
-16
crates/buttplug_server/src/device/protocol_impl/fredorch.rs
···
33
33
use tokio::select;
34
34
use uuid::{Uuid, uuid};
35
35
36
-
use super::fleshlight_launch_helper::calculate_speed;
37
-
38
36
const FREDORCH_COMMAND_TIMEOUT_MS: u64 = 500;
39
37
40
38
const CRC_HI: [u8; 256] = [
···
186
184
}
187
185
}
188
186
187
+
const speed_matrix: [[u32; 20]; 15] = [
188
+
// distance, speed 1-20
189
+
/* 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 */
190
+
/* 1*/ [1000, 800, 400, 235, 200, 172, 155, 92, 60, 45, 38, 34, 32, 28, 27, 26, 25, 24, 23, 22 ],
191
+
/* 2*/ [1500, 1000, 800, 680, 600, 515, 425, 265, 165, 115, 80, 70, 50, 48, 45, 35, 34, 33, 32, 30 ],
192
+
/* 3*/ [2500, 2310, 1135, 925, 792, 695, 565, 380, 218, 155, 105, 82, 70, 68, 65, 60, 48, 45, 43, 40 ],
193
+
/* 4*/ [3000, 2800, 1500, 1155, 965, 810, 690, 465, 260, 195, 140, 110, 85, 75, 74, 73, 70, 65, 60, 55 ],
194
+
/* 5*/ [3400, 3232, 2305, 1380, 1200, 1165, 972, 565, 328, 235, 162, 132, 98, 78, 75, 74, 73, 72, 71, 70 ],
195
+
/* 6*/ [3500, 3350, 2500, 1640, 1250, 1210, 1010, 645, 385, 275, 175, 160, 115, 95, 91, 90, 85, 80, 77, 75 ],
196
+
/* 7*/ [3600, 3472, 2980, 2060, 1560, 1275, 1132, 738, 430, 310, 230, 170, 128, 122, 110, 108, 105, 103, 101, 100],
197
+
/* 8*/ [3800, 3500, 3055, 2105, 1740, 1370, 1290, 830, 490, 355, 235, 195, 150, 140, 135, 132, 130, 125, 120, 119],
198
+
/* 9*/ [3900, 3518, 3190, 2315, 2045, 1510, 1442, 1045, 552, 392, 280, 225, 172, 145, 140, 138, 135, 134, 132, 130],
199
+
/*10*/ [6000, 5755, 3240, 2530, 2135, 1605, 1500, 1200, 595, 425, 285, 245, 175, 170, 160, 155, 150, 145, 142, 140],
200
+
/*11*/ [6428, 5872, 3335, 2780, 2270, 1782, 1590, 1310, 648, 470, 315, 255, 182, 180, 175, 172, 170, 162, 160, 155],
201
+
/*12*/ [6730, 5950, 3490, 2995, 2395, 1890, 1650, 1350, 700, 500, 350, 290, 220, 190, 185, 180, 175, 170, 165, 160],
202
+
/*13*/ [6962, 6122, 3880, 3205, 2465, 1900, 1700, 1400, 835, 545, 375, 310, 228, 195, 190, 185, 182, 181, 180, 175],
203
+
/*14*/ [7945, 6365, 4130, 3470, 2505, 1910, 1755, 1510, 855, 580, 400, 330, 235, 210, 205, 200, 195, 190, 185, 180],
204
+
/*15*/ [8048, 7068, 4442, 3708, 2668, 1930, 1800, 1520, 878, 618, 428, 365, 260, 255, 250, 240, 230, 220, 210, 200],
205
+
];
206
+
207
+
fn calculate_speed(mut distance: u32, duration: u32) -> u8 {
208
+
let distance = distance.clamp(0,15);
209
+
if distance == 0 {return 0;}
210
+
211
+
let mut speed= 1;
212
+
while speed < 20 {
213
+
if speed_matrix[distance as usize - 1][speed as usize - 1] < duration {
214
+
return speed;
215
+
}
216
+
speed += 1;
217
+
}
218
+
speed
219
+
}
220
+
189
221
#[derive(Default)]
190
222
pub struct Fredorch {
191
223
previous_position: Arc<AtomicU8>,
···
199
231
position: u32,
200
232
duration: u32,
201
233
) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
202
-
// In the protocol, we know max speed is 99, so convert here. We have to
203
-
// use AtomicU8 because there's no AtomicF64 yet.
204
234
let previous_position = self.previous_position.load(Ordering::Relaxed);
205
-
let distance = (previous_position as f64 - position as f64).abs() / 99f64;
235
+
let distance = (previous_position as i32 - position as i32).abs() as u32;
236
+
// The Fredorch only has 15 positions, but scales them to 0-150
237
+
let pos = (position * 10) as u8;
206
238
207
-
// TODO Clean this up, we do not need the conversions anymore since we'll have done the
208
-
// calculations before we get to the protocol layer.
209
-
let position = ((position as f64 / 99.0) * 150.0) as u8;
210
-
let converted_speed = calculate_speed(distance, duration.try_into().unwrap()) * 99f64;
211
-
let speed = ((converted_speed / 99.0) * 15.0) as u8;
239
+
let speed = calculate_speed(distance, duration);
212
240
let mut data: Vec<u8> = vec![
213
-
0x01, 0x10, 0x00, 0x6B, 0x00, 0x05, 0x0a, 0x00, speed, 0x00, speed, 0x00, position, 0x00,
214
-
position, 0x00, 0x01,
241
+
0x01, 0x10, 0x00, 0x6B, 0x00, 0x05, 0x0a, 0x00, speed, 0x00, speed, 0x00, pos, 0x00, pos,
242
+
0x00, 0x01,
215
243
];
216
244
let crc = crc16(&data);
217
245
data.push(crc[0]);
218
246
data.push(crc[1]);
219
-
self.previous_position.store(position, Ordering::Relaxed);
220
-
Ok(vec![
221
-
HardwareWriteCmd::new(&[FREDORCH_PROTOCOL_UUID], Endpoint::Tx, data, false).into(),
222
-
])
247
+
self.previous_position.store(position as u8, Ordering::Relaxed);
248
+
Ok(vec![HardwareWriteCmd::new(
249
+
&[FREDORCH_PROTOCOL_UUID],
250
+
Endpoint::Tx,
251
+
data,
252
+
false,
253
+
)
254
+
.into()])
223
255
}
224
256
}