control structure generally works. lots of height change while servoing. update PID