Testing current code works in main board

Compile, Bluetooth OK