Changes to motor control

Tuned individual motors to control yaw before hitting the PID. Not
finished yet, but it is better.