Updated controller so now has two PIDs and might even work.

Replace simulink file in master with this updated one.