Admittance model

Advanced Pedal Kinematics & Task Space Mapping

To ensure a perfectly fluid, hysteresis-free force feedback experience, the physical linear movement of the motor (Actuator Space) must be accurately translated into the rotational movement of the human foot (Task Space). This requires precise Forward and Inverse Kinematics.

1. Forward Kinematics: Sled Position to Pedal Angle

We model the pedal as a multi-body system forming two triangles. We want to find the total pedal incline angle $\phi$ based on the horizontal sled position $x$.

The pivot points are defined as follows:

• $A$: Lower pedal plate pivot (Origin $0,0$)
• $B$: Pivot where the loadcell rod connects to the sled
• $C$: Pivot where the loadcell rod connects to the pedal arm

The known mechanical lengths are:

• $a$: Length of the loadcell rod (Distance $B \leftrightarrow C$)
• $b$: Length of the lower pedal arm (Distance $A \leftrightarrow C$)
• $c_{\text{vert}}$: Vertical distance from origin to the sled spindle
• $c_{\text{hor,0}}$: Horizontal distance from origin to the sled at its minimum (0%) position.

As the motor moves the sled, the absolute horizontal distance $c_{\text{hor}}$ becomes:

$$c_{\text{hor}} = c_{\text{hor,0}} + x$$

Calculating the Virtual Hypotenuse $c$

First, we calculate the diagonal distance $c$ between the lower pedal pivot $A$ and the moving sled pivot $B$ using the Pythagorean theorem:

$$c = \sqrt{c_{\text{hor}}^2 + c_{\text{vert}}^2}$$

Calculating the Base Angle $\alpha^+$

The angle between the horizontal $+x$-axis and the virtual hypotenuse $c$ is defined as $\alpha^+$. We calculate it using the inverse tangent:

$$\alpha^+ = \arctan\left(\frac{c_{\text{vert}}}{c_{\text{hor}}}\right)$$

Calculating the Inner Angle $\alpha$

The angle between the pedal arm $b$ and the virtual hypotenuse $c$ is defined as $\alpha$. Since we know all three sides ($a, b, c$) of the triangle $A$-$B$-$C$, we can solve for $\alpha$ using the Law of Cosines:

$$a^2 = b^2 + c^2 - 2bc \cdot \cos(\alpha)$$

Rearranging for $\alpha$ yields:

$$\alpha = \arccos\left(\frac{b^2 + c^2 - a^2}{2bc}\right)$$

Total Pedal Angle $\phi$

The total pedal faceplate angle $\phi$ relative to the horizontal plane is simply the sum of the two calculated angles:

$$\phi = \alpha + \alpha^+$$

---

2. Task Space Mapping (Linear Arc Length)

The admittance control model ($F = m \cdot \ddot{s} + c \cdot \dot{s} + k \cdot s$) simulates a mass moving along a 1-dimensional path $s$. For a pedal, this path is not a straight line, but a circular arc created by the foot rotating around the lower pivot $A$.

The arc length $s$ is proportional to the radius $r$ (the lever arm of the pedal) and the angle $\phi$ (in radians):

$$s = r \cdot \phi$$

Because the lever arm $r$ is perfectly rigid, any percentage of travel $u \in [0.0, 1.0]$ along the arc length linearly corresponds to the exact same percentage of angular rotation.

$$\phi(u) = \phi_{\text{min}} + u \cdot (\phi_{\text{max}} - \phi_{\text{min}})$$

This mathematically proves that we can natively map the normalized position of the physics engine (g_vModelPos_01) directly to the target pedal angle, completely bypassing the non-linearities of the loadcell pushrod.

---

3. Inverse Kinematics: Analytical Sled Position Optimization

To command the stepper motor, we must translate the target pedal angle $\phi_{\text{target}}$ back into a linear sled target position $x_{\text{target}}$.

Previously, this was approximated using a Newton-Raphson solver. However, iterative solvers can couple with physical motor lag (tracking error) upon direction reversals, inducing mathematical deadbands and perceived mechanical hysteresis (stiction).

To achieve $O(1)$ time complexity and perfectly hysteresis-free motion, we replace the solver with an exact analytical geometric intersection.

Geometric Intersection

We define the coordinates of the pedal pivot $C$ based on the desired target angle $\phi$:

• $C_x = b \cdot \cos(\phi)$
• $C_y = b \cdot \sin(\phi)$

We know the coordinates of the sled pivot $B$ lie on a fixed horizontal line:

