完成したプログラムがこちらです。試行錯誤を繰り返して作りました。大まかな動作の手順は次のとおりです。
(1)アームを持ち上げる。グリッパーを開く。
(2)前進する。
(3)超音波センサーが反応したら、アームを下げて、ちょっと余分に前進してから停止。
(4)グリッパーを閉じて、アームを持ち上げて終了。

(2)～(3)の処理を抜き出してみました。
超音波センサーの値が20cmより短くなったらループを脱出しますので、そこから0.8秒ほど待機してから停止させています。8cmくらい余分に前進して、カップとの距離が12cmくらいに近づいて、上手くつかめるという仕組みです。

(2)～(3)の処理を図にしてみました。

プログラムを実行してみました。
ロボットが前進して、カップを検出したらグリッパー＆アームでつかみます。

うまくカップをつかむんで持ち上げることができました。成功です。
まめに調整したのでうまく動きましたが、バッテリの残量や路面の状態によって、きちんと再現しないかもしれません。待機の時間に頼るのは危険です。プログラムの再現性を高めるためには超音波センサーの取り付け位置を直して、つかむ直前まできょりを測定できるようにするといいでしょう。

以上でTETRIX入門は終了です。
TETRIX製のロボットはレゴブロック製のロボットにくらべて作業量も考える事も多いのですが、そのぶん勉強になると思います。頑丈で巨大なロボットを作れることも魅力です。
ちなみに付属のCD-ROMにはライントレースとグリッパー＆アーム以外にも製作例が3種類、さらに別売のパーツを使っての製作例が6種類ほど載っていますので、参考にしてみてください。