サーボモータ（SG-90）をPIC16F18313で動かすための基本的な計算が出来たところで実際のプログラムを作る。サーボモータを動かすために必要なパルス波形は次のようなものである。
2つのパルスの周期（20ms)はTMR２で決める。２つのサーボモータの角度に対応する2つのパルス幅（0.5ms～2.4ms）はパルス幅変調器（PWM5及びPWM6）で決める。
サーボモータがある角度にある時、突然別の角度を指示するとサーボモータは最速（仕様では0.12sec/60度=500度/秒）でこれに追従する。これが目的とする動作に合っているのか、またサーボモータの負荷を高め寿命を縮めることにならないか気になるところである。そこでTMR2+PWMで生成できる最小の分解能である3度毎にステップを進める事とする。もし20ms毎に割り込みをかけて3度進めると150度/秒となり、良さそうである。TMR2にはポストスケーラがあるので、これを使うとこの1倍から1/16倍までのスピード制御が可能であり、即ち9.4度/秒～150度/秒の速度制御が可能である。
ロジックとしてはTMR2（のポストスケーラ）により割込みがかかったときサーボが目的とする角度に達していない場合は要回転を意味するフラグを立てる。さらにパルスの立下りで（IOCの機能を使って）割り込みをかけ、要回転フラグが立っている場合は次のパルス幅に相当する値をPWMに書き込む。そのタイミングがパルスの立下り後である理由はパルスの立下り時に処理を始めると処理のための時間的余裕が十分あるからである。時間的には20ms以内に処理を終わらせればよいので問題は無いだろう。なおIOC（Interrupt On Change）とはPICのピンの信号の変化を捉えて割込みを起こす機能であり、立ち上がりと立下りを個別に捉え割り込みを起こす事ができる。通常は入力信号に対して使う機能だと思うが、ここでは出力信号に対して使っている。
ソフトの開発と並行して汎用基板にプロトタイプのハード（コントローラ）を作った。 大きさは概ね2cm×4cmに収まった。
基板に載っている部品はPIC16F18313と電源のパスコン、LEDと電流制限用抵抗だけで他に接続用に電源コネクタ、サーバモータ用3Pコネクタが2つ、ホストと繋ぐシリアル通信用コネクタが1つあるだけである。シリアルインターフェイスはUSBを介してTeraTermから文字ベースでテストできるようにした。
電源を入れると各部の初期化を行い、TME2やPWMを起動、1.45msのパルスを作ってサーボを中点まで移動、その後パルス幅を0としてサーボを休止、割り込み可としてSLEEPする。
RS-Kager2-Servo313-032 2024/10/23 for RS-Kager2 ver 0.27
Copyright(C)2024 MYcrosLip
*** Init Servo.
*** Start Interrupt.
*** Start Servo.
*** Sleep Loop.
あとはシリアル通信で受信したコマンドを解釈してPWMを制御しサーボを３度ステップで動かす。目的の角度に達したらパルスを止める。3度進むのに必要な時間は7.5msであるから50Hzの１周期（20ms）以内に終わり、タイマーなどを使った遅延操作は不要である。
実装した主なコマンドは<nn>H、<nn>V、Z、<mm>Gである。Hは水平サーボ、Vは垂直サーボの軸を回転（指向）させるコマンドで<nn>は-90度～90度の範囲であり、Zは原点（0,0）に復帰、Gは速度制御（<mm>は引数で1～16）である。
次の実行例で、90Hは水平サーボを90度に向け、90Vは垂直サーボを90度に向け、Zで原点に戻っている。小文字のhやvは動作確認のため1ステップ進むごとに表示させている。
90Hhhhhhhhhhhhhhhhhhhhhhhhhhhhhhh
90Vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
Zhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhvhv
サーボの動作中でも次のコマンドを受け付ける。この場合、実行中のコマンドの終了を待たずにキャンセルし、後で来たコマンドを処理する。この動作を2つのサーボに対して各々独立して行う（逆に言えば処理待ちキューを作らずに済ませたという事）。
これで外部から文字ベースでサーボを制御できるようになった。
（続く）
