サーボの動作チェック

ラジコンなどに使うサーボ(SG92R)の動作チェック用に作ったソフトです。
サーボはタイマー1のPWM(OC1B)につなぎ、ADに接続したボリュームを回して１８０°動くことが確認できました。
（関係するところだけの抜粋です。）

$regfile "m328pdef.DAT"

$crystal = 16000000

Config Portb.2 = Output                                                    '駆動用PWM OUT        

Dim Vin As Word                                                            'ADC ch1 
Dim I As Word                                                              'ループ変数

Dim Adch1 As Word                                                          'AD ch1の測定データ

Dim V As Word
Dim V_int As Integer


   Config Adc = Single , Prescaler = Auto , Reference = Avcc               'A/Dコンバータの設定。   10bit=1024

'- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

   Config Timer1 = Pwm , Prescale = 8 , Clear Timer = 1 , Compare B Pwm = Clear Up       '16MHz-->2MHz-->50Hz


   Set Tccr1a.wgm11                                                        'fast PWM
   Set Tccr1b.wgm13                                                        'top=ocr1A

   Compare1a = 39999                                                       '分周値設定
   Compare1b = 2899                                                        '

Enable Interrupts                                                          'すべての割り込みを許可。

   Start Adc                                                               'A/Dコンバータに電源を供給。

'main

   Do

         Gosub Adc_sub

         V_int = Adch1 * 3.71
         V = V_int + 1000

         Compare1b = V - 1

         Waitms 10

   Loop


Adc_sub:

         Vin = 0

         For I = 1 To 5

            Vin = Vin + Getadc(0)                                          ' アナログ入力０の変換値を変数へ代入。

            Waitus 100

         Next I

         Adch1 = Vin / 5                                                   '５回サンプリングなので５で割る

         Return