Semaphores, Mutex and RTOS on Arduino (ChibiOS Help?)

I did make that shared variable volatile and during use by a thread. I as able to execute the script correctly buy running it on an arduino with no servos attached, unfortunately I don't think it was a code issue but a power supply issue as the "ping" sensor itself was not lighting fully with the servos on but with them disabled I had no problems.