Created 2d linear controller for full robot sim