1use serde::{Deserialize, Serialize};
2use thiserror::Error;
3
4const SNAPSHOT_VERSION: u32 = 2;
5const MIN_INERTIA_KG_M2: f64 = 1.0e-7;
6const MIN_INTEGRATION_HZ: f64 = 1_000.0;
7const MAX_INTEGRATION_HZ: f64 = 768_000.0;
8const MAX_ADVANCE_SECONDS: f64 = 10.0;
9const REST_ANGULAR_VELOCITY_RAD_S: f64 = 1.0e-6;
10const REST_TORQUE_NM: f64 = 1.0e-9;
11const STATIC_ENTRY_RATIO: f64 = 0.98;
12const MAX_EXPLICIT_STEP_RATIO: f64 = 0.5;
13
14pub const MAXIMUM_DECK_RATE: f64 = 20.0;
17pub const MAXIMUM_HAND_NORMAL_FORCE_N: f64 = 100.0;
18const FINGERTIP_GRIP_FORCE_N: f64 = 5.0;
25const PALM_GRIP_FORCE_N: f64 = 35.0;
26const STATIONARY_PRESS_FORCE_N: f64 = 15.0;
29const STATIONARY_PRESS_FADE_RATE: f64 = 0.25;
32const HAND_CATCHUP_RATE_SHARE: f64 = 0.25;
35pub const MAXIMUM_HAND_CONTACT_RADIUS_M: f64 = 0.20;
36pub const MAXIMUM_STYLUS_TORQUE_NM: f64 = 1.0;
37const MAXIMUM_DECK_SNAPSHOT_RATE: f64 = 40.0;
38const MAXIMUM_DECK_SNAPSHOT_CONTACT_TORQUE_NM: f64 = 100.0;
39
40#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
41#[serde(rename_all = "camelCase")]
42pub enum MotorMode {
43 Off,
44 Servo,
45 Brake,
46}
47
48#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
49#[serde(rename_all = "camelCase")]
50pub enum ContactMode {
51 Separated,
52 Sticking,
53 SlidingPositive,
54 SlidingNegative,
55}
56
57#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
58#[serde(rename_all = "camelCase")]
59pub struct PhysicalDeckConfig {
60 pub nominal_rpm: f64,
61 pub platter_inertia_kg_m2: f64,
62 pub record_inertia_kg_m2: f64,
63 pub motor_starting_torque_nm: f64,
64 pub motor_servo_kp_nm_per_rad_s: f64,
65 pub motor_servo_ki_nm_per_rad: f64,
66 pub motor_integral_limit_nm: f64,
67 pub motor_brake_torque_nm: f64,
68 pub motor_brake_gain_nm_per_rad_s: f64,
69 pub bearing_static_torque_nm: f64,
70 pub bearing_kinetic_torque_nm: f64,
71 pub bearing_viscous_torque_nm_per_rad_s: f64,
72 pub slipmat_static_torque_nm: f64,
73 pub slipmat_kinetic_torque_nm: f64,
74 pub slipmat_viscous_torque_nm_per_rad_s: f64,
75 pub hand_static_friction_coefficient: f64,
76 pub hand_kinetic_friction_coefficient: f64,
77 pub hand_viscous_torque_nm_per_rad_s: f64,
78 pub hand_position_stabilization_seconds: f64,
79 pub hand_max_position_correction_rad_s: f64,
80 pub integration_hz: f64,
81}
82
83impl PhysicalDeckConfig {
84 pub fn sl_1200mk7_seed() -> Self {
87 Self {
88 nominal_rpm: 33.333_333_333_333_336,
89 platter_inertia_kg_m2: 0.5 * 1.8 * 0.166 * 0.166,
92 record_inertia_kg_m2: 0.5 * 0.180 * 0.150 * 0.150,
94 motor_starting_torque_nm: 0.18,
96 motor_servo_kp_nm_per_rad_s: 0.50,
98 motor_servo_ki_nm_per_rad: 12.0,
99 motor_integral_limit_nm: 0.08,
100 motor_brake_torque_nm: 0.12,
101 motor_brake_gain_nm_per_rad_s: 0.50,
102 bearing_static_torque_nm: 0.000_24,
103 bearing_kinetic_torque_nm: 0.000_18,
104 bearing_viscous_torque_nm_per_rad_s: 0.000_12,
105 slipmat_static_torque_nm: 0.032,
107 slipmat_kinetic_torque_nm: 0.022,
108 slipmat_viscous_torque_nm_per_rad_s: 0.002,
109 hand_static_friction_coefficient: 0.75,
111 hand_kinetic_friction_coefficient: 0.55,
112 hand_viscous_torque_nm_per_rad_s: 0.002,
113 hand_position_stabilization_seconds: 0.004,
114 hand_max_position_correction_rad_s: 25.0,
115 integration_hz: 192_000.0,
116 }
117 }
118
119 pub fn high_torque_dj_seed() -> Self {
123 let mut config = Self::sl_1200mk7_seed();
124 config.platter_inertia_kg_m2 = 0.5 * 1.5 * 0.166 * 0.166;
126 config.motor_starting_torque_nm = 4.5 * 0.098_066_5;
128 config.motor_servo_kp_nm_per_rad_s = 0.80;
130 config.motor_servo_ki_nm_per_rad = 20.0;
131 config.motor_integral_limit_nm = 0.12;
132 config.motor_brake_torque_nm = 0.20;
133 config.motor_brake_gain_nm_per_rad_s = 0.80;
134 config.slipmat_static_torque_nm = 0.075;
137 config.slipmat_kinetic_torque_nm = 0.060;
138 config
139 }
140
141 pub fn validate(self) -> Result<Self, PhysicalDeckConfigError> {
142 validate_positive("nominalRpm", self.nominal_rpm)?;
143 validate_minimum(
144 "platterInertiaKgM2",
145 self.platter_inertia_kg_m2,
146 MIN_INERTIA_KG_M2,
147 )?;
148 validate_minimum(
149 "recordInertiaKgM2",
150 self.record_inertia_kg_m2,
151 MIN_INERTIA_KG_M2,
152 )?;
153 validate_nonnegative("motorStartingTorqueNm", self.motor_starting_torque_nm)?;
154 validate_nonnegative("motorServoKpNmPerRadS", self.motor_servo_kp_nm_per_rad_s)?;
155 validate_nonnegative("motorServoKiNmPerRad", self.motor_servo_ki_nm_per_rad)?;
156 validate_nonnegative("motorIntegralLimitNm", self.motor_integral_limit_nm)?;
157 validate_nonnegative("motorBrakeTorqueNm", self.motor_brake_torque_nm)?;
158 validate_nonnegative(
159 "motorBrakeGainNmPerRadS",
160 self.motor_brake_gain_nm_per_rad_s,
161 )?;
162 validate_nonnegative("bearingStaticTorqueNm", self.bearing_static_torque_nm)?;
163 validate_nonnegative("bearingKineticTorqueNm", self.bearing_kinetic_torque_nm)?;
164 validate_nonnegative(
165 "bearingViscousTorqueNmPerRadS",
166 self.bearing_viscous_torque_nm_per_rad_s,
167 )?;
168 validate_nonnegative("slipmatStaticTorqueNm", self.slipmat_static_torque_nm)?;
169 validate_nonnegative("slipmatKineticTorqueNm", self.slipmat_kinetic_torque_nm)?;
170 validate_nonnegative(
171 "slipmatViscousTorqueNmPerRadS",
172 self.slipmat_viscous_torque_nm_per_rad_s,
173 )?;
174 validate_nonnegative(
175 "handStaticFrictionCoefficient",
176 self.hand_static_friction_coefficient,
177 )?;
178 validate_nonnegative(
179 "handKineticFrictionCoefficient",
180 self.hand_kinetic_friction_coefficient,
181 )?;
182 validate_nonnegative(
183 "handViscousTorqueNmPerRadS",
184 self.hand_viscous_torque_nm_per_rad_s,
185 )?;
186 validate_positive(
187 "handPositionStabilizationSeconds",
188 self.hand_position_stabilization_seconds,
189 )?;
190 validate_nonnegative(
191 "handMaxPositionCorrectionRadS",
192 self.hand_max_position_correction_rad_s,
193 )?;
194 validate_minimum("integrationHz", self.integration_hz, MIN_INTEGRATION_HZ)?;
195 if self.integration_hz > MAX_INTEGRATION_HZ {
196 return Err(PhysicalDeckConfigError::AboveMaximum {
197 field: "integrationHz",
198 maximum: MAX_INTEGRATION_HZ,
199 });
200 }
201 if self.bearing_kinetic_torque_nm > self.bearing_static_torque_nm {
202 return Err(PhysicalDeckConfigError::KineticExceedsStatic { contact: "bearing" });
203 }
204 if self.slipmat_kinetic_torque_nm > self.slipmat_static_torque_nm {
205 return Err(PhysicalDeckConfigError::KineticExceedsStatic { contact: "slipmat" });
206 }
207 if self.hand_kinetic_friction_coefficient > self.hand_static_friction_coefficient {
208 return Err(PhysicalDeckConfigError::KineticExceedsStatic { contact: "hand" });
209 }
210
211 let dt = 1.0 / self.integration_hz;
212 validate_step_ratio(
213 "motorServoKpNmPerRadS",
214 self.motor_servo_kp_nm_per_rad_s * dt / self.platter_inertia_kg_m2,
215 )?;
216 validate_step_ratio(
217 "motorServoKiNmPerRad",
218 self.motor_servo_ki_nm_per_rad * dt * dt / self.platter_inertia_kg_m2,
219 )?;
220 validate_step_ratio(
221 "motorBrakeGainNmPerRadS",
222 self.motor_brake_gain_nm_per_rad_s * dt / self.platter_inertia_kg_m2,
223 )?;
224 validate_step_ratio(
225 "bearingViscousTorqueNmPerRadS",
226 self.bearing_viscous_torque_nm_per_rad_s * dt / self.platter_inertia_kg_m2,
227 )?;
228 validate_step_ratio(
229 "slipmatViscousTorqueNmPerRadS",
230 self.slipmat_viscous_torque_nm_per_rad_s
231 * dt
232 * (1.0 / self.platter_inertia_kg_m2 + 1.0 / self.record_inertia_kg_m2),
233 )?;
234 Ok(self)
235 }
236
237 pub fn nominal_angular_velocity_rad_s(self) -> f64 {
238 self.nominal_rpm * std::f64::consts::TAU / 60.0
239 }
240}
241
242impl Default for PhysicalDeckConfig {
243 fn default() -> Self {
244 Self::sl_1200mk7_seed()
245 }
246}
247
248#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
249#[serde(rename_all = "camelCase")]
250pub struct DeckMechanicalControl {
251 pub motor_mode: MotorMode,
252 pub motor_target_angular_velocity_rad_s: f64,
253 pub hand_contact: bool,
254 pub hand_target_angle_rad: Option<f64>,
255 pub hand_target_angular_velocity_rad_s: f64,
256 pub hand_normal_force_n: f64,
257 pub hand_contact_radius_m: f64,
258 pub stylus_torque_nm: f64,
260}
261
262#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
263#[serde(rename_all = "camelCase")]
264pub struct NormalizedDeckControl {
265 pub motor_mode: MotorMode,
266 pub motor_rate: f64,
267 pub hand_contact: bool,
268 pub hand_target_angle_turns: Option<f64>,
269 pub hand_rate: f64,
270 pub grip: f64,
271 pub stylus_torque_nm: f64,
272}
273
274impl DeckMechanicalControl {
275 pub fn from_normalized(config: PhysicalDeckConfig, input: NormalizedDeckControl) -> Self {
276 let nominal = config.nominal_angular_velocity_rad_s();
277 let normalized_grip = finite_or_zero(input.grip).clamp(0.0, 1.0);
278 Self {
279 motor_mode: input.motor_mode,
280 motor_target_angular_velocity_rad_s: finite_or_zero(input.motor_rate)
281 .clamp(-MAXIMUM_DECK_RATE, MAXIMUM_DECK_RATE)
282 * nominal,
283 hand_contact: input.hand_contact,
284 hand_target_angle_rad: input
285 .hand_target_angle_turns
286 .filter(|value| value.is_finite())
287 .map(|turns| turns * std::f64::consts::TAU),
288 hand_target_angular_velocity_rad_s: finite_or_zero(input.hand_rate)
289 .clamp(-MAXIMUM_DECK_RATE, MAXIMUM_DECK_RATE)
290 * nominal,
291 hand_normal_force_n: {
298 let stationary = (1.0
299 - finite_or_zero(input.hand_rate).abs() / STATIONARY_PRESS_FADE_RATE)
300 .clamp(0.0, 1.0);
301 normalized_grip * normalized_grip * FINGERTIP_GRIP_FORCE_N
302 + normalized_grip.powi(3) * PALM_GRIP_FORCE_N
303 + normalized_grip.powi(8) * STATIONARY_PRESS_FORCE_N * stationary
304 },
305 hand_contact_radius_m: 0.12,
306 stylus_torque_nm: finite_or_zero(input.stylus_torque_nm)
307 .clamp(-MAXIMUM_STYLUS_TORQUE_NM, MAXIMUM_STYLUS_TORQUE_NM),
308 }
309 }
310
311 fn validate(self) -> Result<Self, DeckMechanicalError> {
312 validate_control_finite(
313 "motorTargetAngularVelocityRadS",
314 self.motor_target_angular_velocity_rad_s,
315 )?;
316 if self
317 .hand_target_angle_rad
318 .is_some_and(|value| !value.is_finite())
319 {
320 return Err(DeckMechanicalError::InvalidControl {
321 field: "handTargetAngleRad",
322 });
323 }
324 validate_control_finite(
325 "handTargetAngularVelocityRadS",
326 self.hand_target_angular_velocity_rad_s,
327 )?;
328 validate_control_nonnegative("handNormalForceN", self.hand_normal_force_n)?;
329 validate_control_nonnegative("handContactRadiusM", self.hand_contact_radius_m)?;
330 validate_control_finite("stylusTorqueNm", self.stylus_torque_nm)?;
331 Ok(self)
332 }
333
334 pub fn validate_for_config(
335 self,
336 config: PhysicalDeckConfig,
337 ) -> Result<Self, DeckMechanicalError> {
338 self.validate()?;
339 let maximum_angular_velocity = MAXIMUM_DECK_RATE * config.nominal_angular_velocity_rad_s();
340 if self.motor_target_angular_velocity_rad_s.abs() > maximum_angular_velocity {
341 return Err(DeckMechanicalError::InvalidControl {
342 field: "motorTargetAngularVelocityRadS",
343 });
344 }
345 if self.hand_target_angular_velocity_rad_s.abs() > maximum_angular_velocity {
346 return Err(DeckMechanicalError::InvalidControl {
347 field: "handTargetAngularVelocityRadS",
348 });
349 }
350 if self.hand_normal_force_n > MAXIMUM_HAND_NORMAL_FORCE_N {
351 return Err(DeckMechanicalError::InvalidControl {
352 field: "handNormalForceN",
353 });
354 }
355 if self.hand_contact_radius_m > MAXIMUM_HAND_CONTACT_RADIUS_M {
356 return Err(DeckMechanicalError::InvalidControl {
357 field: "handContactRadiusM",
358 });
359 }
360 if self.stylus_torque_nm.abs() > MAXIMUM_STYLUS_TORQUE_NM {
361 return Err(DeckMechanicalError::InvalidControl {
362 field: "stylusTorqueNm",
363 });
364 }
365 Ok(self)
366 }
367}
368
369impl Default for DeckMechanicalControl {
370 fn default() -> Self {
371 Self {
372 motor_mode: MotorMode::Off,
373 motor_target_angular_velocity_rad_s: 0.0,
374 hand_contact: false,
375 hand_target_angle_rad: None,
376 hand_target_angular_velocity_rad_s: 0.0,
377 hand_normal_force_n: 0.0,
378 hand_contact_radius_m: 0.12,
379 stylus_torque_nm: 0.0,
380 }
381 }
382}
383
384#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
385#[serde(rename_all = "camelCase")]
386pub struct DeckMechanicalTelemetry {
387 pub mechanical_time_seconds: f64,
388 pub platter_rate: f64,
389 pub record_rate: f64,
390 pub platter_angle_turns: f64,
391 pub record_angle_turns: f64,
392 pub motor_torque_nm: f64,
393 pub slipmat_torque_nm: f64,
394 pub hand_torque_nm: f64,
395 pub bearing_torque_nm: f64,
396 pub stylus_torque_nm: f64,
397 pub slipmat_mode: ContactMode,
398 pub hand_mode: ContactMode,
399 pub bearing_sticking: bool,
400}
401
402#[derive(Debug, Clone, Copy, PartialEq, Serialize)]
403#[serde(rename_all = "camelCase")]
404pub struct DeckMechanicalState {
405 config: PhysicalDeckConfig,
406 platter_angle_rad: f64,
407 record_angle_rad: f64,
408 platter_angular_velocity_rad_s: f64,
409 record_angular_velocity_rad_s: f64,
410 motor_integral_torque_nm: f64,
411 integration_remainder_seconds: f64,
412 completed_steps: u64,
413 slipmat_mode: ContactMode,
414 hand_mode: ContactMode,
415 last_control: DeckMechanicalControl,
416 last_motor_torque_nm: f64,
417 last_slipmat_torque_nm: f64,
418 last_hand_torque_nm: f64,
419 last_bearing_torque_nm: f64,
420 last_stylus_torque_nm: f64,
421 bearing_sticking: bool,
422 last_telemetry: DeckMechanicalTelemetry,
423}
424
425#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
426#[serde(rename_all = "camelCase")]
427pub struct DeckMechanicalSnapshot {
428 version: u32,
429 config: PhysicalDeckConfig,
430 platter_angle_rad: f64,
431 record_angle_rad: f64,
432 platter_angular_velocity_rad_s: f64,
433 record_angular_velocity_rad_s: f64,
434 motor_integral_torque_nm: f64,
435 integration_remainder_seconds: f64,
436 completed_steps: u64,
437 slipmat_mode: ContactMode,
438 hand_mode: ContactMode,
439 last_control: DeckMechanicalControl,
440 last_motor_torque_nm: f64,
441 last_slipmat_torque_nm: f64,
442 last_hand_torque_nm: f64,
443 last_bearing_torque_nm: f64,
444 last_stylus_torque_nm: f64,
445 bearing_sticking: bool,
446}
447
448impl DeckMechanicalState {
449 pub fn new(config: PhysicalDeckConfig) -> Result<Self, PhysicalDeckConfigError> {
450 let config = config.validate()?;
451 let mut state = Self {
452 config,
453 platter_angle_rad: 0.0,
454 record_angle_rad: 0.0,
455 platter_angular_velocity_rad_s: 0.0,
456 record_angular_velocity_rad_s: 0.0,
457 motor_integral_torque_nm: 0.0,
458 integration_remainder_seconds: 0.0,
459 completed_steps: 0,
460 slipmat_mode: ContactMode::Sticking,
461 hand_mode: ContactMode::Separated,
462 last_control: DeckMechanicalControl::default(),
463 last_motor_torque_nm: 0.0,
464 last_slipmat_torque_nm: 0.0,
465 last_hand_torque_nm: 0.0,
466 last_bearing_torque_nm: 0.0,
467 last_stylus_torque_nm: 0.0,
468 bearing_sticking: true,
469 last_telemetry: zero_telemetry(),
470 };
471 state.publish_telemetry();
472 Ok(state)
473 }
474
475 pub fn config(&self) -> PhysicalDeckConfig {
476 self.config
477 }
478
479 pub fn reconfigure(
480 &mut self,
481 config: PhysicalDeckConfig,
482 ) -> Result<(), PhysicalDeckConfigError> {
483 let config = config.validate()?;
484 self.config = config;
485 self.motor_integral_torque_nm = self.motor_integral_torque_nm.clamp(
486 -config.motor_integral_limit_nm,
487 config.motor_integral_limit_nm,
488 );
489 self.integration_remainder_seconds %= 1.0 / config.integration_hz;
490 self.publish_telemetry();
491 Ok(())
492 }
493
494 pub fn reset(
495 &mut self,
496 platter_rate: f64,
497 record_rate: f64,
498 platter_angle_turns: f64,
499 record_angle_turns: f64,
500 ) -> Result<(), DeckMechanicalError> {
501 for (field, value) in [
502 ("platterRate", platter_rate),
503 ("recordRate", record_rate),
504 ("platterAngleTurns", platter_angle_turns),
505 ("recordAngleTurns", record_angle_turns),
506 ] {
507 validate_control_finite(field, value)?;
508 }
509 if platter_rate.abs() > MAXIMUM_DECK_RATE || record_rate.abs() > MAXIMUM_DECK_RATE {
510 return Err(DeckMechanicalError::InvalidControl { field: "resetRate" });
511 }
512 let nominal = self.config.nominal_angular_velocity_rad_s();
513 self.platter_angular_velocity_rad_s = platter_rate * nominal;
514 self.record_angular_velocity_rad_s = record_rate * nominal;
515 self.platter_angle_rad = platter_angle_turns * std::f64::consts::TAU;
516 self.record_angle_rad = record_angle_turns * std::f64::consts::TAU;
517 self.motor_integral_torque_nm = 0.0;
518 self.integration_remainder_seconds = 0.0;
519 self.completed_steps = 0;
520 self.slipmat_mode = ContactMode::Sticking;
521 self.hand_mode = ContactMode::Separated;
522 self.clear_last_torques();
523 self.publish_telemetry();
524 Ok(())
525 }
526
527 pub fn advance(
528 &mut self,
529 duration_seconds: f64,
530 control: DeckMechanicalControl,
531 ) -> Result<DeckMechanicalTelemetry, DeckMechanicalError> {
532 if !duration_seconds.is_finite() || duration_seconds < 0.0 {
533 return Err(DeckMechanicalError::InvalidDuration);
534 }
535 if duration_seconds > MAX_ADVANCE_SECONDS {
536 return Err(DeckMechanicalError::DurationAboveMaximum {
537 maximum: MAX_ADVANCE_SECONDS,
538 });
539 }
540 let control = control.validate_for_config(self.config)?;
541 let mut next = *self;
542 next.last_control = control;
543 next.integration_remainder_seconds += duration_seconds;
544 let dt = 1.0 / next.config.integration_hz;
545 let steps = ((next.integration_remainder_seconds / dt) + 1.0e-10).floor() as u64;
546 next.integration_remainder_seconds -= steps as f64 * dt;
547 if next.integration_remainder_seconds < 0.0 {
548 next.integration_remainder_seconds = 0.0;
549 }
550 for _ in 0..steps {
551 next.advance_step(dt, control)?;
552 }
553 next.publish_telemetry();
554 *self = next;
555 Ok(self.last_telemetry)
556 }
557
558 pub fn telemetry(&self) -> DeckMechanicalTelemetry {
559 self.last_telemetry
560 }
561
562 pub fn snapshot(&self) -> DeckMechanicalSnapshot {
563 DeckMechanicalSnapshot {
564 version: SNAPSHOT_VERSION,
565 config: self.config,
566 platter_angle_rad: self.platter_angle_rad,
567 record_angle_rad: self.record_angle_rad,
568 platter_angular_velocity_rad_s: self.platter_angular_velocity_rad_s,
569 record_angular_velocity_rad_s: self.record_angular_velocity_rad_s,
570 motor_integral_torque_nm: self.motor_integral_torque_nm,
571 integration_remainder_seconds: self.integration_remainder_seconds,
572 completed_steps: self.completed_steps,
573 slipmat_mode: self.slipmat_mode,
574 hand_mode: self.hand_mode,
575 last_control: self.last_control,
576 last_motor_torque_nm: self.last_motor_torque_nm,
577 last_slipmat_torque_nm: self.last_slipmat_torque_nm,
578 last_hand_torque_nm: self.last_hand_torque_nm,
579 last_bearing_torque_nm: self.last_bearing_torque_nm,
580 last_stylus_torque_nm: self.last_stylus_torque_nm,
581 bearing_sticking: self.bearing_sticking,
582 }
583 }
584
585 pub fn restore(&mut self, snapshot: DeckMechanicalSnapshot) -> Result<(), DeckMechanicalError> {
586 if snapshot.version != SNAPSHOT_VERSION {
587 return Err(DeckMechanicalError::UnsupportedSnapshotVersion {
588 version: snapshot.version,
589 });
590 }
591 let config = snapshot.config.validate()?;
592 snapshot.last_control.validate_for_config(config)?;
593 for (field, value) in [
594 ("platterAngleRad", snapshot.platter_angle_rad),
595 ("recordAngleRad", snapshot.record_angle_rad),
596 (
597 "platterAngularVelocityRadS",
598 snapshot.platter_angular_velocity_rad_s,
599 ),
600 (
601 "recordAngularVelocityRadS",
602 snapshot.record_angular_velocity_rad_s,
603 ),
604 ("motorIntegralTorqueNm", snapshot.motor_integral_torque_nm),
605 (
606 "integrationRemainderSeconds",
607 snapshot.integration_remainder_seconds,
608 ),
609 ("lastMotorTorqueNm", snapshot.last_motor_torque_nm),
610 ("lastSlipmatTorqueNm", snapshot.last_slipmat_torque_nm),
611 ("lastHandTorqueNm", snapshot.last_hand_torque_nm),
612 ("lastBearingTorqueNm", snapshot.last_bearing_torque_nm),
613 ("lastStylusTorqueNm", snapshot.last_stylus_torque_nm),
614 ] {
615 validate_snapshot_finite(field, value)?;
616 }
617 let dt = 1.0 / config.integration_hz;
618 let maximum_snapshot_angular_velocity =
619 MAXIMUM_DECK_SNAPSHOT_RATE * config.nominal_angular_velocity_rad_s();
620 if snapshot.integration_remainder_seconds < 0.0
621 || snapshot.integration_remainder_seconds >= dt
622 || snapshot.motor_integral_torque_nm.abs() > config.motor_integral_limit_nm
623 || snapshot.platter_angular_velocity_rad_s.abs() > maximum_snapshot_angular_velocity
624 || snapshot.record_angular_velocity_rad_s.abs() > maximum_snapshot_angular_velocity
625 || snapshot.last_motor_torque_nm.abs() > config.motor_starting_torque_nm
626 || snapshot.last_stylus_torque_nm.abs() > MAXIMUM_STYLUS_TORQUE_NM
627 || snapshot.last_slipmat_torque_nm.abs() > MAXIMUM_DECK_SNAPSHOT_CONTACT_TORQUE_NM
628 || snapshot.last_hand_torque_nm.abs() > MAXIMUM_DECK_SNAPSHOT_CONTACT_TORQUE_NM
629 || snapshot.last_bearing_torque_nm.abs() > MAXIMUM_DECK_SNAPSHOT_CONTACT_TORQUE_NM
630 {
631 return Err(DeckMechanicalError::InvalidSnapshot);
632 }
633
634 self.config = config;
635 self.platter_angle_rad = snapshot.platter_angle_rad;
636 self.record_angle_rad = snapshot.record_angle_rad;
637 self.platter_angular_velocity_rad_s = snapshot.platter_angular_velocity_rad_s;
638 self.record_angular_velocity_rad_s = snapshot.record_angular_velocity_rad_s;
639 self.motor_integral_torque_nm = snapshot.motor_integral_torque_nm;
640 self.integration_remainder_seconds = snapshot.integration_remainder_seconds;
641 self.completed_steps = snapshot.completed_steps;
642 self.slipmat_mode = snapshot.slipmat_mode;
643 self.hand_mode = snapshot.hand_mode;
644 self.last_control = snapshot.last_control;
645 self.last_motor_torque_nm = snapshot.last_motor_torque_nm;
646 self.last_slipmat_torque_nm = snapshot.last_slipmat_torque_nm;
647 self.last_hand_torque_nm = snapshot.last_hand_torque_nm;
648 self.last_bearing_torque_nm = snapshot.last_bearing_torque_nm;
649 self.last_stylus_torque_nm = snapshot.last_stylus_torque_nm;
650 self.bearing_sticking = snapshot.bearing_sticking;
651 self.publish_telemetry();
652 Ok(())
653 }
654
655 fn advance_step(
656 &mut self,
657 dt: f64,
658 control: DeckMechanicalControl,
659 ) -> Result<(), DeckMechanicalError> {
660 let config = self.config;
661 let motor_torque = self.motor_torque(control, dt);
662 let stylus_torque = control.stylus_torque_nm;
663 let previous_platter_velocity = self.platter_angular_velocity_rad_s;
664 let previous_record_velocity = self.record_angular_velocity_rad_s;
665 let contact = solve_friction_step(
666 previous_platter_velocity,
667 previous_record_velocity,
668 self.record_angle_rad,
669 motor_torque,
670 stylus_torque,
671 control,
672 config,
673 self.slipmat_mode,
674 self.hand_mode,
675 self.bearing_sticking,
676 dt,
677 )?;
678
679 self.platter_angular_velocity_rad_s = contact.platter_velocity_rad_s;
680 self.record_angular_velocity_rad_s = contact.record_velocity_rad_s;
681 self.platter_angle_rad +=
682 0.5 * (previous_platter_velocity + self.platter_angular_velocity_rad_s) * dt;
683 self.record_angle_rad +=
684 0.5 * (previous_record_velocity + self.record_angular_velocity_rad_s) * dt;
685 self.completed_steps = self
686 .completed_steps
687 .checked_add(1)
688 .ok_or(DeckMechanicalError::StepCounterOverflow)?;
689 self.slipmat_mode = contact.slipmat_mode;
690 self.hand_mode = contact.hand_mode;
691 self.last_motor_torque_nm = motor_torque;
692 self.last_slipmat_torque_nm = contact.slipmat_torque_nm;
693 self.last_hand_torque_nm = contact.hand_torque_nm;
694 self.last_bearing_torque_nm = contact.bearing_torque_nm;
695 self.last_stylus_torque_nm = stylus_torque;
696 self.bearing_sticking = contact.bearing_sticking;
697 if [
698 self.platter_angle_rad,
699 self.record_angle_rad,
700 self.platter_angular_velocity_rad_s,
701 self.record_angular_velocity_rad_s,
702 self.motor_integral_torque_nm,
703 self.last_motor_torque_nm,
704 self.last_slipmat_torque_nm,
705 self.last_hand_torque_nm,
706 self.last_bearing_torque_nm,
707 self.last_stylus_torque_nm,
708 ]
709 .into_iter()
710 .any(|value| !value.is_finite())
711 {
712 return Err(DeckMechanicalError::NumericalFailure);
713 }
714 Ok(())
715 }
716
717 fn motor_torque(&mut self, control: DeckMechanicalControl, dt: f64) -> f64 {
718 let config = self.config;
719 match control.motor_mode {
720 MotorMode::Off => {
721 self.motor_integral_torque_nm = 0.0;
722 0.0
723 }
724 MotorMode::Brake => {
725 self.motor_integral_torque_nm = 0.0;
726 (-self.platter_angular_velocity_rad_s * config.motor_brake_gain_nm_per_rad_s)
727 .clamp(-config.motor_brake_torque_nm, config.motor_brake_torque_nm)
728 }
729 MotorMode::Servo => {
730 let error = control.motor_target_angular_velocity_rad_s
731 - self.platter_angular_velocity_rad_s;
732 let feed_forward = opposing_torque(
733 -control.motor_target_angular_velocity_rad_s,
734 config.bearing_kinetic_torque_nm
735 + config.bearing_viscous_torque_nm_per_rad_s
736 * control.motor_target_angular_velocity_rad_s.abs(),
737 );
738 let candidate_integral = (self.motor_integral_torque_nm
739 + config.motor_servo_ki_nm_per_rad * error * dt)
740 .clamp(
741 -config.motor_integral_limit_nm,
742 config.motor_integral_limit_nm,
743 );
744 let candidate =
745 config.motor_servo_kp_nm_per_rad_s * error + candidate_integral + feed_forward;
746 let saturated = candidate.clamp(
747 -config.motor_starting_torque_nm,
748 config.motor_starting_torque_nm,
749 );
750 if candidate == saturated
751 || (candidate > saturated && error < 0.0)
752 || (candidate < saturated && error > 0.0)
753 {
754 self.motor_integral_torque_nm = candidate_integral;
755 }
756 (config.motor_servo_kp_nm_per_rad_s * error
757 + self.motor_integral_torque_nm
758 + feed_forward)
759 .clamp(
760 -config.motor_starting_torque_nm,
761 config.motor_starting_torque_nm,
762 )
763 }
764 }
765 }
766
767 fn clear_last_torques(&mut self) {
768 self.last_motor_torque_nm = 0.0;
769 self.last_slipmat_torque_nm = 0.0;
770 self.last_hand_torque_nm = 0.0;
771 self.last_bearing_torque_nm = 0.0;
772 self.last_stylus_torque_nm = 0.0;
773 self.bearing_sticking = false;
774 }
775
776 fn publish_telemetry(&mut self) {
777 let nominal = self.config.nominal_angular_velocity_rad_s();
778 self.last_telemetry = DeckMechanicalTelemetry {
779 mechanical_time_seconds: self.completed_steps as f64 / self.config.integration_hz,
780 platter_rate: self.platter_angular_velocity_rad_s / nominal,
781 record_rate: self.record_angular_velocity_rad_s / nominal,
782 platter_angle_turns: self.platter_angle_rad / std::f64::consts::TAU,
783 record_angle_turns: self.record_angle_rad / std::f64::consts::TAU,
784 motor_torque_nm: self.last_motor_torque_nm,
785 slipmat_torque_nm: self.last_slipmat_torque_nm,
786 hand_torque_nm: self.last_hand_torque_nm,
787 bearing_torque_nm: self.last_bearing_torque_nm,
788 stylus_torque_nm: self.last_stylus_torque_nm,
789 slipmat_mode: self.slipmat_mode,
790 hand_mode: self.hand_mode,
791 bearing_sticking: self.bearing_sticking,
792 };
793 }
794}
795
796#[derive(Debug, Clone, Copy)]
797struct ContactSolution {
798 platter_velocity_rad_s: f64,
799 record_velocity_rad_s: f64,
800 bearing_torque_nm: f64,
801 slipmat_torque_nm: f64,
802 hand_torque_nm: f64,
803 slipmat_mode: ContactMode,
804 hand_mode: ContactMode,
805 bearing_sticking: bool,
806}
807
808#[derive(Debug, Clone, Copy, PartialEq, Eq)]
809enum FrictionMode {
810 Stick,
811 SlidingPositive,
812 SlidingNegative,
813 Separated,
814}
815
816#[allow(clippy::too_many_arguments)]
817fn solve_friction_step(
818 previous_platter_velocity_rad_s: f64,
819 previous_record_velocity_rad_s: f64,
820 record_angle_rad: f64,
821 motor_torque_nm: f64,
822 stylus_torque_nm: f64,
823 control: DeckMechanicalControl,
824 config: PhysicalDeckConfig,
825 previous_slipmat_mode: ContactMode,
826 previous_hand_mode: ContactMode,
827 previous_bearing_sticking: bool,
828 dt: f64,
829) -> Result<ContactSolution, DeckMechanicalError> {
830 let slip_static_limit = entry_limit(config.slipmat_static_torque_nm, previous_slipmat_mode);
831 let hand_active = control.hand_contact
832 && control.hand_normal_force_n > 0.0
833 && control.hand_contact_radius_m > 0.0;
834 let hand_velocity = effective_hand_velocity(control, config, record_angle_rad);
835 let hand_static_limit = entry_limit(
836 config.hand_static_friction_coefficient
837 * control.hand_normal_force_n
838 * control.hand_contact_radius_m,
839 previous_hand_mode,
840 );
841 let hand_kinetic_limit = config.hand_kinetic_friction_coefficient
842 * control.hand_normal_force_n
843 * control.hand_contact_radius_m;
844 if hand_active
845 && motor_torque_nm == 0.0
846 && stylus_torque_nm == 0.0
847 && previous_platter_velocity_rad_s.abs() <= REST_ANGULAR_VELOCITY_RAD_S
848 && previous_record_velocity_rad_s.abs() <= REST_ANGULAR_VELOCITY_RAD_S
849 && hand_velocity.abs() <= REST_ANGULAR_VELOCITY_RAD_S
850 {
851 return Ok(ContactSolution {
855 platter_velocity_rad_s: 0.0,
856 record_velocity_rad_s: 0.0,
857 bearing_torque_nm: 0.0,
858 slipmat_torque_nm: 0.0,
859 hand_torque_nm: 0.0,
860 slipmat_mode: ContactMode::Sticking,
861 hand_mode: ContactMode::Sticking,
862 bearing_sticking: true,
863 });
864 }
865 let bearing_static_limit = if previous_bearing_sticking {
866 config.bearing_static_torque_nm
867 } else {
868 config.bearing_static_torque_nm * STATIC_ENTRY_RATIO
869 };
870 let bearing_modes = if previous_bearing_sticking {
871 [
872 FrictionMode::Stick,
873 FrictionMode::SlidingPositive,
874 FrictionMode::SlidingNegative,
875 ]
876 } else if previous_platter_velocity_rad_s >= 0.0 {
877 [
878 FrictionMode::SlidingPositive,
879 FrictionMode::Stick,
880 FrictionMode::SlidingNegative,
881 ]
882 } else {
883 [
884 FrictionMode::SlidingNegative,
885 FrictionMode::Stick,
886 FrictionMode::SlidingPositive,
887 ]
888 };
889 let slipmat_modes = contact_mode_order(previous_slipmat_mode);
890 let active_hand_modes = contact_mode_order(previous_hand_mode);
891 let hand_modes: &[FrictionMode] = if hand_active {
892 &active_hand_modes
893 } else {
894 &[FrictionMode::Separated]
895 };
896
897 for bearing_mode in bearing_modes {
898 for slipmat_mode in slipmat_modes {
899 for &hand_mode in hand_modes {
900 let Some(solution) = solve_friction_modes(
901 previous_platter_velocity_rad_s,
902 previous_record_velocity_rad_s,
903 motor_torque_nm,
904 stylus_torque_nm,
905 hand_velocity,
906 bearing_mode,
907 slipmat_mode,
908 hand_mode,
909 hand_kinetic_limit,
910 config,
911 dt,
912 ) else {
913 continue;
914 };
915 if mode_is_valid(
916 bearing_mode,
917 solution.platter_velocity_rad_s,
918 solution.bearing_torque_nm,
919 bearing_static_limit,
920 ) && mode_is_valid(
921 slipmat_mode,
922 solution.platter_velocity_rad_s - solution.record_velocity_rad_s,
923 solution.slipmat_torque_nm,
924 slip_static_limit,
925 ) && mode_is_valid(
926 hand_mode,
927 hand_velocity - solution.record_velocity_rad_s,
928 solution.hand_torque_nm,
929 hand_static_limit,
930 ) {
931 return Ok(solution);
932 }
933 }
934 }
935 }
936
937 Err(DeckMechanicalError::ContactSolveFailure)
938}
939
940#[allow(clippy::too_many_arguments)]
941fn solve_friction_modes(
942 previous_platter_velocity_rad_s: f64,
943 previous_record_velocity_rad_s: f64,
944 motor_torque_nm: f64,
945 stylus_torque_nm: f64,
946 hand_velocity_rad_s: f64,
947 bearing_mode: FrictionMode,
948 slipmat_mode: FrictionMode,
949 hand_mode: FrictionMode,
950 hand_kinetic_torque_nm: f64,
951 config: PhysicalDeckConfig,
952 dt: f64,
953) -> Option<ContactSolution> {
954 let mut augmented = [[0.0_f64; 6]; 5];
955 let mut variable_count = 2;
956 let platter_mass = config.platter_inertia_kg_m2 / dt;
957 let record_mass = config.record_inertia_kg_m2 / dt;
958 augmented[0][0] = platter_mass;
959 augmented[0][5] = platter_mass * previous_platter_velocity_rad_s + motor_torque_nm;
960 augmented[1][1] = record_mass;
961 augmented[1][5] = record_mass * previous_record_velocity_rad_s + stylus_torque_nm;
962
963 let mut bearing_static_column = None;
964 match bearing_mode {
965 FrictionMode::Stick => {
966 bearing_static_column = Some(variable_count);
967 augmented[0][variable_count] = -1.0;
968 variable_count += 1;
969 }
970 FrictionMode::SlidingPositive => {
971 augmented[0][0] += config.bearing_viscous_torque_nm_per_rad_s;
972 augmented[0][5] -= config.bearing_kinetic_torque_nm;
973 }
974 FrictionMode::SlidingNegative => {
975 augmented[0][0] += config.bearing_viscous_torque_nm_per_rad_s;
976 augmented[0][5] += config.bearing_kinetic_torque_nm;
977 }
978 FrictionMode::Separated => return None,
979 }
980
981 let mut slipmat_static_column = None;
982 match slipmat_mode {
983 FrictionMode::Stick => {
984 slipmat_static_column = Some(variable_count);
985 augmented[0][variable_count] = 1.0;
986 augmented[1][variable_count] = -1.0;
987 variable_count += 1;
988 }
989 FrictionMode::SlidingPositive | FrictionMode::SlidingNegative => {
990 let bias = if slipmat_mode == FrictionMode::SlidingPositive {
991 config.slipmat_kinetic_torque_nm
992 } else {
993 -config.slipmat_kinetic_torque_nm
994 };
995 let damping = config.slipmat_viscous_torque_nm_per_rad_s;
996 augmented[0][0] += damping;
997 augmented[0][1] -= damping;
998 augmented[0][5] -= bias;
999 augmented[1][0] -= damping;
1000 augmented[1][1] += damping;
1001 augmented[1][5] += bias;
1002 }
1003 FrictionMode::Separated => return None,
1004 }
1005
1006 let mut hand_static_column = None;
1007 match hand_mode {
1008 FrictionMode::Stick => {
1009 hand_static_column = Some(variable_count);
1010 augmented[1][variable_count] = -1.0;
1011 variable_count += 1;
1012 }
1013 FrictionMode::SlidingPositive | FrictionMode::SlidingNegative => {
1014 let bias = if hand_mode == FrictionMode::SlidingPositive {
1015 hand_kinetic_torque_nm
1016 } else {
1017 -hand_kinetic_torque_nm
1018 };
1019 let damping = config.hand_viscous_torque_nm_per_rad_s;
1020 augmented[1][1] += damping;
1021 augmented[1][5] += bias + damping * hand_velocity_rad_s;
1022 }
1023 FrictionMode::Separated => {}
1024 }
1025
1026 if let Some(column) = bearing_static_column {
1027 let row = 2;
1028 augmented[row][0] = 1.0;
1029 augmented[row][5] = 0.0;
1030 debug_assert_eq!(column, 2);
1031 }
1032 if let Some(column) = slipmat_static_column {
1033 let row = column;
1034 augmented[row][0] = 1.0;
1035 augmented[row][1] = -1.0;
1036 augmented[row][5] = 0.0;
1037 }
1038 if let Some(column) = hand_static_column {
1039 let row = column;
1040 augmented[row][1] = 1.0;
1041 augmented[row][5] = hand_velocity_rad_s;
1042 }
1043
1044 let solution = solve_contact_linear_system(&mut augmented, variable_count)?;
1045 let platter_velocity_rad_s = solution[0];
1046 let record_velocity_rad_s = solution[1];
1047 let bearing_torque_nm = bearing_static_column.map_or_else(
1048 || {
1049 let sign = if bearing_mode == FrictionMode::SlidingPositive {
1050 -1.0
1051 } else {
1052 1.0
1053 };
1054 sign * config.bearing_kinetic_torque_nm
1055 - config.bearing_viscous_torque_nm_per_rad_s * platter_velocity_rad_s
1056 },
1057 |column| solution[column],
1058 );
1059 let slipmat_torque_nm = slipmat_static_column.map_or_else(
1060 || {
1061 let sign = if slipmat_mode == FrictionMode::SlidingPositive {
1062 1.0
1063 } else {
1064 -1.0
1065 };
1066 sign * config.slipmat_kinetic_torque_nm
1067 + config.slipmat_viscous_torque_nm_per_rad_s
1068 * (platter_velocity_rad_s - record_velocity_rad_s)
1069 },
1070 |column| solution[column],
1071 );
1072 let hand_torque_nm = match hand_mode {
1073 FrictionMode::Separated => 0.0,
1074 FrictionMode::Stick => solution[hand_static_column?],
1075 FrictionMode::SlidingPositive | FrictionMode::SlidingNegative => {
1076 let sign = if hand_mode == FrictionMode::SlidingPositive {
1077 1.0
1078 } else {
1079 -1.0
1080 };
1081 sign * hand_kinetic_torque_nm
1082 + config.hand_viscous_torque_nm_per_rad_s
1083 * (hand_velocity_rad_s - record_velocity_rad_s)
1084 }
1085 };
1086 Some(ContactSolution {
1087 platter_velocity_rad_s,
1088 record_velocity_rad_s,
1089 bearing_torque_nm,
1090 slipmat_torque_nm,
1091 hand_torque_nm,
1092 slipmat_mode: contact_mode(slipmat_mode),
1093 hand_mode: contact_mode(hand_mode),
1094 bearing_sticking: bearing_mode == FrictionMode::Stick,
1095 })
1096}
1097
1098fn effective_hand_velocity(
1099 control: DeckMechanicalControl,
1100 config: PhysicalDeckConfig,
1101 record_angle_rad: f64,
1102) -> f64 {
1103 let position_correction = control.hand_target_angle_rad.map_or(0.0, |target| {
1104 let correction_limit = config.hand_max_position_correction_rad_s.max(
1108 HAND_CATCHUP_RATE_SHARE * control.hand_target_angular_velocity_rad_s.abs(),
1109 );
1110 ((target - record_angle_rad) / config.hand_position_stabilization_seconds)
1111 .clamp(-correction_limit, correction_limit)
1112 });
1113 let maximum_velocity = MAXIMUM_DECK_RATE * config.nominal_angular_velocity_rad_s();
1114 (control.hand_target_angular_velocity_rad_s + position_correction)
1115 .clamp(-maximum_velocity, maximum_velocity)
1116}
1117
1118fn solve_contact_linear_system(augmented: &mut [[f64; 6]; 5], size: usize) -> Option<[f64; 5]> {
1119 for pivot_column in 0..size {
1120 let pivot_row = (pivot_column..size).max_by(|left, right| {
1121 augmented[*left][pivot_column]
1122 .abs()
1123 .total_cmp(&augmented[*right][pivot_column].abs())
1124 })?;
1125 let pivot = augmented[pivot_row][pivot_column];
1126 if !pivot.is_finite() || pivot.abs() < 1.0e-18 {
1127 return None;
1128 }
1129 augmented.swap(pivot_column, pivot_row);
1130 for value in &mut augmented[pivot_column][pivot_column..size] {
1131 *value /= pivot;
1132 }
1133 augmented[pivot_column][5] /= pivot;
1134 let pivot_values = augmented[pivot_column];
1135 for (row, augmented_row) in augmented.iter_mut().enumerate().take(size) {
1136 if row == pivot_column {
1137 continue;
1138 }
1139 let scale = augmented_row[pivot_column];
1140 for (value, pivot_value) in augmented_row[pivot_column..size]
1141 .iter_mut()
1142 .zip(&pivot_values[pivot_column..size])
1143 {
1144 *value -= scale * pivot_value;
1145 }
1146 augmented_row[5] -= scale * pivot_values[5];
1147 }
1148 }
1149 let mut solution = [0.0; 5];
1150 for row in 0..size {
1151 solution[row] = augmented[row][5];
1152 }
1153 solution
1154 .iter()
1155 .take(size)
1156 .all(|value| value.is_finite())
1157 .then_some(solution)
1158}
1159
1160fn mode_is_valid(
1161 mode: FrictionMode,
1162 relative_velocity_rad_s: f64,
1163 torque_nm: f64,
1164 static_limit_nm: f64,
1165) -> bool {
1166 if !relative_velocity_rad_s.is_finite() || !torque_nm.is_finite() {
1167 return false;
1168 }
1169 match mode {
1170 FrictionMode::Stick => {
1171 relative_velocity_rad_s.abs() <= REST_ANGULAR_VELOCITY_RAD_S
1172 && torque_nm.abs() <= static_limit_nm + REST_TORQUE_NM
1173 }
1174 FrictionMode::SlidingPositive => relative_velocity_rad_s > 0.0,
1175 FrictionMode::SlidingNegative => relative_velocity_rad_s < 0.0,
1176 FrictionMode::Separated => torque_nm == 0.0,
1177 }
1178}
1179
1180fn contact_mode(mode: FrictionMode) -> ContactMode {
1181 match mode {
1182 FrictionMode::Stick => ContactMode::Sticking,
1183 FrictionMode::SlidingPositive => ContactMode::SlidingPositive,
1184 FrictionMode::SlidingNegative => ContactMode::SlidingNegative,
1185 FrictionMode::Separated => ContactMode::Separated,
1186 }
1187}
1188
1189fn contact_mode_order(previous: ContactMode) -> [FrictionMode; 3] {
1190 match previous {
1191 ContactMode::Sticking | ContactMode::Separated => [
1192 FrictionMode::Stick,
1193 FrictionMode::SlidingPositive,
1194 FrictionMode::SlidingNegative,
1195 ],
1196 ContactMode::SlidingPositive => [
1197 FrictionMode::SlidingPositive,
1198 FrictionMode::Stick,
1199 FrictionMode::SlidingNegative,
1200 ],
1201 ContactMode::SlidingNegative => [
1202 FrictionMode::SlidingNegative,
1203 FrictionMode::Stick,
1204 FrictionMode::SlidingPositive,
1205 ],
1206 }
1207}
1208
1209fn opposing_torque(angular_velocity: f64, magnitude: f64) -> f64 {
1210 if angular_velocity.abs() <= REST_ANGULAR_VELOCITY_RAD_S {
1211 0.0
1212 } else {
1213 -angular_velocity.signum() * magnitude
1214 }
1215}
1216
1217fn entry_limit(static_limit: f64, mode: ContactMode) -> f64 {
1218 if mode == ContactMode::Sticking {
1219 static_limit
1220 } else {
1221 static_limit * STATIC_ENTRY_RATIO
1222 }
1223}
1224
1225#[derive(Debug, Clone, Copy, PartialEq, Error)]
1226pub enum PhysicalDeckConfigError {
1227 #[error("{field} must be finite and nonnegative")]
1228 InvalidNonnegative { field: &'static str },
1229 #[error("{field} must be finite and positive")]
1230 InvalidPositive { field: &'static str },
1231 #[error("{field} is below its minimum {minimum}")]
1232 BelowMinimum { field: &'static str, minimum: f64 },
1233 #[error("{field} is above its maximum {maximum}")]
1234 AboveMaximum { field: &'static str, maximum: f64 },
1235 #[error("{contact} kinetic friction exceeds static friction")]
1236 KineticExceedsStatic { contact: &'static str },
1237 #[error("{field} is unstable at the configured integration rate")]
1238 UnstableIntegration { field: &'static str },
1239}
1240
1241#[derive(Debug, Clone, Copy, PartialEq, Error)]
1242pub enum DeckMechanicalError {
1243 #[error(transparent)]
1244 InvalidConfig(#[from] PhysicalDeckConfigError),
1245 #[error("duration must be finite and nonnegative")]
1246 InvalidDuration,
1247 #[error("duration exceeds {maximum} seconds")]
1248 DurationAboveMaximum { maximum: f64 },
1249 #[error("the midpoint solver requires exactly one deck integration step")]
1250 MidpointStepRequiresOneIntegrationStep,
1251 #[error("control field {field} is invalid")]
1252 InvalidControl { field: &'static str },
1253 #[error("snapshot field {field} is invalid")]
1254 InvalidSnapshotField { field: &'static str },
1255 #[error("snapshot state is inconsistent")]
1256 InvalidSnapshot,
1257 #[error("snapshot version {version} is unsupported")]
1258 UnsupportedSnapshotVersion { version: u32 },
1259 #[error("the deck friction constraints have no consistent solution")]
1260 ContactSolveFailure,
1261 #[error("the deck step counter exceeded its supported range")]
1262 StepCounterOverflow,
1263 #[error("the deck integration produced a nonfinite value")]
1264 NumericalFailure,
1265}
1266
1267fn validate_nonnegative(field: &'static str, value: f64) -> Result<(), PhysicalDeckConfigError> {
1268 if value.is_finite() && value >= 0.0 {
1269 Ok(())
1270 } else {
1271 Err(PhysicalDeckConfigError::InvalidNonnegative { field })
1272 }
1273}
1274
1275fn validate_positive(field: &'static str, value: f64) -> Result<(), PhysicalDeckConfigError> {
1276 if value.is_finite() && value > 0.0 {
1277 Ok(())
1278 } else {
1279 Err(PhysicalDeckConfigError::InvalidPositive { field })
1280 }
1281}
1282
1283fn validate_minimum(
1284 field: &'static str,
1285 value: f64,
1286 minimum: f64,
1287) -> Result<(), PhysicalDeckConfigError> {
1288 validate_positive(field, value)?;
1289 if value < minimum {
1290 Err(PhysicalDeckConfigError::BelowMinimum { field, minimum })
1291 } else {
1292 Ok(())
1293 }
1294}
1295
1296fn validate_step_ratio(field: &'static str, ratio: f64) -> Result<(), PhysicalDeckConfigError> {
1297 if ratio.is_finite() && ratio <= MAX_EXPLICIT_STEP_RATIO {
1298 Ok(())
1299 } else {
1300 Err(PhysicalDeckConfigError::UnstableIntegration { field })
1301 }
1302}
1303
1304fn validate_control_finite(field: &'static str, value: f64) -> Result<(), DeckMechanicalError> {
1305 if value.is_finite() {
1306 Ok(())
1307 } else {
1308 Err(DeckMechanicalError::InvalidControl { field })
1309 }
1310}
1311
1312fn validate_control_nonnegative(
1313 field: &'static str,
1314 value: f64,
1315) -> Result<(), DeckMechanicalError> {
1316 if value.is_finite() && value >= 0.0 {
1317 Ok(())
1318 } else {
1319 Err(DeckMechanicalError::InvalidControl { field })
1320 }
1321}
1322
1323fn validate_snapshot_finite(field: &'static str, value: f64) -> Result<(), DeckMechanicalError> {
1324 if value.is_finite() {
1325 Ok(())
1326 } else {
1327 Err(DeckMechanicalError::InvalidSnapshotField { field })
1328 }
1329}
1330
1331fn finite_or_zero(value: f64) -> f64 {
1332 if value.is_finite() {
1333 value
1334 } else {
1335 0.0
1336 }
1337}
1338
1339fn zero_telemetry() -> DeckMechanicalTelemetry {
1340 DeckMechanicalTelemetry {
1341 mechanical_time_seconds: 0.0,
1342 platter_rate: 0.0,
1343 record_rate: 0.0,
1344 platter_angle_turns: 0.0,
1345 record_angle_turns: 0.0,
1346 motor_torque_nm: 0.0,
1347 slipmat_torque_nm: 0.0,
1348 hand_torque_nm: 0.0,
1349 bearing_torque_nm: 0.0,
1350 stylus_torque_nm: 0.0,
1351 slipmat_mode: ContactMode::Sticking,
1352 hand_mode: ContactMode::Separated,
1353 bearing_sticking: true,
1354 }
1355}
1356
1357#[cfg(test)]
1358mod tests {
1359 use super::*;
1360
1361 fn running_motor(config: PhysicalDeckConfig, rate: f64) -> DeckMechanicalControl {
1362 DeckMechanicalControl::from_normalized(
1363 config,
1364 NormalizedDeckControl {
1365 motor_mode: MotorMode::Servo,
1366 motor_rate: rate,
1367 hand_contact: false,
1368 hand_target_angle_turns: None,
1369 hand_rate: 0.0,
1370 grip: 0.0,
1371 stylus_torque_nm: 0.0,
1372 },
1373 )
1374 }
1375
1376 fn scratching(
1377 config: PhysicalDeckConfig,
1378 motor_rate: f64,
1379 hand_rate: f64,
1380 normal_force_n: f64,
1381 ) -> DeckMechanicalControl {
1382 let mut control = DeckMechanicalControl::from_normalized(
1383 config,
1384 NormalizedDeckControl {
1385 motor_mode: MotorMode::Servo,
1386 motor_rate,
1387 hand_contact: true,
1388 hand_target_angle_turns: None,
1389 hand_rate,
1390 grip: 1.0,
1391 stylus_torque_nm: 0.0,
1392 },
1393 );
1394 control.hand_normal_force_n = normal_force_n;
1395 control
1396 }
1397
1398 fn advance_seconds(
1399 state: &mut DeckMechanicalState,
1400 seconds: f64,
1401 control: DeckMechanicalControl,
1402 ) -> DeckMechanicalTelemetry {
1403 let mut remaining = seconds;
1404 while remaining > 0.0 {
1405 let duration = remaining.min(0.01);
1406 state.advance(duration, control).unwrap();
1407 remaining -= duration;
1408 }
1409 state.telemetry()
1410 }
1411
1412 #[test]
1413 fn seed_uses_published_deck_scale_without_claiming_calibration() {
1414 let config = PhysicalDeckConfig::default();
1415 assert!((config.motor_starting_torque_nm - 0.18).abs() < f64::EPSILON);
1416 assert!((config.platter_inertia_kg_m2 - 0.024_800_4).abs() < 1.0e-7);
1417 assert!((config.nominal_angular_velocity_rad_s() - 3.490_658_503_988_659).abs() < 1.0e-12);
1418 assert!(config.validate().is_ok());
1419 }
1420
1421 #[test]
1422 fn motor_reaches_nominal_speed_within_the_published_start_time() {
1423 let config = PhysicalDeckConfig::default();
1424 let mut state = DeckMechanicalState::new(config).unwrap();
1425 let at_700_ms = advance_seconds(&mut state, 0.7, running_motor(config, 1.0));
1426 assert!(at_700_ms.platter_rate > 0.99, "{}", at_700_ms.platter_rate);
1427 assert!(at_700_ms.record_rate > 0.99, "{}", at_700_ms.record_rate);
1428 assert!((at_700_ms.platter_rate - at_700_ms.record_rate).abs() < 1.0e-9);
1429 }
1430
1431 #[test]
1432 fn high_torque_seed_reaches_nominal_speed_within_two_hundred_ms() {
1433 let config = PhysicalDeckConfig::high_torque_dj_seed();
1434 let mut state = DeckMechanicalState::new(config).unwrap();
1435 let at_200_ms = advance_seconds(&mut state, 0.2, running_motor(config, 1.0));
1436 assert!(at_200_ms.platter_rate > 0.99, "{at_200_ms:?}");
1437 assert!(at_200_ms.record_rate > 0.99, "{}", at_200_ms.record_rate);
1438 assert!(at_200_ms.platter_rate < 1.015, "{}", at_200_ms.platter_rate);
1439 }
1440
1441 #[test]
1442 fn pi_servo_removes_steady_load_droop() {
1443 let config = PhysicalDeckConfig::default();
1444 let mut state = DeckMechanicalState::new(config).unwrap();
1445 let mut control = running_motor(config, 1.0);
1446 control.stylus_torque_nm = -0.000_4;
1447 let settled = advance_seconds(&mut state, 3.0, control);
1448 assert!(
1449 (settled.platter_rate - 1.0).abs() < 1.0e-5,
1450 "{}",
1451 settled.platter_rate
1452 );
1453 }
1454
1455 #[test]
1456 fn firm_hand_separates_and_reverses_record_over_platter() {
1457 let config = PhysicalDeckConfig::default();
1458 let mut state = DeckMechanicalState::new(config).unwrap();
1459 advance_seconds(&mut state, 1.0, running_motor(config, 1.0));
1460 let grabbed = advance_seconds(&mut state, 0.045, scratching(config, 1.0, -1.0, 5.0));
1461 assert!(grabbed.record_rate < -0.75, "{}", grabbed.record_rate);
1462 assert!(grabbed.platter_rate > 0.75, "{}", grabbed.platter_rate);
1463 assert_ne!(grabbed.slipmat_mode, ContactMode::Sticking);
1464 }
1465
1466 #[test]
1467 fn releasing_hand_recouples_without_resetting_phase() {
1468 let config = PhysicalDeckConfig::default();
1469 let mut state = DeckMechanicalState::new(config).unwrap();
1470 advance_seconds(&mut state, 1.0, running_motor(config, 1.0));
1471 advance_seconds(&mut state, 0.04, scratching(config, 1.0, -0.8, 5.0));
1472 let phase_before = state.telemetry().record_angle_turns;
1473 let caught = advance_seconds(&mut state, 0.5, running_motor(config, 1.0));
1474 assert!(caught.record_rate > 0.99, "{}", caught.record_rate);
1475 assert!((caught.platter_rate - caught.record_rate).abs() < 1.0e-9);
1476 assert_ne!(caught.record_angle_turns, phase_before);
1477 }
1478
1479 #[test]
1480 fn simultaneous_static_constraints_produce_equal_velocities() {
1481 let config = PhysicalDeckConfig::default();
1482 let mut state = DeckMechanicalState::new(config).unwrap();
1483 state.reset(1.0, 1.0, 0.0, 0.0).unwrap();
1484 let control = scratching(config, 1.0, 1.0, 5.0);
1485 let telemetry = state.advance(1.0 / config.integration_hz, control).unwrap();
1486 assert_eq!(telemetry.slipmat_mode, ContactMode::Sticking);
1487 assert_eq!(telemetry.hand_mode, ContactMode::Sticking);
1488 assert_eq!(telemetry.platter_rate, telemetry.record_rate);
1489 }
1490
1491 #[test]
1492 fn stationary_hand_holds_an_unpowered_stopped_deck_without_rank_failure() {
1493 let mut config = PhysicalDeckConfig::high_torque_dj_seed();
1494 config.nominal_rpm = 45.0;
1495 config.integration_hz = 48_000.0;
1496 let mut state = DeckMechanicalState::new(config).unwrap();
1497 let turns = 61.250_890_548_885_84;
1498 let residual_rate = -2.246_824_675_286_976e-23;
1499 state
1500 .reset(residual_rate, residual_rate, turns, turns)
1501 .unwrap();
1502 let control = DeckMechanicalControl::from_normalized(
1503 config,
1504 NormalizedDeckControl {
1505 motor_mode: MotorMode::Off,
1506 motor_rate: 0.0,
1507 hand_contact: true,
1508 hand_target_angle_turns: Some(turns),
1509 hand_rate: 0.0,
1510 grip: 0.988_256_371_542_977_2,
1511 stylus_torque_nm: 0.0,
1512 },
1513 );
1514
1515 for _ in 0..12_000 {
1516 let telemetry = state.advance(1.0 / 48_000.0, control).unwrap();
1517 assert_eq!(telemetry.platter_rate, 0.0);
1518 assert_eq!(telemetry.record_rate, 0.0);
1519 assert_eq!(telemetry.slipmat_mode, ContactMode::Sticking);
1520 assert_eq!(telemetry.hand_mode, ContactMode::Sticking);
1521 assert!(telemetry.bearing_sticking);
1522 }
1523 }
1524
1525 #[test]
1526 fn bearing_and_slipmat_balance_stylus_torque_in_the_same_sample() {
1527 let config = PhysicalDeckConfig::default();
1528 let mut state = DeckMechanicalState::new(config).unwrap();
1529 let control = DeckMechanicalControl {
1530 stylus_torque_nm: 0.000_1,
1531 ..DeckMechanicalControl::default()
1532 };
1533 let telemetry = state.advance(1.0 / config.integration_hz, control).unwrap();
1534 assert_eq!(telemetry.platter_rate, 0.0);
1535 assert_eq!(telemetry.record_rate, 0.0);
1536 assert_eq!(telemetry.slipmat_mode, ContactMode::Sticking);
1537 assert!(telemetry.bearing_sticking);
1538 assert!((telemetry.slipmat_torque_nm + control.stylus_torque_nm).abs() < 1.0e-15);
1539 assert!((telemetry.bearing_torque_nm - telemetry.slipmat_torque_nm).abs() < 1.0e-15);
1540 }
1541
1542 #[test]
1543 fn kinetic_bearing_friction_stops_without_reversing_at_zero() {
1544 let config = PhysicalDeckConfig {
1545 integration_hz: 1_000.0,
1546 platter_inertia_kg_m2: MIN_INERTIA_KG_M2,
1547 record_inertia_kg_m2: MIN_INERTIA_KG_M2,
1548 motor_servo_kp_nm_per_rad_s: 0.0,
1549 motor_servo_ki_nm_per_rad: 0.0,
1550 motor_brake_gain_nm_per_rad_s: 0.0,
1551 bearing_viscous_torque_nm_per_rad_s: 0.0,
1552 slipmat_viscous_torque_nm_per_rad_s: 0.0,
1553 ..PhysicalDeckConfig::default()
1554 };
1555 let mut state = DeckMechanicalState::new(config).unwrap();
1556 let initial_velocity = 2.0 * REST_ANGULAR_VELOCITY_RAD_S;
1557 let initial_rate = initial_velocity / config.nominal_angular_velocity_rad_s();
1558 state.reset(initial_rate, initial_rate, 0.0, 0.0).unwrap();
1559 let telemetry = state
1560 .advance(
1561 1.0 / config.integration_hz,
1562 DeckMechanicalControl::default(),
1563 )
1564 .unwrap();
1565 assert_eq!(telemetry.platter_rate, 0.0);
1566 assert_eq!(telemetry.record_rate, 0.0);
1567 assert!(telemetry.bearing_sticking);
1568 }
1569
1570 #[test]
1571 fn angle_uses_the_interval_average_velocity_under_constant_torque() {
1572 let config = PhysicalDeckConfig {
1573 bearing_static_torque_nm: 0.0,
1574 bearing_kinetic_torque_nm: 0.0,
1575 bearing_viscous_torque_nm_per_rad_s: 0.0,
1576 slipmat_static_torque_nm: 0.0,
1577 slipmat_kinetic_torque_nm: 0.0,
1578 slipmat_viscous_torque_nm_per_rad_s: 0.0,
1579 ..PhysicalDeckConfig::default()
1580 };
1581 let mut state = DeckMechanicalState::new(config).unwrap();
1582 let control = DeckMechanicalControl {
1583 stylus_torque_nm: 0.01,
1584 ..DeckMechanicalControl::default()
1585 };
1586 let dt = 1.0 / config.integration_hz;
1587 let telemetry = state.advance(dt, control).unwrap();
1588 let expected_velocity = control.stylus_torque_nm / config.record_inertia_kg_m2 * dt;
1589 let expected_angle = 0.5 * expected_velocity * dt;
1590 assert!((state.record_angular_velocity_rad_s - expected_velocity).abs() < 1.0e-15);
1591 assert!(
1592 (telemetry.record_angle_turns * std::f64::consts::TAU - expected_angle).abs() < 1.0e-18
1593 );
1594 }
1595
1596 #[test]
1597 fn every_friction_port_opposes_relative_motion_during_rapid_reversals() {
1598 let config = PhysicalDeckConfig::default();
1599 let mut state = DeckMechanicalState::new(config).unwrap();
1600 state.reset(1.0, 1.0, 0.0, 0.0).unwrap();
1601 let dt = 1.0 / config.integration_hz;
1602 for sample in 0..20_000 {
1603 let hand_rate = if sample / 61 % 2 == 0 {
1604 MAXIMUM_DECK_RATE
1605 } else {
1606 -MAXIMUM_DECK_RATE
1607 };
1608 let control = scratching(config, 0.0, hand_rate, 100.0);
1609 let hand_velocity = effective_hand_velocity(control, config, state.record_angle_rad);
1610 let telemetry = state.advance(dt, control).unwrap();
1611 let platter_velocity = telemetry.platter_rate * config.nominal_angular_velocity_rad_s();
1612 let record_velocity = telemetry.record_rate * config.nominal_angular_velocity_rad_s();
1613 let bearing_power = telemetry.bearing_torque_nm * platter_velocity;
1614 let slipmat_power = telemetry.slipmat_torque_nm * (record_velocity - platter_velocity);
1615 let hand_friction_power = telemetry.hand_torque_nm * (record_velocity - hand_velocity);
1616 assert!(bearing_power <= 1.0e-12, "{sample}: {bearing_power}");
1617 assert!(slipmat_power <= 1.0e-12, "{sample}: {slipmat_power}");
1618 assert!(
1619 hand_friction_power <= 1.0e-12,
1620 "{sample}: {hand_friction_power}"
1621 );
1622 assert!(telemetry.platter_rate.is_finite());
1623 assert!(telemetry.record_rate.is_finite());
1624 }
1625 }
1626
1627 #[test]
1628 fn step_counter_overflow_does_not_mutate_the_deck() {
1629 let config = PhysicalDeckConfig::default();
1630 let mut state = DeckMechanicalState::new(config).unwrap();
1631 let mut snapshot = state.snapshot();
1632 snapshot.completed_steps = u64::MAX;
1633 state.restore(snapshot).unwrap();
1634 let before = state;
1635
1636 let error = state
1637 .advance(
1638 1.0 / config.integration_hz,
1639 DeckMechanicalControl::default(),
1640 )
1641 .unwrap_err();
1642
1643 assert_eq!(error, DeckMechanicalError::StepCounterOverflow);
1644 assert_eq!(state, before);
1645 }
1646
1647 #[test]
1648 fn hand_position_correction_cannot_exceed_the_deck_rate_limit() {
1649 let config = PhysicalDeckConfig::default();
1650 let maximum_velocity = MAXIMUM_DECK_RATE * config.nominal_angular_velocity_rad_s();
1651 let mut forward = scratching(config, 0.0, MAXIMUM_DECK_RATE, 5.0);
1652 forward.hand_target_angle_rad = Some(10.0);
1653 assert_eq!(
1654 effective_hand_velocity(forward, config, 0.0),
1655 maximum_velocity
1656 );
1657
1658 let mut reverse = scratching(config, 0.0, -MAXIMUM_DECK_RATE, 5.0);
1659 reverse.hand_target_angle_rad = Some(-10.0);
1660 assert_eq!(
1661 effective_hand_velocity(reverse, config, 0.0),
1662 -maximum_velocity
1663 );
1664 }
1665
1666 #[test]
1667 fn internal_slipmat_torque_conserves_angular_momentum() {
1668 let config = PhysicalDeckConfig {
1669 motor_starting_torque_nm: 0.0,
1670 motor_brake_torque_nm: 0.0,
1671 bearing_static_torque_nm: 0.0,
1672 bearing_kinetic_torque_nm: 0.0,
1673 bearing_viscous_torque_nm_per_rad_s: 0.0,
1674 ..PhysicalDeckConfig::default()
1675 };
1676 let mut state = DeckMechanicalState::new(config).unwrap();
1677 state.reset(1.0, -1.0, 0.0, 0.0).unwrap();
1678 let before = config.platter_inertia_kg_m2 * state.platter_angular_velocity_rad_s
1679 + config.record_inertia_kg_m2 * state.record_angular_velocity_rad_s;
1680 state
1681 .advance(0.25, DeckMechanicalControl::default())
1682 .unwrap();
1683 let after = config.platter_inertia_kg_m2 * state.platter_angular_velocity_rad_s
1684 + config.record_inertia_kg_m2 * state.record_angular_velocity_rad_s;
1685 assert!((after - before).abs() < 1.0e-10, "{before} -> {after}");
1686 }
1687
1688 #[test]
1689 fn friction_never_increases_isolated_mechanical_energy() {
1690 let config = PhysicalDeckConfig {
1691 motor_starting_torque_nm: 0.0,
1692 motor_brake_torque_nm: 0.0,
1693 ..PhysicalDeckConfig::default()
1694 };
1695 let mut state = DeckMechanicalState::new(config).unwrap();
1696 state.reset(1.0, -1.0, 0.0, 0.0).unwrap();
1697 let energy = |state: &DeckMechanicalState| {
1698 0.5 * config.platter_inertia_kg_m2 * state.platter_angular_velocity_rad_s.powi(2)
1699 + 0.5 * config.record_inertia_kg_m2 * state.record_angular_velocity_rad_s.powi(2)
1700 };
1701 let before = energy(&state);
1702 state
1703 .advance(0.5, DeckMechanicalControl::default())
1704 .unwrap();
1705 assert!(energy(&state) <= before + 1.0e-12);
1706 }
1707
1708 #[test]
1709 fn fixed_clock_is_invariant_to_call_partitioning() {
1710 let config = PhysicalDeckConfig::default();
1711 let control = running_motor(config, 1.0);
1712 let mut whole = DeckMechanicalState::new(config).unwrap();
1713 let mut partitioned = DeckMechanicalState::new(config).unwrap();
1714 whole.advance(0.731, control).unwrap();
1715 for duration in [0.001, 0.017, 0.000_3, 0.2, 0.111, 0.401_7] {
1716 partitioned.advance(duration, control).unwrap();
1717 }
1718 let whole = whole.telemetry();
1719 let partitioned = partitioned.telemetry();
1720 assert_eq!(
1721 whole.mechanical_time_seconds,
1722 partitioned.mechanical_time_seconds
1723 );
1724 assert!((whole.platter_rate - partitioned.platter_rate).abs() < 1.0e-12);
1725 assert!((whole.record_angle_turns - partitioned.record_angle_turns).abs() < 1.0e-12);
1726 }
1727
1728 #[test]
1729 fn two_seconds_in_one_call_matches_two_one_second_calls() {
1730 let config = PhysicalDeckConfig::default();
1731 let control = running_motor(config, 1.0);
1732 let mut whole = DeckMechanicalState::new(config).unwrap();
1733 let mut split = DeckMechanicalState::new(config).unwrap();
1734 whole.advance(2.0, control).unwrap();
1735 split.advance(1.0, control).unwrap();
1736 split.advance(1.0, control).unwrap();
1737 assert_eq!(whole.snapshot(), split.snapshot());
1738 }
1739
1740 #[test]
1741 fn invalid_control_does_not_change_valid_state() {
1742 let config = PhysicalDeckConfig::default();
1743 let mut state = DeckMechanicalState::new(config).unwrap();
1744 state.advance(0.1, running_motor(config, 1.0)).unwrap();
1745 let before = state.snapshot();
1746 let mut invalid = running_motor(config, 1.0);
1747 invalid.hand_normal_force_n = f64::NAN;
1748 assert!(state.advance(0.1, invalid).is_err());
1749 assert_eq!(state.snapshot(), before);
1750 }
1751
1752 #[test]
1753 fn engineering_control_limits_reject_numeric_hazards_without_mutation() {
1754 let config = PhysicalDeckConfig::default();
1755 let mut state = DeckMechanicalState::new(config).unwrap();
1756 let before = state.snapshot();
1757 let mut invalid = running_motor(config, 1.0);
1758 invalid.motor_target_angular_velocity_rad_s =
1759 (MAXIMUM_DECK_RATE + 1.0) * config.nominal_angular_velocity_rad_s();
1760 invalid.hand_normal_force_n = MAXIMUM_HAND_NORMAL_FORCE_N + 1.0;
1761 assert!(state.advance(1.0 / config.integration_hz, invalid).is_err());
1762 assert_eq!(state.snapshot(), before);
1763 assert!(state.reset(MAXIMUM_DECK_RATE + 1.0, 0.0, 0.0, 0.0).is_err());
1764 assert_eq!(state.snapshot(), before);
1765 }
1766
1767 #[test]
1768 fn snapshot_restores_the_coupling_memory_used_by_the_next_step() {
1769 let config = PhysicalDeckConfig::default();
1770 let control = scratching(config, 1.0, -1.0, 5.0);
1771 let mut source = DeckMechanicalState::new(config).unwrap();
1772 source.reset(1.0, 1.0, 0.0, 0.0).unwrap();
1773 source.advance(0.01, control).unwrap();
1774 let snapshot = source.snapshot();
1775 source
1776 .advance(1.0 / config.integration_hz, control)
1777 .unwrap();
1778 let expected = source.snapshot();
1779
1780 let mut restored = DeckMechanicalState::new(config).unwrap();
1781 restored.restore(snapshot).unwrap();
1782 restored
1783 .advance(1.0 / config.integration_hz, control)
1784 .unwrap();
1785 assert_eq!(restored.snapshot(), expected);
1786 }
1787
1788 #[test]
1789 fn snapshot_restores_independent_record_and_platter_phase() {
1790 let config = PhysicalDeckConfig::default();
1791 let mut state = DeckMechanicalState::new(config).unwrap();
1792 state.reset(1.0, -0.5, 17.0, -4.0).unwrap();
1793 state
1794 .advance(0.123_456, scratching(config, 1.0, -0.5, 5.0))
1795 .unwrap();
1796 let snapshot = state.snapshot();
1797 state.reset(0.0, 0.0, 0.0, 0.0).unwrap();
1798 state.restore(snapshot).unwrap();
1799 assert_eq!(state.snapshot(), snapshot);
1800 assert_ne!(
1801 state.telemetry().platter_angle_turns,
1802 state.telemetry().record_angle_turns
1803 );
1804 }
1805
1806 #[test]
1807 fn invalid_configuration_is_rejected() {
1808 let invalid = PhysicalDeckConfig {
1809 platter_inertia_kg_m2: f64::NAN,
1810 ..PhysicalDeckConfig::default()
1811 };
1812 assert!(matches!(
1813 invalid.validate(),
1814 Err(PhysicalDeckConfigError::InvalidPositive { .. })
1815 ));
1816
1817 let unstable = PhysicalDeckConfig {
1818 bearing_viscous_torque_nm_per_rad_s: 1.0e9,
1819 ..PhysicalDeckConfig::default()
1820 };
1821 assert!(matches!(
1822 unstable.validate(),
1823 Err(PhysicalDeckConfigError::UnstableIntegration { .. })
1824 ));
1825
1826 let unstable_brake = PhysicalDeckConfig {
1827 motor_brake_gain_nm_per_rad_s: 1.0e9,
1828 ..PhysicalDeckConfig::default()
1829 };
1830 assert!(matches!(
1831 unstable_brake.validate(),
1832 Err(PhysicalDeckConfigError::UnstableIntegration {
1833 field: "motorBrakeGainNmPerRadS"
1834 })
1835 ));
1836 }
1837}