Rename myprintf to my_printf.
Rename set_servo_pos.
Seperate test function into unit test lib.
Rename MAX_CMD_LEN avoid reusing the name.