¿Y si empiezas con algo más sencillo y te centras solo en la comunicación bluetooth? Cuando sepas manejarla podrás juntarla con el resto del código. De todas formas te diría que intentaras usar el modelo de trabajo que usan los PLC:
1. Comunicación con el exterior. Serial COM, USB, Bluetooth.
2. Leer las entradas.
3. Ejecución del programa, cálculos, acciones, etc.
4. Refresco de las salidas.
5. Vuelta al punto 1.
Como puedes ver es una máquina de estados con trabajos bien definidos y fácil de depurar.
1. Comunicación con el exterior. Serial COM, USB, Bluetooth.
2. Leer las entradas.
3. Ejecución del programa, cálculos, acciones, etc.
4. Refresco de las salidas.
5. Vuelta al punto 1.
Como puedes ver es una máquina de estados con trabajos bien definidos y fácil de depurar.