Main loop for testing somem apping and PD-control