Fixed sim and control law a little bit

Still have a ton of issues. Can't get control stable yet.