mostly mistakes
Fixed bugs, tuned PID and made beautiful