Switched to PI - PID control, with D taken directly from gyroscope