float speed = (4 * pi / 65536) * 168000000 / ((float)timer2 - 1.0f);
 

I was able setup a secondary (slave) timer on the encoder input. This means we time/track every rising edge on one of the encoder inputs. This can also be every encoder pulse, but we need the larger range to count.  

Chat.GPT was so kind to help with the conversion to rad/s.

TIMER2 CCR1 - >  1585
Rad/s - >  20.34
TIMER2 CCR1 - >  1560
Rad/s - >  20.66
TIMER2 CCR1 - >  1621
Rad/s - >  19.88
TIMER2 CCR1 - >  1728
Rad/s - >  18.65
TIMER2 CCR1 - >  1611
Rad/s - >  20.01
TIMER2 CCR1 - >  1575
Rad/s - >  20.47
TIMER2 CCR1 - >  1550
Rad/s - >  20.80
TIMER2 CCR1 - >  1662

This is the output with a 20 rad/s target using the conversion above: