Bug in script
Added Function test to check if servos working.
Added test program for debugging uart.