Hi there,
I would suspect in the code Setup do you have the “while Serial!”
statement? comment that out and try the tests again, even better use the code tags above “</>” and paste it in there. the setup portion only if it is proprietary, we can have a look.
HTH
GL 
PJ 
You’re close you’ll get it sorted. 