Optimizacion de Codigo

Tengo un problema, trate de utilizar funciones para que mi codigo no quedara tan largo y tuviera problemas con mi arduino para que jalara mejor.

Quisiera saber como le puedo hacer para que mi codigo, ahora con funciones, pueda jalar la informacion de las IMU's y pueda mostrarmelas en mi monitor.

Gracias.

7sensors_compressed.ino (6.85 KB)

inicializacion_disp.ino (1.77 KB)

MPU_intrp.ino (1.55 KB)