finished coding hw control loop, added diagnostics