Junk from UART means either mis-matched baud, or on rare chance a *lot* of electrical noise interfering with the UART. On quick glance your code looks fine.
I strongly recommend using WebbotLib . . . but if not, it's still open source and having a look through it could help you pinpoint your mistake in your code.