• $B_x = c_{\text{hor}}$ (The unknown variable we need to solve for)
• $B_y = c_{\text{vert}}$ (A constant mechanical parameter)

Because the loadcell rod $a$ connects points $B$ and $C$, the distance between them must always equal $a$. We express this using the circle equation (Pythagorean theorem):

$$(B_x - C_x)^2 + (B_y - C_y)^2 = a^2$$

Substituting our known coordinates:

$$(c_{\text{hor}} - C_x)^2 + (c_{\text{vert}} - C_y)^2 = a^2$$

Analytical Solution for the Sled Position

Let $\Delta y$ be the vertical distance between the sled and the pedal pivot:

$$\Delta y = c_{\text{vert}} - C_y$$

Substitute $\Delta y$ into the equation and isolate $c_{\text{hor}}$:

$$(c_{\text{hor}} - C_x)^2 + \Delta y^2 = a^2$$

$$(c_{\text{hor}} - C_x)^2 = a^2 - \Delta y^2$$

Taking the square root (we only use the positive root, as the sled $B$ is physically designed to always sit to the right of the pedal arm $C$):

$$c_{\text{hor}} - C_x = \sqrt{a^2 - \Delta y^2}$$

$$c_{\text{hor}} = C_x + \sqrt{a^2 - \Delta y^2}$$

Finally, we subtract the mechanical offset $c_{\text{hor,0}}$ to get the pure, relative linear travel $x$ required by the stepper motor:

$$x_{\text{target}} = c_{\text{hor}} - c_{\text{hor,0}}$$

This provides an exact, immediate target position for the motor driver that is completely decoupled from previous physical states, entirely eliminating mechanical turnaround lag.

Admittance Model Implementation

The core of the pedal's haptic feedback is the Admittance Control loop. Unlike position control, admittance control measures an input force and calculates a resulting motion based on a virtual physical model.

1. The Virtual Physics Model

We simulate a 1-Degree-of-Freedom (1-DOF) Mass-Spring-Damper system. The behavior is governed by the fundamental equation of motion:

$$F_{\text{net}} = M \cdot \ddot{s} + C \cdot \dot{s} + K \cdot s$$

Where:

• $F_{\text{net}}$: The sum of all forces acting on the virtual mass.
• $M$: Virtual mass (Inertia) – determines how "heavy" the pedal feels to accelerate.
• $C$: Virtual damping – determines the "viscosity" or resistance to movement.
• $K$: Virtual stiffness – derived from your custom Force-Travel Curve.

2. Calculation of Net Force

Every control cycle (e.g., 4000 times per second), the firmware calculates the net force:

$$F_{\text{net}} = F_{\text{human}} - F_{\text{spring}}(s) - F_{\text{damping}}(\dot{s})$$

1. $F_{\text{human}}$: The force you apply, measured by the loadcell and converted to the pedal faceplate (Task Space).
2. $F_{\text{spring}}(s)$: The target force retrieved from your S-Curve or Linear spline based on the current virtual position $s$.
3. $F_{\text{damping}}(\dot{s})$: Calculated as $C \cdot \dot{s}$ to provide resistance and prevent oscillations.

3. Discrete Integration (Tustin Transform)

To find the new position from the calculated force, we must integrate twice. For high stability and to prevent the "hard pedal" feel or jitter, the implementation uses the Tustin (Bilinear) Transformation.

The continuous transfer function in the Laplace domain is:

$$H(s) = \frac{V(s)}{F(s)} = \frac{1}{M \cdot s + C}$$

Using the Tustin substitution $s \approx \frac{2}{dt} \frac{1 - z^{-1}}{1 + z^{-1}}$, we derive a stable Infinite Impulse Response (IIR) filter:

$$v[n] = b_0 \cdot F_{\text{net}}[n] + b_1 \cdot F_{\text{net}}[n-1] - a_1 \cdot v[n-1]$$

This ensures that the virtual velocity $v$ and subsequently the position $s$ react smoothly to your foot pressure, even with very small virtual masses.

4. Signal Flow Summary

1. Input: Loadcell measures raw force $\rightarrow$ Kinematics convert this to $F_{\text{human}}$.
2. Physics: $F_{\text{net}}$ is fed into the Tustin integrator $\rightarrow$ Outputs new virtual position g_vModelPos_01.
3. Task Space: Normalized position maps linearly to target angle $\phi$ (as proven in Section 2).
4. Output: Analytical Inverse Kinematics convert $\phi$ to the exact motor step position $x$.