今回がTETRIX入門の最終回ですので、過去に紹介してきた技術のまとめとして、物をつかむ動作と移動を組み合わせた制御を行いたいと思います。
つかむ対象物は「紙の筒」ではなく「カップ」に変更します。ちなみにこのカップはカフェオレとかの空き容器で、ある程度は弾力があります。

カップをつかむためには、前回に設定したサーボモータの角度をすべて見直す必要があります。

まず、グリッパー側のサーボモータの制御を見直します。前回はサーボの角度を固定していて、紙の筒が変形してしまいましたが、今回は角度を可変式にして、カップが変形しないようにします。

そこで、グリッパーに搭載されているタッチセンサーを使います。カップをつかんだ状態になるとタッチセンサーがオンになるという仕掛けです。そのタイミングでサーボを止めるとカップが変形しなくなります。

続いて、アーム側のサーボモータの制御も見直します。 アームが水平の状態ではカップがつかめません。そこで、20度くらい余分にサーボを回転させます。

以上の点をふまえて、カップをつかんで持ち上げるプログラムを作ってみました。

アーム側のサーボモータの角度は写真のように設定しました。 アームを20度ほど余分に回転させたい場合、 ギヤ比が2対1なので、40度回転させますので、水平位置90度+40度＝130度回転させます。 これをサーボパラメータに置き換えると、130×255÷180＝約184となります。

グリッパー側のサーボモータを制御している部分を抜き出してみました。1秒あたり15度のペースでサーボモータを回して、グリッパーを閉じていきます。その途中でタッチセンサーがオンになるとループを終了してサーボモータが止まります。安全装置として、サーボパラメータが限界値を下回ってもループを終了します。 2つの条件のどちらかでもループを終了できるようにするためOR（オア）回路を使っています。

プログラムの実行結果がこちらです。
見事カップをつかんで持ち上げることができました。タッチセンサーのおかげでカップに無理な力がかかっていません。

カップがない場合も試してみました。 グリッパーが写真の角度に達するとサーボモータが止まります。 この安全装置が働くことによって、サーボモータに無理な回転がかかりません。