Intergration and setup on real test hardware

Testing

Better determine algorithm

Complete OA Controller