You shouldn’t do any serial printing every time through the main loop like that. I do it like this:
static u32 lastPrintTime = 0;
u32 t = millis();
if (printVariable && t >= lastPrintTime + 50)
{
lastPrintTime = t;
char string[256];
switch(printVariable)
{
case 'a': sprintf(string, "%i\n", (int)(motor.shaft_angle*1000)); break;
case 'v': sprintf(string, "%i\n", (int)(motor.shaft_velocity*1000)); break;
case 'b': sprintf(string, "%i\t%i\n", (int)(motor.shaft_angle*1000), (int)(motor.shaft_velocity*1000)); break;
default: string[0] = 0;
}
if(string[0]) Serial.print(string);
}
So I can easily add different things to monitor and switch between them by changing the value of printVariable. And there’s only one call to Serial.print because consecutive calls on STM32 will block (I don’t know about ESP32). STM32’s implementation of sprintf does not support floating point, so I have to multiply by 1000 and cast to int to be able to see some decimal places, but you can try sprintf(string, “%f\n”, motor.shaft_angle); and see if ESP32’s works properly.