Set angular speed

[gabryelreyes]

Using the following call:

/* int32_t turtleSpeedData->angular = 1500  [mrad/s] */
int16_t angularSpeed = static_cast<int16_t>(turtleSpeedData->angular);
diffDrive.setAngularSpeed(angularSpeed);

This results in a counter-clockwise rotation of aroung 270° while the expected behavior is the rotation of the Zumo clockwise around 90° in one second.

Issue has been tracked to DifferentialDrive::calculateLinearSpeedLeftRight where the result of angularSpeed32 * wheelBase32 exceeds the maximum value of int16_t and wraps over to its negative domain.

[BlueAndi]

Issue has been tracked to DifferentialDrive::calculateLinearSpeedLeftRight where the result of angularSpeed32 * wheelBase32 exceeds the maximum value of int16_t and wraps over to its negative domain.

Can't be, because calculated with int32_t and divided by 2. Result will be lower than 65535. https://github.com/BlueAndi/RadonUlzer/blob/d7d53e5b116f64c77faa25195fecd9158b95f7ea/lib/Service/src/DifferentialDrive.cpp#L258-L259

[gabryelreyes]

True, in the case of 1500 mrad/s, the result is 63750. Nonetheless the direction of the turning is wrong

[gabryelreyes]

I do not quite understand how we come from mrad/s to steps/s in the mentioned formula, as the wheelbase is in mm https://github.com/BlueAndi/RadonUlzer/blob/d7d53e5b116f64c77faa25195fecd9158b95f7ea/lib/Service/src/DifferentialDrive.cpp#L258-L259

[BlueAndi]

Yes, looks not that good. Would calculate it like this ... please check.

Robot angular speed vs wheel linear speed

$v [\frac{mm}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2}$

• $v$: The linear speed of one wheel relative to the other $[\frac{mm}{s}]$.
• $w_r$: The robot's angular speed $[\frac{rad}{s}]$.
• $W$: The robot's wheel base $[mm]$.

Linear speed left

$v_L [\frac{mm}{s}] = -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2}$

Linear speed right

$v_R [\frac{mm}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2}$

Consider encoder steps per m

$v [\frac{steps}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$

• $v$: The linear speed of one wheel relative to the other $[\frac{steps}{s}]$.
• $w_r$: The robot's angular speed $[\frac{rad}{s}]$.
• $W$: The robot's wheel base $[mm]$.
• $ENC$: The number of encoder steps per m $[\frac{steps}{m}]$.

Linear speed left

$v_L [\frac{steps}{s}] = -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$

Linear speed right

$v_R [\frac{steps}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$

Consider robot linear speed center

Linear speed left

$vL [\frac{steps}{s}] = \frac{v{Linear}}{2} [\frac{steps}{s}] -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$

Linear speed right

$vR [\frac{steps}{s}] = \frac{v{Linear}}{2} [\frac{steps}{s}] + \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$

Consider angular speed in mrad per s

Linear speed left

$vL [\frac{steps}{s}] = \frac{v{Linear}}{2} [\frac{steps}{s}] -\frac{w_r [\frac{mrad}{s}] \cdot W [mm]}{2 \cdot 1000} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$

Linear speed right

$vR [\frac{steps}{s}] = \frac{v{Linear}}{2} [\frac{steps}{s}] + \frac{w_r [\frac{mrad}{s}] \cdot W [mm]}{2 \cdot 1000} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$

[jkerpe]

Can't be, because calculated with int32_t and divided by 2. Result will be lower than 65535.

The results are assigned to linearSpeedLeft and linearSpeedRight, which both are int16_t.

[BlueAndi]

Can't be, because calculated with int32_t and divided by 2. Result will be lower than 65535.

The results are assigned to linearSpeedLeft and linearSpeedRight, which both are int16_t.

And in range with provided values.

[gabryelreyes]

I believe the first equation is wrong when dividing the wheelbase by 2. This Source proposes an equation for $w$ in Eq. 3, from which we can derive the following:

$v [\frac{mm}{s}] = w_r [\frac{rad}{s}] \cdot W [mm]$

Carrying this change over all equations presented earlier, the final equations for linear speeds are as follows:

Linear speed left

$vL [\frac{steps}{s}] = \frac{v{Linear}}{2} [\frac{steps}{s}] -\frac{w_r [\frac{mrad}{s}] \cdot W [mm]}{1000} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$

Linear speed right

$vR [\frac{steps}{s}] = \frac{v{Linear}}{2} [\frac{steps}{s}] + \frac{w_r [\frac{mrad}{s}] \cdot W [mm]}{1000} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$

I have tested both variants and the solution I am proposing seems to achieve the goal and turning in the correct direction and the correct distance.

Regarding the size of int16 and the maximal result of the equation, an angular speed of $48186 [\frac{mrad}{s}]$ would be required to reach the maximum value for int16. We are quite far away from this value.

[BlueAndi]

But your linked documentation shows a division by 2. :-? And if I calculate with real values, the divison by 2 is required to get the right result. Did you test it only empirical or by calculation?

• $w = \frac{v_R - v_L}{W}$
• $v_L = -v_R$ by rotation about midpoint of the wheel axis
• => $w = \frac{v_R - (-v_R)}{W} = \frac{2 \cdot v_R}{W}$
• => $w = \frac{(-v_L) - v_L)}{W} = \frac{-2 \cdot v_L}{W}$
• => $v_R = \frac{w \cdot W}{2}$
• => $v_L = -\frac{w \cdot W}{2}$