移動の制御も追加してみましょう。
ロボットが前進して、超音波センサーが反応したらカップをつかんで持ち上げるという制御を行いたいと思います。
移動をさせるにはタイヤに直結した2つのDCモータを回します。今までやってきたプログラムを合体させるだけだから簡単だろうと思っていたのですが、意外にハマってしまいました。以下に工夫したポイントを紹介します。

まず、超音波センサーの誤動作をなくすために、アームを持ち上げるようにします。アームが水平になっていると、超音波センサーのじゃまになってしまいます。今までは水平にしていても誤動作は無かったのですが、たまたま運が良かっただけのようです。

アームを持ち上げて移動するということは、どこかのタイミングでアームを下げないといけません。
定規で測ってみた感じでは、センサーから約12.5cm手前まで近づいてしまうとアームがぶつかって下げられなくなってしまいます。

さらに別の問題が見つかりました。アームを下げるとギリギリに接していたはずのグリッパーが2cmくらいカップから遠ざかってしまいます。このため、アームを下げたあと、少し余分に（2cm以上）前進する必要があります。