diff --git a/buttplug/buttplug-device-config/build-config/buttplug-device-config-v3.json b/buttplug/buttplug-device-config/build-config/buttplug-device-config-v3.json index 801d71af8..b0f45ef42 100644 --- a/buttplug/buttplug-device-config/build-config/buttplug-device-config-v3.json +++ b/buttplug/buttplug-device-config/build-config/buttplug-device-config-v3.json @@ -18686,6 +18686,63 @@ } } ] + }, + "syncbot": { + "defaults": { + "name": "Syncbot", + "features": [ + { + "feature-type": "Rotate", + "actuator": { + "step-range": [ + 0, + 127 + ], + "messages": [ + "RotateCmd" + ] + } + }, + { + "feature-type": "Constrict", + "actuator": { + "step-range": [ + 0, + 88 + ], + "messages": [ + "ScalarCmd" + ] + } + }, + { + "feature-type": "Position", + "actuator": { + "step-range": [ + 0, + 255 + ], + "messages": [ + "LinearCmd" + ] + } + } + ] + }, + "communication": [ + { + "btle": { + "names": [ + "V" + ], + "services": { + "0000ffe0-0000-1000-8000-00805f9b34fb": { + "tx": "0000ffe1-0000-1000-8000-00805f9b34fb" + } + } + } + } + ] } } } diff --git a/buttplug/buttplug-device-config/device-config-v3/buttplug-device-config-v3.yml b/buttplug/buttplug-device-config/device-config-v3/buttplug-device-config-v3.yml index e8239ac9a..afa31d5c8 100644 --- a/buttplug/buttplug-device-config/device-config-v3/buttplug-device-config-v3.yml +++ b/buttplug/buttplug-device-config/device-config-v3/buttplug-device-config-v3.yml @@ -10711,4 +10711,36 @@ protocols: - S6 services: 0000ffb0-0000-1000-8000-00805f9b34fb: - tx: 0000ffb2-0000-1000-8000-00805f9b34fb \ No newline at end of file + tx: 0000ffb2-0000-1000-8000-00805f9b34fb + syncbot: + defaults: + name: Syncbot + features: + - feature-type: Rotate + actuator: + step-range: + - 0 + - 127 + messages: + - RotateCmd + - feature-type: Constrict + actuator: + step-range: + - 0 + - 88 + messages: + - ScalarCmd + - feature-type: Position + actuator: + step-range: + - 0 + - 255 + messages: + - LinearCmd + communication: + - btle: + names: + - V + services: + 0000ffe0-0000-1000-8000-00805f9b34fb: + tx: 0000ffe1-0000-1000-8000-00805f9b34fb \ No newline at end of file diff --git a/buttplug/src/server/device/protocol/mod.rs b/buttplug/src/server/device/protocol/mod.rs index d32097f03..4199ef186 100644 --- a/buttplug/src/server/device/protocol/mod.rs +++ b/buttplug/src/server/device/protocol/mod.rs @@ -125,6 +125,7 @@ pub mod svakom_v3; pub mod svakom_v4; pub mod svakom_v5; pub mod svakom_v6; +pub mod syncbot; pub mod synchro; pub mod tcode_v03; pub mod thehandy; @@ -608,6 +609,10 @@ pub fn get_default_protocol_map() -> HashMap, + _: &UserDeviceDefinition, + ) -> Result, ButtplugDeviceError> { + hardware + .write_value(&HardwareWriteCmd::new( + Endpoint::Tx, + // f0c82c000000000000000000000000000000e4 + vec![ + 0xf0, 0xc8, 0x2c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xe4, + ], + false, + )) + .await?; + Ok(Arc::new(Syncbot::new(hardware))) + } +} + +/// Generates the command data for the Syncbot protocol. +/// Protocol Format: +/// vec![ +/// 0xf0, // byte 0: signature +/// 0xc9, // byte 1: signature +/// 0x00, // byte 2: data (position (0-255); encrypted) +/// 0x00, // byte 3: data (rotation (0-255: 128 is neutral); encrypted) +/// 0x00, // byte 4: data (grip (38-216: 128 is neutral); encrypted) +/// 0x00, // byte 5: data (unused?; encrypted) +/// 0x00, // byte 6: checksum1 (sum of unencrypted bytes 2-5; encrypted) +/// 0x00, // byte 7: null? +/// 0x00, // byte 8: frame ID +/// 0x00, // byte 9: encrypt key +/// 0x00, // byte 10: encrypt key +/// 0x00, // byte 11: encrypt key +/// 0x00, // byte 12: encrypt key +/// 0x00, // byte 13: encrypt key +/// 0x00, // byte 14: null +/// 0x00, // byte 15: null +/// 0x00, // byte 16: null +/// 0x00, // byte 17: null +/// 0x00, // byte 18: checksum2 (sum of bytes 0-17) +/// ], +fn generate_command_data(command: [u8; 3], frame_id: u8) -> Vec { + let data1: u8 = command[0]; + let data2: u8 = command[1]; + let data3: u8 = command[2]; + let data4: u8 = 0; + let checksum1: u8 = ((data1 as u32 + data2 as u32 + data3 as u32 + data4 as u32) & 0xff) as u8; + let encrypt_key = ENCRYPT_KEYS[frame_id as usize].to_be_bytes(); + let mut command_data: Vec = vec![ + 0xf0, + 0xc9, + data1 ^ encrypt_key[3], + data2 ^ encrypt_key[4], + data3 ^ encrypt_key[5], + data4 ^ encrypt_key[6], + checksum1 ^ encrypt_key[7], + 0x00, + frame_id, + encrypt_key[3], + encrypt_key[4], + encrypt_key[5], + encrypt_key[6], + encrypt_key[7], + 0x00, + 0x00, + 0x00, + 0x00, + ]; + let checksum2: u8 = command_data.iter().fold(0u8, |acc, x| acc.wrapping_add(*x)) & 0xff; + command_data.extend(&[checksum2]); + command_data +} + +async fn command_update_handler(device: Arc, syncbot: Syncbot) { + debug!("Entering Syncbot Control Loop"); + let mut current_command = syncbot.current_command.read().await.clone(); + let mut frame_id = 0_u8; + while device + .write_value(&HardwareWriteCmd::new( + Endpoint::Tx, + generate_command_data(current_command, frame_id), + false, + )) + .await + .is_ok() + { + sleep(Duration::from_millis(CONTROL_LOOP_INTERVAL_MS as u64)).await; + frame_id = frame_id.wrapping_add(1); + let current_position = syncbot.current_position.read().await.clone(); + let target_position = syncbot.target_position.read().await.clone(); + let position_speed = syncbot.position_speed.read().await.clone(); + if current_position < target_position { + let mut new_position = current_position + position_speed * CONTROL_LOOP_INTERVAL_MS; + if new_position > target_position { + new_position = target_position; + } + let mut current_position = syncbot.current_position.write().await; + *current_position = new_position; + } else if current_position > target_position { + let mut new_position = current_position - position_speed * CONTROL_LOOP_INTERVAL_MS; + if new_position < target_position { + new_position = target_position; + } + let mut current_position = syncbot.current_position.write().await; + *current_position = new_position; + } + current_command = syncbot.current_command.write().await.clone(); + current_command[0] = current_position as u8; + trace!("Syncbot Command: {:?}", current_command); + } + info!("Syncbot control loop exiting, most likely due to device disconnection."); +} + +#[derive(Default, Clone)] +pub struct Syncbot { + current_command: Arc>, + current_position: Arc>, + target_position: Arc>, + position_speed: Arc>, +} + +impl Syncbot { + pub fn new(device: Arc) -> Self { + let syncbot = Self { + current_command: Arc::new(RwLock::new([0, 128, 128])), + current_position: Arc::new(RwLock::new(0.0)), + target_position: Arc::new(RwLock::new(0.0)), + position_speed: Arc::new(RwLock::new(0.0)), + }; + let syncbot_clone = syncbot.clone(); + async_manager::spawn(async move { command_update_handler(device, syncbot_clone).await }); + syncbot + } +} + +impl ProtocolHandler for Syncbot { + fn handle_linear_cmd( + &self, + message: LinearCmdV4, + ) -> Result, ButtplugDeviceError> { + debug!("Syncbot: Handling linear command: {:?}", message); + let vector = message.vectors()[0].clone(); + let position = vector.position() * 255f64; + let duration = vector.duration() as f64; + let current_position = self.current_position.clone(); + let position_speed = self.position_speed.clone(); + let target_position = self.target_position.clone(); + async_manager::spawn(async move { + let current_position = current_position.read().await; + let speed = (position - *current_position).abs() / duration; + let mut position_speed = position_speed.write().await; + *position_speed = speed; + let mut target_position = target_position.write().await; + *target_position = position; + }); + Ok(vec![]) + } + + fn handle_rotate_cmd( + &self, + cmds: &[Option<(u32, bool)>], + ) -> Result, ButtplugDeviceError> { + debug!("Syncbot: Handling rotate command: {:?}", cmds); + if let Some((speed, clockwise)) = cmds[0] { + let current_command = self.current_command.clone(); + let rotate_byte = if clockwise { + 128_u8 + speed as u8 + } else { + 128_u8 - speed as u8 + }; + async_manager::spawn(async move { + let mut command_writer = current_command.write().await; + command_writer[1] = rotate_byte; + }); + } + Ok(vec![]) + } + + fn handle_scalar_constrict_cmd( + &self, + _index: u32, + scalar: u32, + ) -> Result, ButtplugDeviceError> { + debug!("Syncbot: Handling constrict command: {:?}", scalar); + let current_command = self.current_command.clone(); + // Gripping into negative direction is currently not supported + let constrict_byte = 128_u8 + scalar as u8; + async_manager::spawn(async move { + let mut command_writer = current_command.write().await; + command_writer[2] = constrict_byte; + }); + Ok(vec![]) + } +} diff --git a/buttplug/tests/test_device_protocols.rs b/buttplug/tests/test_device_protocols.rs index 362933030..62c32543f 100644 --- a/buttplug/tests/test_device_protocols.rs +++ b/buttplug/tests/test_device_protocols.rs @@ -128,6 +128,7 @@ async fn load_test_case(test_file: &str) -> DeviceTestCase { #[test_case("test_luvmazer_protocol.yaml" ; "Luvmazer Protocol")] #[test_case("test_bananasome_protocol.yaml" ; "Bananasome Protocol")] #[test_case("test_omobo_protocol.yaml" ; "Omobo Protocol")] +#[test_case("test_syncbot_protocol.yaml" ; "Syncbot Protocol")] #[tokio::test] async fn test_device_protocols_embedded_v3(test_file: &str) { //tracing_subscriber::fmt::init(); @@ -246,6 +247,7 @@ async fn test_device_protocols_embedded_v3(test_file: &str) { #[test_case("test_luvmazer_protocol.yaml" ; "Luvmazer Protocol")] #[test_case("test_bananasome_protocol.yaml" ; "Bananasome Protocol")] #[test_case("test_omobo_protocol.yaml" ; "Omobo Protocol")] +#[test_case("test_syncbot_protocol.yaml" ; "Syncbot Protocol")] #[tokio::test] async fn test_device_protocols_json_v3(test_file: &str) { //tracing_subscriber::fmt::init(); diff --git a/buttplug/tests/util/device_test/device_test_case/test_syncbot_protocol.yaml b/buttplug/tests/util/device_test/device_test_case/test_syncbot_protocol.yaml new file mode 100644 index 000000000..38f116e29 --- /dev/null +++ b/buttplug/tests/util/device_test/device_test_case/test_syncbot_protocol.yaml @@ -0,0 +1,159 @@ +devices: + - identifier: + name: "V" + expected_name: "Syncbot" +device_commands: + # We'll get a stop packet first as the repeat task spins up. + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + data: [240, 200, 44, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 228] + write_with_response: false + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 234 ^ 234 = 0 + # Rotation: 36 ^ 164 = 128 + # Grip: 50 ^ 178 = 128 + data: [240, 201, 234, 36, 50, 35, 33, 0, 0, 234, 164, 178, 35, 33, 0, 0, 0, 0, 193] + write_with_response: false + - !Messages + device_index: 0 + messages: + - !Rotate + - Index: 0 + Speed: 0.5 + Clockwise: true + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 67 ^ 67 = 0 + # Rotation: 182 ^ 118 = 192 + # Grip: 163 ^ 35 = 128 + data: [240, 201, 67, 182, 163, 109, 57, 0, 1, 67, 118, 35, 109, 121, 0, 0, 0, 0, 190] + write_with_response: false + - !Messages + device_index: 0 + messages: + - !Rotate + - Index: 0 + Speed: 1 + Clockwise: false + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 54 ^ 54 = 0 + # Rotation: 146 ^ 147 = 1 + # Grip: 69 ^ 197 = 128 + data: [240, 201, 54, 146, 69, 39, 110, 0, 2, 54, 147, 197, 39, 239, 0, 0, 0, 0, 1] + write_with_response: false + - !Messages + device_index: 0 + messages: + - !Scalar + - Index: 0 + Scalar: 0.5 + ActuatorType: Constrict + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 25 ^ 25 = 0 + # Rotation: 109 ^ 108 = 1 + # Grip: 71 ^ 235 = 172 + data: [240, 201, 25, 109, 71, 45, 169, 0, 3, 25, 108, 235, 45, 4, 0, 0, 0, 0, 0] + write_with_response: false + - !Messages + device_index: 0 + messages: + - !Scalar + - Index: 0 + Scalar: 0 + ActuatorType: Constrict + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 229 ^ 229 = 0 + # Rotation: 213 ^ 212 = 1 + # Grip: 89 ^ 217 = 128 + data: [240, 201, 229, 213, 89, 222, 181, 0, 4, 229, 212, 217, 222, 52, 0, 0, 0, 0, 7] + write_with_response: false + - !Messages + device_index: 0 + messages: + - !Linear + - Index: 0 + Position: 0.5 + Duration: 1 + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 56 ^ 56 = 0 + # Rotation: 46 ^ 47 = 1 + # Grip: 234 ^ 106 = 128 + data: [240, 201, 56, 46, 234, 235, 74, 0, 5, 56, 47, 106, 235, 203, 0, 0, 0, 0, 202] + write_with_response: false + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 65 ^ 62 = 127 + # Rotation: 180 ^ 181 = 1 + # Grip: 70 ^ 198 = 128 + data: [240, 201, 65, 180, 70, 236, 75, 0, 6, 62, 181, 198, 236, 75, 0, 0, 0, 0, 33] + write_with_response: false + - !Messages + device_index: 0 + messages: + - !Linear + - Index: 0 + Position: 0 + Duration: 1 + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 153 ^ 230 = 127 + # Rotation: 76 ^ 77 = 1 + # Grip: 79 ^ 207 = 128 + data: [240, 201, 153, 76, 79, 249, 38, 0, 7, 230, 77, 207, 249, 38, 0, 0, 0, 0, 52] + write_with_response: false + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 72 ^ 72 = 0 + # Rotation: 90 ^ 91 = 1 + # Grip: 53 ^ 181 = 128 + data: [240, 201, 72, 90, 53, 212, 147, 0, 8, 72, 91, 181, 212, 18, 0, 0, 0, 0, 61] + write_with_response: false + - !Messages + device_index: 0 + messages: + - !Stop + - !Commands + device_index: 0 + commands: + - !Write + endpoint: tx + # Stroking: 196 ^ 196 = 0 + # Rotation: 104 ^ 232 = 128 + # Grip: 253 ^ 125 = 128 + data: [240, 201, 196, 104, 253, 133, 72, 0, 9, 196, 232, 125, 133, 72, 0, 0, 0, 0, 174] + write_with_response: false