added main loop, running stable at 20hz, all functions working