IntRx:
Fill command queue

IntTx:
Send answer queue

Timer0:
LCD

Timer1:
Control servo

Timer2:
key input

Main loop:
Execute command queue
Compile strings for LCD and UART
Calculations and A/D