電子工作79 – サーボモーターを動かしてみる

Electronics Project 79 – How to Control Servo Motors

 

 

概要    Introduction

   RCサーボとか言われる小さく瞬時に主軸の回転角を変えることが出来るデバイスが比較的安価に、しかも多種類のものが出回っている。ずっと前からいじくって見たいと思っていた。まず、安いものを入手して駆動してみることにした。

 

  Plenty of RC servo motors are available today and their prices are not that expensive. I have been wanted to know what they were and how to control them. As the first step, I bought one of the cheapest servo motors called Micro Servo Motor and tried to control it.

 

回路図   Circuit Diagram

下にサーボモーターを駆動する回路図の1例を示す。最低限必要な配線がしてある。電源はボード上に組んだ可変電圧三端子レギュレータの317がつくり出す5Vを利用する。それをPIC12F683とサーボモーターの両方に使う。モーターからノイズが出るのでコンデンサーは必須である。PIC12F683を用いた理由は後々にそれに内蔵されているADC(アナログーデジタルコンバータ)を使うためである。ここで使うマイクロサーボSG92Rの仕様は; トルク:2.5kgf-cm、動作速度:0.1sec/60 degrees、 動作電圧:4.8–6V、外寸: 23x12.2x27mm、 重量:9g で、ちなみに価格は500円であった。

 

Following is the simplest circuit diagram necessary for driving a servo motor (SG92R, Tower Pro Pte Ltd., costing 500 Yen). Using a three-terminal regulator 317 generates its power supply 5.0V. Both 12F683 and the servo motor use the same 5.0V power source, so the noise from the servo motor must be blocked with 0.1uF capacitor. The reason to use 12F683 is because there is a plan to use its built-in ADC (Analog-Digital Converter) function in future applications. The specification of this servo motor is; Torque 2.5 kgf-cm, Operational speed 0.1 sec/60 degrees, Power supply 4.8-6V, Dimension 23x12.2x27mm, Weight 9 g.

 

 (Fig.1)

 

(Photo 1) 実験中

Under experiment

 

(Photo 2) サーボモーター  

Servo motor

 

(Photo 3) パルス幅と回転角の関係

Relationship between pulse width and rotational angle

Pulse width=2.40 ms               Pulse width=1.55 ms              Pulse width=0.50 ms

 

(Photo 4) プログラムが発生させるパルス幅0.5ms、高さ5V、間隔20msの場合の5本のパルス。この図で実測するとパルス幅も間隔もほぼ理論値と同様であった。

Five pulses with a pulse width of 0.5ms generated by the program below. The measured pulse width and interval values were found almost the same as the theoretical ones.

 

結果&コメント

1)     上記の回路と下のプログラムでサーボモーターはキビキビ動作する。

2)     最大回転角は一般に180度位らしいので、大体170度位の範囲で動作するようにプログラムの中でパルス幅を調整した。

3)     PicBasic ProのコマンドPulsoutやPauseで作成される時間幅(例、0.5msとか20ms)がサーボモーターを動かすのに実用上問題ないくらいに正確であることが分かった。

4)     サーボモーター(SG92R)の仕様は英語版のWeb上に載っているがあまり詳しくはない。例えば、消費電流とか不明。テスターでは消費電流を測れなかった(数値が出ない)。

5)     (追加2015.2.16) 3.0V位から動作するがちょっと異音がでる。3.3V位に上げてやるとスムースに動作し、速度も多少低下するので、多分使いやすくなる

6)     (追加2015.2.16) 個々のパルス幅を含めたパルス周期を正確に20msになるようにプログラムを改良した('Servomotor12F683modiB.bas)。サーボモーターの停止位置の変化が前と比べてどれぐらいかは目で見ては判別できない。サーボモーターの電源は12F683のそれ(5.0V)と分離し3.3Vで機敏に動いている。

7)     (追加2015.2.21) VR (ボリューム)でサーボモーターが180度回転するように回路とプログラムを改良した。最初20クラスに分割して動くようにしたが荒いので、41クラスに分割。1分割が4.4度でまずまずスムースに動く。ただ、モーターの停止位置によってはピリピリというノイズが発生することがある。VRをゆっくり回して41ヶ所を1ヶ所ずつたどることが出来る。VRを急速に回すと大体1秒間で180度回転させることが出来る。41分割を2倍の80分割位にすると回転がもっとスムースになりそう。12F863のメモリ2Kの内、今回は1321使用。従って、80分割するにはメモリの大きい14ピンのPICを使う必要あり(例えば、16F688 4K ¥160).今回Adcinの後にPause を入れなかった。Webで見ると必要ないようだ。Define ADC_SAMPLEUS 50 'Sample timingで50usを既に確保しているのかも知れない。

8)     (追加2015.2.24) 41分割を67に増やし、回転単位(180/67=2.67、約2.7度/分割)を小さくしたのでよりスムースに動作するようになった。2402 wordsを使う今回のプログラムでは2Kメモリの12F683は使えないので、4Kメモリの16F688を使うことにした(電子工作51参照)。VRの急速回転に対するモーター軸の追従速度はちょっと遅くなった感じ。プログラムは知恵が浮かばずコンパクトに出来なかった。プログラミングでコンパイラ時、Resultsに3行のメッセージ(エラーメッセージではない)が出たが問題なくPICに焼き込みが出来き、動作した。

9)     (追加2015.3.3) 分割クラスを67から100に増やした。回転のスムースさと、不都合が起きないかどうか(ADCの追従性など)をテストするためだ。1.8度づつ動くので確かにより滑らかに回転するようになった。ボリュウームの回転に対するモーター軸の回転追従性も前とあまり変わらない。回転位置によっては多少振動を出して不安定になることもある。何しろプログラムが長い、短くしようとしたが知恵が浮かばなかった。改良の余地はある

10) (追加2015.3.27) 上のプログラムを工夫して短くした。機能に変化はない。PICのメモリをより消費することが分かった。画期的に短くする方法はまだ見つからない。

11)(追加2015.4.3) PWが大きくなる(0.5msから2.5ms)と周期が大きくなることがオシロを使って発見。20msでなければならないのが30msにもなっていた。プログラムの中を通過するステップ数によるようだ。そこでPWに関連づけた補正を加えた。具体的にはプログラムの一行を変えるだけ。従来のPauseus (20000-PW*10)に補正項PW-50)*47 を加えてPauseus ((20000-(PW*10))-(PW-50)*47)とした。ここでPW-50の50はPWの初期値、47はオシロで波形を見ながら実験的に決めた値(以下の'Servomotor16F688PotG.basの場合、一方、'Servomotor16F688PotGmod.basでは51を使う)。 PWの全域で周期が20msに保たれることが分かった。ADCへの入力電圧波幅周期の間の関係を示すTable1を加えた。

 

 

Results and Comments

1)     With the above circuit and the program shown below, the servo motor works well.

2)     Because the maximum rotational angle is commonly known to be 180 degrees, the program was designed by adjusting the pulse widths properly so that the maximum rotational angle of this servo motor became around 170 degrees.

3)     It is found that the pulse width and interval values generated by using PicBasic Pro’s commands PULSOUT and PAUSE are accurate enough to control this servo motor.

4)     (Newly added on 2015.2.21) A variable resistor (VR) was introduced to the existing system to rotate the axis of servo motor in an angular range of 180 degrees according to the rotary movement of VR from end to end. It works reasonably smoothly though some rotary position of VR occasionally makes the functioning of servo motor slightly unstable, causing a little vibration.

5)     (Newly added on 2015.3.3) A new program was built, in which 180-degree rotary angle was divided into 100 divisions (180/100=1.8 degrees/division), in place of 67 (about 2.7 degrees/division) in the previous program, in order to enhance the smoother rotation of servo motor axis.

 

 

プログラム       

   今回のプログラムはサーボモーターが実際に動作するかどうかを検証するためのものである。パルス幅0.5−2.5msで高さ5V程度のパルスを15−20msの間隔でサーボモーターの信号線に加えてやると、パルス幅に応じて一定の角度位置を保持し続けるというのがその動作原理である。PicBasic ProにはPulsoutという便利なコマンドがあり、これを使うと簡単に任意の幅のパルスが作り出せる。例えば、Pulsout GPIO.2、50 と書くと、4MHzの内部クロックを使う場合、12F683のPin5より幅0.5msのパルスが発生する。後は同じくコマンドPauseを使って 20msの0V(Low)の間隔を入れる。今回はこのプロセスを5回繰り返させ(5本のパルスをつくる)、3秒待って別のパルス幅のピークを5本発生させてサーボモーターの角度を変える。さらに、パルス幅の違うものをもう一回加えて合計3つの角度位置で3秒間ずつ停止するようにプログラムされている。

 

Program

The program was designed to test the operation of this servo motor. As commonly known, RC servo motors can operate if pulses with a pulse width of 0.5-2.5ms, a pulse interval of 15-20ms and a voltage of about 5V are added to their signal cable. And their rotational angle is decided by an added pulse width. PicBasic Pro has the commands Pulsout and Pause with which it is possible to easily generate both any pulse width and pulse interval required for driving servo motors. For instance, Pulsout GPIO.2, 50 used in the program means to generate a 0.50ms width pulse from GPIO.2 (Pin 5) of 12F683. Meanwhile Pause 20 can provide a 20ms interval (0V or Low) between pulses. In the program, the pulses of 0.5ms, 1.55ms and 2.40ms widths are continuously generated one after another. That is, five pulses having 0.5ms width are generated, and then after 3-seconds wait, the next five pulses having 1.55ms width are generated, followed by five 2.40ms width pulses.

'****************************************************************

'Servomotor12F683.bas  2015.2.4  2015.2.7

'12F683 with internal clock 4MHz

'PicBasic Pro V2.50A used

'Servo motor used: SG92R

'********* Pin connection ************

'Pin 1: Vdd (5.0V)

'Pin 8: Vss (GND)

'Pin 7: GPIO.0: NC

'Pin 6: GPIO.1: NC

'Pin 5: GPIO.2: output - connected to Signal input of Servomotor

'Pin 4: GPIO.3: input - connected to MCLR via 4.7kR to Vdd

'Pin 3: GPIO.4: NC

'Pin 2: GPIO.5: NC

'**************** Program **************

TRISIO=%001000       'GP3 - Input, GP2 - Output

ANSEL=0              'All ports are digital

CMCON0=0             'No comparator used

 

ServOUT var GPIO.2  'Servomotor driving signal out

m var byte

 

GPIO.2=0

Loop:

For m=1 to 5

Pulsout ServOUT, 50  'Servo pulse width=50 (0.50ms=500us and 500/10=50)

Pause 20  'Wait 20 ms

Next m

Pause 3000  'Wait 3 sec

 

m=0

For m=1 to 5

Pulsout ServOUT, 155

Pause 20

Next m

Pause 3000  'Wait 3 sec

 

m=0

For m=1 to 5

Pulsout ServOUT, 240

Pause 20

Next m

Pause 3000  'Wait 3 sec

 

Goto Loop

End

'****** End of Program ********

'Note:                                                                                             

   'melabs U2 Programmer's configurations  12F683

'FOSC – INTOSCIO; WDTE - Enable; Power-up Timer – Enable;  'MCLRpin – Reset ; BODEN - Enable; Internal External Switch Over – Enable,

'Fail-Safe Clock Monitor – Enable, Code - Disable; Data EPPROM – Disable

'**************************************************

 

 

パルス周期をより正確になるように改良(2015.2.16追加)    Modified to generate more accurate pulse cycle (newly added on 2015.2.16)

  前のプログラムではサーボモーターの信号線に加えるパルスは、3種の異なる幅のパルスに対して一様にパルス幅+20msとしていた。正確には正しくなく、各パルス幅+X=20msになるようにXを決める必要があった。プログラムのコマンドPauseでは最低ms単位なので、精度を上げるためus(micro second)を扱えるPauseusを使うことにした。例えば、Pulsout ServOUT、155の場合、実際に出力されるパルス幅は1.55ms(=1550us)である。一方、Pauseus 18450 はパルス間の距離(周期)で単位はusなので、18450=18.450ms となり、パルス幅+パルス間隔=1.55+18.45=20msで、パルス周期は20msと一定に保たれる。他の0.50msと2.40msも同様にパルス周期が20msなる様に調整した。

 

Modified program

  In the above program, each pulse width + 20ms was used to test the movement of the servo motor SG92R. But it is commonly mentioned for the pulse cycle to be 15-20ms including the individual pulse width. To meet this, Picbasic Pro's command PAUSEUS instead of PAUSE is used. For example, as shown in the program below, Pauseus 18450 means a delay of 18450us (micro second) or 18.45ms. Meanwhile, Pulsout ServOUT, 155, which comes above the Pauseus 18450, means to generate a pulse of a width of 155. Here, 155 is equal to 1550us or 1.55ms in the command Pulsout. Therefore, the addition of 18450us and 1550us makes 20000us or 20.0ms. In fact, the X of Pauseus X is calculated by subtracting the individual pulse width from 20000us.

 

(Photo 5) パルス幅1.55msの出力を実際に測った結果1.546msと出た。

Actually measured pulse width for the designated pulse width 1.55ms – quite accurate

 

(Photo 6) 正確になったパルス周期(20ms)

Pulse cycle fixed to 20ms for 3 kinds of pulse widths. This is for the pulse width 1.55ms.

 

改良したプログラム

Modified Program

'*****************************************************

'Servomotor12F683modiB.bas  2015.2.4  2015.2.7  2015.2.15

'12F683 with internal clock 4MHz

'PicBasic Pro V2.50A used

'Servo motor used: SG92R

'Using PBP's PAUSEUS command (2015.2.15)

'**********Pin connection **********

'Pin 1: Vdd (5.0V)

'Pin 8: Vss (GND)

'Pin 7: GPIO.0: NC

'Pin 6: GPIO.1: NC

'Pin 5: GPIO.2: output - connected to Signal input of Servomotor

'Pin 4: GPIO.3: input - connected to MCLR via 4.7kR to Vdd

'Pin 3: GPIO.4: NC

'Pin 2: GPIO.5: NC

 

'******* Program *********

TRISIO=%001000       'GP3 – Input, GP2 - Output

ANSEL=0              'All ports are digital?

CMCON0=0             'No comparator used?

 

ServOUT var GPIO.2  'Servomotor driving signal out

m var byte

 

GPIO.2=0

Loop:

For m=1 to 5

Pulsout ServOUT, 50  'Servo pulse width=50 (0.50ms=500us and 500/10=50)

Pauseus 19500   '=20000us-500us

Next m

Pause 3000  'Wait 3 sec

 

m=0

For m=1 to 5

Pulsout ServOUT, 155   '=1550us=1.55ms

Pauseus 18450  '20ms=20000us 1.55ms=1550us therefore 20000-1550=18450us

Next m

Pause 3000  'Wait 3 sec

 

m=0

For m=1 to 5

Pulsout ServOUT, 240   '=2400us=2.40ms

Pauseus 17600   '=20000us-240us

Next m

Pause 3000  'Wait 3 sec

 

Goto Loop

End

'*************************************

 

 

ボリュームの回転と同期してサーボモーターを180度回転させる回路とプログラム(2015.2.20追加)

  100Kのボリュームを端から端まで回すとサーボモーターの回転角が0から180度変化するような回路とプログラムを作成した。原理は12F683の電源5.0Vをボリューム(VR100K)に加え、回転にしたがって0−5.0Vまで変わる出力電圧を12F683のGP0から取り込んでAD変換を行い、内部で得られる0−1024(10ビット)の値からパルス幅を作成し、GP2から出力してサーボモーターの停止位置(角度)を決めるというのがその原理である。ボリュームの回転軸と同じ回転方向にサーボモーターの軸も回る。VR100Kの出力ケーブルとGNDの間に0.1uFのコンデンサが入れてあるがこれがないとモーターに異常な振動が出て上手く動かない。

 

Circuit and program to rotate a servo motor in an angular range of 180 degrees using a variable resistor (newly added on 2015.2.20)

  Adding a 100K variable resistor (VR) to the above circuit makes it possible to rotate the axis of servo motor in an angle of 180 degrees. Various voltages (0-5.0V) from this VR are read into GP0 of 12F683, within which they are then converted into digital values (0-1024). In its program, the individual digital value is classified into one of 41 classes and the specified class generates a pulse width (ms) and a 0V-level interval (ms) in such a way that the total of the pulse width plus the 0V-level interval always becomes 20ms or 1 cycle. The capacitor 0.1 uF is essential to suppress the vibration of servo motor.

(Fig 2)

 

(Photo 7) ボリュームでサーボモーターが180度回転する

Variable resistor and servo motor

 

プログラム

 ボリューム出力0−5.0Vに対応するADC変換値0−1024を25間隔で41クラスに分け、各クラスに飛ばしてそこでパルス幅とパルス0V部間隔(パルス間の0V部分の間隔)を計算させ、それをサーボモーターの信号線に送る。サーボモーターはパルス幅0.50ms(モーター上から見て左端、0V)から始まって1.50msで中央位置(2.5V)、そして2.50msで180度回転位置(右端、5V)に停止するようにしてあり、パルス幅0.50msから、0.55ms、0.60ms、...のように各クラス+0.05msづつ増加し、最終的には2.50msの最大幅に達する。ちなみに、この時のパルス0V部間隔は20ms(パルス周期)―2.50ms=17.50msとなる。 41クラスに分けたので180/41=4.39、つまり、ボリュームを回すと約4.4度づつ回転位置を変える。クラス数を予備実験で20にしたが、回転が荒いので今回の41にした。

 

Program

  This servo motor appears to be designed to take the leftmost position with an addition of 0.50ms-wide pulse, the center position with 1.50ms-wide pulse and the rightmost position with 2.50ms-wide pulse. So the program was made to rotate the axis of servo motor in an angular range of 180 degrees when VR is rotated from end to end. Because 180 degrees are divided to 41 classes, the unit moving angle is about 4.4 degrees (180/41=4.39).

'*****************************************************

'Servomotor12F683PotB.bas  2015.2.18 2015.2.19 works OK!

'Division increased up to 41 (2015.2.19)

'12F683 with internal clock 4MHz

'PicBasic Pro V2.50A used

'Servo motor used: SG92R

'Using PBP's PAUSEUS command (2015.2.15)

'******* Pin connection *********

'Pin 1: Vdd (5.0V)

'Pin 8: Vss (GND)

'Pin 7: GPIO.0: input - connected to VR=100K (analog value input)

'Pin 6: GPIO.1: NC

'Pin 5: GPIO.2: output - connected to Signal input of Servo motor

'Pin 4: GPIO.3: input - connected to MCLR via 4.7kR to Vdd

'Pin 3: GPIO.4: NC

'Pin 2: GPIO.5: NC

 

'******* Program *********

Define ADC_BITS 10     '10-bit ADC used

Define ADC_CLOCK 3     'Internal clock used ?

Define ADC_SAMPLEUS 50 'Sample timing

ADCON0=%10000011 

'=131 (decimal) 'bit7=1(right justified),bit6=0(volt refer-Vdd),

'bit5-4=0(unimplemented),bit3-2=00(channel0-An0),

'bit1=1(starts right away),bit0=1(on operation)

ANSEL=%00010001   'sets Fosc/8 and GPIO.0 as ANS0 input

'bit7=0(unimplemented),bit6-4=001 (Fosc/8),

'bit3-2-1=0(Digital I/O pins),bit0=1(Analog input)

TRISIO=%001001       'GP0 and 3 - Input, GP2 - Output

CMCON0=0             'No comparator used?

ServOUT var GPIO.2  'Servomotor driving signal out

Vin var Word  'Vin>255

PW var Word  'PW=Pulse width

 

GPIO.2=0

Loop:

Vin=0

Adcin 0, Vin  'Read ADC ch-0 and store it in Vin

 

IF Vin<=25 Then Jump1  '25 interval    'Jump to one of the following points depending on Vin value obtained

IF (Vin>25) and (Vin=<50) Then Jump2   '0 - 5.0V // 1024, 1024 steps/25=40.96 -> 1024 were divided to 41 classes

If (Vin>50) and (Vin=<75) Then Jump3

IF (Vin>75) and (Vin=<100) Then Jump4

IF (Vin>100) and (Vin=<125) Then Jump5

IF (Vin>125) and (Vin=<150) Then Jump6

IF (Vin>150) and (Vin=<175) Then Jump7

IF (Vin>175) and (Vin=<200) Then Jump8

IF (Vin>200) and (Vin=<225) Then Jump9

IF (Vin>225) and (Vin=<250) Then Jump10

IF (Vin>250) and (Vin=<275) Then Jump11

IF (Vin>275) and (Vin=<300) Then Jump12

IF (Vin>300) and (Vin=<325) Then Jump13

IF (Vin>325) and (Vin=<350) Then Jump14

IF (Vin>350) and (Vin=<375) Then Jump15

IF (Vin>375) and (Vin=<400) Then Jump16

IF (Vin>400) and (Vin=<425) Then Jump17

IF (Vin>425) and (Vin=<450) Then Jump18

IF (Vin>450) and (Vin=<475) Then Jump19

IF (Vin>475) and (Vin=<500) Then Jump20

IF (Vin>500) and (Vin=<525) Then Jump21

IF (Vin>525) and (Vin=<550) Then Jump22

IF (Vin>550) and (Vin=<575) Then Jump23

IF (Vin>575) and (Vin=<600) Then Jump24

IF (Vin>600) and (Vin=<625) Then Jump25

IF (Vin>625) and (Vin=<650) Then Jump26

IF (Vin>650) and (Vin=<675) Then Jump27

IF (Vin>675) and (Vin=<700) Then Jump28

IF (Vin>700) and (Vin=<725) Then Jump29

IF (Vin>725) and (Vin=<750) Then Jump30

IF (Vin>750) and (Vin=<775) Then Jump31

IF (Vin>775) and (Vin=<800) Then Jump32

IF (Vin>800) and (Vin=<825) Then Jump33

IF (Vin>825) and (Vin=<850) Then Jump34

IF (Vin>850) and (Vin=<875) Then Jump35

IF (Vin>875) and (Vin=<900) Then Jump36

IF (Vin>900) and (Vin=<925) Then Jump37

IF (Vin>925) and (Vin=<950) Then Jump38

IF (Vin>950) and (Vin=<975) Then Jump39

IF (Vin>975) and (Vin=<1000) Then Jump40

IF Vin>1000 Then Jump41

 

Jump1:

PW=50   'PW=Actual pulse width/10, so 50 is equal to 500us=0.50ms  

Gosub PulsMake  'Goto Subroutine PulsMake

Goto Loop

Jump2:

PW=55   'increment=5(=0.05ms) added to the above base value (=0.50ms)

Gosub PulsMake

Goto Loop

Jump3:

PW=60

Gosub PulsMake

Goto Loop

Jump4:

PW=65

Gosub PulsMake

Goto Loop

Jump5:

PW=70

Gosub PulsMake

Goto Loop

Jump6:

PW=75

Gosub PulsMake

Goto Loop

Jump7:

PW=80

Gosub PulsMake

Goto Loop

Jump8:

PW=85

Gosub PulsMake

Goto Loop

Jump9:

PW=90

Gosub PulsMake

Goto Loop

Jump10:

PW=95

Gosub PulsMake

Goto Loop

Jump11:

PW=100

Gosub PulsMake

Goto Loop

Jump12:

PW=105

Gosub PulsMake

Goto Loop

Jump13:

PW=110

Gosub PulsMake

Goto Loop

Jump14:

PW=115

Gosub PulsMake

Goto Loop

Jump15:

PW=120

Gosub PulsMake

Goto Loop

Jump16:

PW=125

Gosub PulsMake

Goto Loop

Jump17:

PW=130

Gosub PulsMake

Goto Loop

Jump18:

PW=135

Gosub PulsMake

Goto Loop

Jump19:

PW=140

Gosub PulsMake

Goto Loop

Jump20:

PW=145

Gosub PulsMake

Goto Loop

Jump21:

PW=150

Gosub PulsMake

Goto Loop

Jump22:

PW=155

Gosub PulsMake

Goto Loop

Jump23:

PW=160

Gosub PulsMake

Goto Loop

Jump24:

PW=165

Gosub PulsMake

Goto Loop

Jump25:

PW=170

Gosub PulsMake

Goto Loop

Jump26:

PW=175

Gosub PulsMake

Goto Loop

Jump27:

PW=180

Gosub PulsMake

Goto Loop

Jump28:

PW=185

Gosub PulsMake

Goto Loop

Jump29:

PW=190

Gosub PulsMake

Goto Loop

Jump30:

PW=195

Gosub PulsMake

Goto Loop

Jump31:

PW=200

Gosub PulsMake

Goto Loop

Jump32:

PW=205

Gosub PulsMake

Goto Loop

Jump33:

PW=210

Gosub PulsMake

Goto Loop

Jump34:

PW=215

Gosub PulsMake

Goto Loop

Jump35:

PW=220

Gosub PulsMake

Goto Loop

Jump36:

PW=225

Gosub PulsMake

Goto Loop

Jump37:

PW=230

Gosub PulsMake

Goto Loop

Jump38:

PW=235

Gosub PulsMake

Goto Loop

Jump39:

PW=240

Gosub PulsMake

Goto Loop

Jump40:

PW=245

Gosub PulsMake

Goto Loop

Jump41:

PW=250

Gosub PulsMake

Goto Loop

 

PulsMake:  'Subroutine

Pulsout ServOUT, PW    'Making a pulse with a width of PW*10us

Pauseus (20000-PW*10)  'Generating an 0V-level interval of (20000us - PW*10us) after the above pulse

Return

End

'***** End of Program ******

'Note:                                                                                             

   'melabs U2 Programmer's configurations  12F683

'FOSC – INTOSCIO; WDTE - Enable; Power-up Timer – Enable;  'MCLRpin – Reset ; BODEN - Enable; Internal External Switch Over – Enable,

'Fail-Safe Clock Monitor – Enable, Code - Disable; Data EPPROM – Disable

'**************************************************

 

 

ボリュームの回転と同期してサーボモーターを180度回転させる回路とプログラム - 分解能を67に向上(2015.2.24追加)

 上ののプログラムではサーボモーターの回転角を約180度回すのに、0−1024(0−5.0V)を41クラスに分離したが、今回は更に67クラスに分離して分解能を上げた。プログラムが大きくなって12F683の2Kのメモリでは足りなくなったので、4Kのメモリを持つ16F688を使うことにした。

 

Improved rotary movement resolution of servo motor's axis by increasing the number of division classes from 41 to 67 (newly added on 2015.2.24)

  For this purpose, 16F688 with 4K memory in place of 12F683 with 2K memory was used this time. In fact, 2402 words were consumed in the program.

(Fig. 3)

 

プログラム

 今回のプログラムも前の12F683の場合とほぼ同じである。12F683では2Kメモリの内1321使用された。今回は16F688の4Kの内2402が使用され、12F683は使えないことが分かる。10bitADCの0−1024を15毎に増加させ67クラスに分離、パルス幅0.50ms−1.50ms―2.50msでは0.03ms毎に値を増加させた。予想通り、ボリュームの回転にしたがってよりスムースにモーター軸が回転するようになった。180/67=2.68、つまり、ボリュームを回すと約2.7度づつ回転位置を変える(前の41クラス分離では約4.4度)

 

Program

  The ADC range 0-1024 for 0-5.0V was divided into 67 classes with an increment of 15. The corresponding unit angular movement is 0.03ms, starting from 0.50ms (leftmost position), 0.53ms, .. 1.50ms (central position), 1.53ms .. and 2.50ms (rightmost position). The rotary angle changes by about 2.7 degrees (180/67=2.68) according to a change of pulse width 0.03ms. The rotary movement of the axis has become smoother.

 

'**************************************************

'Servomotor16F688PotC.bas  2015.2.18 2015.2.19 2015.2.23 work OK!

'Division increased up to 67 (2015.2.23)

'16F688 with internal clock 4MHz

'PicBasic Pro V2.50A used

'Servo motor used: SG92R

'Using PBP's PAUSEUS command (2015.2.15)

'******* Pin connection *********

'Pin 1: Vdd (5V)

'Pin 14: Vss (GND)

'Pin 13: PORTA.0: input - connected to VR=100K (analog value input)

'Pin 12: PORTA.1: output - connected to Signal input of Servomotor

'Pin 4: PORTA.3: input - connected to Reset

'All other pins - NC

'******* Program *********

Define ADC_BITS 10     '10-bit ADC used

Define ADC_CLOCK 3     'Internal clock used

Define ADC_SAMPLEUS 50 'Sample timing

ADCON0=%10000011 

'=131 (decimal) 'bit7=1(right justified),bit6=0(volt refer-Vdd),

'bit5-4=0(unimplemented),bit3-2=00(channel0-An0),

'bit1=1(starts right away),bit0=1(on operation)

ANSEL=%00010001   'AN0 of A/D used

CMCON0=7          'turns off comparator function

TRISA=%00001001   '0-5.0V input from divider, input from Reset

TRISC=%00000000   'all are outputs

 

ServOUT var PORTA.1 'Servomotor driving signal out

A0 var PORTA.0

Vin var Word

PW var Byte  'PW=Pulse width

 

Loop:

Vin=0

Adcin A0, Vin  'Read ADC ch-0 and store it in Vin

IF Vin<=25 Then Jump1  '15 interval    'Jump to one of the following points depending on Vin value obtained

IF (Vin>25) and (Vin=<40) Then Jump2   '0 - 5.0V // 1024, 1024 steps/25=40.96 -> 1024 were divided to 41 classes

If (Vin>40) and (Vin=<55) Then Jump3

IF (Vin>55) and (Vin=<70) Then Jump4

IF (Vin>70) and (Vin=<85) Then Jump5

IF (Vin>85) and (Vin=<100) Then Jump6

IF (Vin>100) and (Vin=<115) Then Jump7

IF (Vin>115) and (Vin=<130) Then Jump8

IF (Vin>130) and (Vin=<145) Then Jump9

IF (Vin>145) and (Vin=<160) Then Jump10

IF (Vin>160) and (Vin=<175) Then Jump11

IF (Vin>175) and (Vin=<190) Then Jump12

IF (Vin>190) and (Vin=<205) Then Jump13

IF (Vin>205) and (Vin=<220) Then Jump14

IF (Vin>220) and (Vin=<235) Then Jump15

IF (Vin>235) and (Vin=<250) Then Jump16

IF (Vin>250) and (Vin=<265) Then Jump17

IF (Vin>265) and (Vin=<280) Then Jump18

IF (Vin>280) and (Vin=<295) Then Jump19

IF (Vin>295) and (Vin=<310) Then Jump20

IF (Vin>310) and (Vin=<325) Then Jump21

IF (Vin>325) and (Vin=<340) Then Jump22

IF (Vin>340) and (Vin=<355) Then Jump23

IF (Vin>355) and (Vin=<370) Then Jump24

IF (Vin>370) and (Vin=<385) Then Jump25

IF (Vin>385) and (Vin=<400) Then Jump26

IF (Vin>400) and (Vin=<415) Then Jump27

IF (Vin>415) and (Vin=<430) Then Jump28

IF (Vin>430) and (Vin=<445) Then Jump29

IF (Vin>445) and (Vin=<460) Then Jump30

IF (Vin>460) and (Vin=<475) Then Jump31

IF (Vin>475) and (Vin=<490) Then Jump32

IF (Vin>490) and (Vin=<505) Then Jump33

IF (Vin>505) and (Vin=<520) Then Jump34

IF (Vin>520) and (Vin=<535) Then Jump35

IF (Vin>535) and (Vin=<550) Then Jump36

IF (Vin>550) and (Vin=<565) Then Jump37

IF (Vin>565) and (Vin=<580) Then Jump38

IF (Vin>580) and (Vin=<595) Then Jump39

IF (Vin>595) and (Vin=<610) Then Jump40

IF (Vin>610) and (Vin=<625) Then Jump41

IF (Vin>625) and (Vin=<640) Then Jump42

IF (Vin>640) and (Vin=<655) Then Jump43

IF (Vin>655) and (Vin=<670) Then Jump44

IF (Vin>670) and (Vin=<685) Then Jump45

IF (Vin>685) and (Vin=<700) Then Jump46

IF (Vin>700) and (Vin=<715) Then Jump47

IF (Vin>715) and (Vin=<730) Then Jump48

IF (Vin>730) and (Vin=<745) Then Jump49

IF (Vin>745) and (Vin=<760) Then Jump50

IF (Vin>760) and (Vin=<775) Then Jump51

IF (Vin>775) and (Vin=<790) Then Jump52

IF (Vin>790) and (Vin=<805) Then Jump53

IF (Vin>805) and (Vin=<820) Then Jump54

IF (Vin>820) and (Vin=<835) Then Jump55

IF (Vin>835) and (Vin=<850) Then Jump56

IF (Vin>850) and (Vin=<865) Then Jump57

IF (Vin>865) and (Vin=<880) Then Jump58

IF (Vin>880) and (Vin=<895) Then Jump59

IF (Vin>895) and (Vin=<910) Then Jump60

IF (Vin>910) and (Vin=<925) Then Jump61

IF (Vin>925) and (Vin=<940) Then Jump62

IF (Vin>940) and (Vin=<955) Then Jump63

IF (Vin>955) and (Vin=<970) Then Jump64

IF (Vin>970) and (Vin=<985) Then Jump65

IF (Vin>985) and (Vin=<1000) Then Jump66

IF Vin>1000 Then Jump67

 

Jump1:

PW=50   'PW=Actual pulse width/10, so 50 is equal to 500us=0.50ms  

Gosub PulsMake  'Goto Subroutine PulsMake

Goto Loop

Jump2:

PW=53   'increment=3(=0.03ms) added to the above base value (=0.50ms)

Gosub PulsMake

Goto Loop

Jump3:PW=56:Gosub PulsMake:Goto Loop

Jump4:PW=59:Gosub PulsMake:Goto Loop

Jump5:PW=62:Gosub PulsMake:Goto Loop

Jump6:PW=65:Gosub PulsMake:Goto Loop

Jump7:PW=68:Gosub PulsMake:Goto Loop

Jump8:PW=71:Gosub PulsMake:Goto Loop

Jump9:PW=74:Gosub PulsMake:Goto Loop

Jump10:PW=77:Gosub PulsMake:Goto Loop

Jump11:PW=80:Gosub PulsMake:Goto Loop

Jump12:PW=83:Gosub PulsMake:Goto Loop

Jump13:PW=86:Gosub PulsMake:Goto Loop

Jump14:PW=89:Gosub PulsMake:Goto Loop

Jump15:PW=92:Gosub PulsMake:Goto Loop

Jump16:PW=95:Gosub PulsMake:Goto Loop

Jump17:PW=98:Gosub PulsMake:Goto Loop

Jump18:PW=101:Gosub PulsMake:Goto Loop

Jump19:PW=104:Gosub PulsMake:Goto Loop

Jump20:PW=107:Gosub PulsMake:Goto Loop

Jump21:PW=110:Gosub PulsMake:Goto Loop

Jump22:PW=113:Gosub PulsMake:Goto Loop

Jump23:PW=116:Gosub PulsMake:Goto Loop

Jump24:PW=119:Gosub PulsMake:Goto Loop

Jump25:PW=122:Gosub PulsMake:Goto Loop

Jump26:PW=125:Gosub PulsMake:Goto Loop

Jump27:PW=128:Gosub PulsMake:Goto Loop

Jump28:PW=131:Gosub PulsMake:Goto Loop

Jump29:PW=134:Gosub PulsMake:Goto Loop

Jump30:PW=137:Gosub PulsMake:Goto Loop

Jump31:PW=140:Gosub PulsMake:Goto Loop

Jump32:PW=143:Gosub PulsMake:Goto Loop

Jump33:PW=146:Gosub PulsMake:Goto Loop

Jump34:PW=149:Gosub PulsMake:Goto Loop

Jump35:PW=152:Gosub PulsMake:Goto Loop

Jump36:PW=155:Gosub PulsMake:Goto Loop

Jump37:PW=158:Gosub PulsMake:Goto Loop

Jump38:PW=161:Gosub PulsMake:Goto Loop

Jump39:PW=164:Gosub PulsMake:Goto Loop

Jump40:PW=167:Gosub PulsMake:Goto Loop

Jump41:PW=170:Gosub PulsMake:Goto Loop

Jump42:PW=173:Gosub PulsMake:Goto Loop

Jump43:PW=176:Gosub PulsMake:Goto Loop

Jump44:PW=179:Gosub PulsMake:Goto Loop

Jump45:PW=182:Gosub PulsMake:Goto Loop

Jump46:PW=185:Gosub PulsMake:Goto Loop

Jump47:PW=188:Gosub PulsMake:Goto Loop

Jump48:PW=191:Gosub PulsMake:Goto Loop

Jump49:PW=194:Gosub PulsMake:Goto Loop

Jump50:PW=197:Gosub PulsMake:Goto Loop

Jump51:PW=200:Gosub PulsMake:Goto Loop

Jump52:PW=203:Gosub PulsMake:Goto Loop

Jump53:PW=206:Gosub PulsMake:Goto Loop

Jump54:PW=209:Gosub PulsMake:Goto Loop

Jump55:PW=212:Gosub PulsMake:Goto Loop

Jump56:PW=215:Gosub PulsMake:Goto Loop

Jump57:PW=218:Gosub PulsMake:Goto Loop

Jump58:PW=221:Gosub PulsMake:Goto Loop

Jump59:PW=224:Gosub PulsMake:Goto Loop

Jump60:PW=227:Gosub PulsMake:Goto Loop

Jump61:PW=230:Gosub PulsMake:Goto Loop

Jump62:PW=233:Gosub PulsMake:Goto Loop

Jump63:PW=236:Gosub PulsMake:Goto Loop

Jump64:PW=239:Gosub PulsMake:Goto Loop

Jump65:PW=242:Gosub PulsMake:Goto Loop

Jump66:PW=245:Gosub PulsMake:Goto Loop

Jump67:PW=248:Gosub PulsMake:Goto Loop   'Actual pulse width=2.48ms

 

PulsMake:  'Subroutine

Pulsout ServOUT, PW    'Making a pulse with a width of PW*10us

Pauseus (20000-PW*10)  'Generating an 0V-level interval of (20000us - PW*10us) after the previous pulse

Return

End

'***** End of Program ******

'Note:                                                                                             

   'melabs U2 Programmer's configurations  12F683

'FOSC – INTOSCIO; WDTE - Enable; Power-up Timer – Enable;  'MCLRpin – Reset ; BODEN - Enable; Internal External Switch Over – Enable,

'Fail-Safe Clock Monitor – Enable, Code - Disable; Data EPPROM – Disable

 

 

ボリュームの回転と同期してサーボモーターを180度回転させるプログラム - 分解能を100に向上(2015.3.3追加) (

サーボモーターの動きの解像度をさらに上げる目的で、プログラムの分離クラス数を前の67から100に増加した(180度/100クラス=1.8度/クラス)。したがって、ボリュームを回すとモーター軸は1.8度づつ回転位置を変える(前の分離クラス数67の場合は約2.7度)。実際ボリュームを回すとより細かく動作することが実感出来る。ボリュームを急速に回した時のモーター軸の追従性も前とほとんど変わらない。回路図は前と同じ。ADCが一生懸命動作していることが分かる。ただ、停止位置によって多少不安定さ(振動など)が時々出るようになった。プログラムが長く(メモリ4Kの内3508語使用)作成が大変だった。

  (プログラムの一部修正2015.4.3)オシロで出力波幅(PW)と周期を検討したところ、ボリュームを回してPWを大きくしていくと周期も設定した20msを超えて30ms近くまで大きくなることが分かった。原因はPWが大きくなるほどプログラムの中の通過するステップ数が増え時間がかかってしまうためである。そこでPWに関連づけた補正が必要になる。具体的には従来のPauseus (20000-PW*10)に補正項 (PW-50)*47 を加えてPauseus ((20000-(PW*10))-(PW-50)*47)として使用した。ここでPW-50の50はPWの初期値で、47はオシロで波形を見ながら実験的に決めた値である。PWの全域で周期が20msに保たれることが分かった。下表の(Table 1)はADC(RA0)への入力電圧をボリュームを回して変え波幅を変えた時の周期を、補正なし、補正ありで測定し比較したものである。波幅はほぼ設定値になっていたが、周期の補正なしの場合は波幅の増加とともに周期が大きくなることが分かる。一方、補正を加えると周期はほぼ20ms強と一定になることが分かる

 

(Table 1)

ADCへの入力電圧波幅周期の間の関係

Relationship among ADC Input Voltage, Pulse Width (PW) and Cycle

ADCへの入力電圧

Input voltage to ADC

波幅

Pulse Width (PW)

周期 (補正なし)*

* Cycle with no correction

周期 (補正あり)**

** Cycle with correction

5.0V

2.47ms

29.85ms

20.88ms

4.0V

2.10ms

28.12ms

20.84ms

3.0V

1.69ms

26.19ms

20.80ms

2.0V

1.27ms

24.13ms

20.75ms

1.5V

1.07ms

23.19ms

20.68ms

1.0V

0.89ms

22.24ms

20.68ms

0.75V

0.77ms

21.75ms

20.72ms

0.50V

0.67ms

21.17ms

20.64ms

0.26V

0.59ms

20.81ms

20.64ms

ADC=Analog to Digital Converter (RA0) of 16F688

Program used for this experiment: Servomotor16F688PotG.bas

* Pauseus (20000-PW*10) が使われた場   * when Pauseus (20000-PW*10) is used

** Pauseus ((20000-(PW*10))-(PW-50)*47) が使われた場合   ** when Pauseus ((20000-(PW*10))-(PW-50)*47) is used

 

Program (Newly added on 2015.3.3)

In this new program, the 180-degree rotary angle of servo motor axis was divided into 100 classes (180 degrees/100 classes=1.80 degrees/class), instead of 67 classes in the previous program. As a result, it's rotation is finer and smoother though the rotary stability somewhat lessens. The followability of motor axis to the rotation of VR seems no different from that in the previous case (where 180 degrees were divided into 67 classes). The same circuit diagram as above (Fig. 3) was used for the new program. Because the program is unfortunately long and not sophisticated, it consumed 3508 words out of 4K memories of 16F688.

(Program partly modified on 2015.4.3)  It was found that the cycle (supposed to be 20ms) increased up to 30ms as PW was increased by turning VR. To solve this problem, a correction was made to the program by changing the existing Pauseus (20000-PW*10) to Pauseus ((20000-(PW*10))-(PW-50)*47). Here, 50 of (PW-50) is the starting PW value while 47 was experimentally decided. The result is satisfactory, keeping the cycle at about 20ms over the entire PW range. The above Table 1 shows the relationship among ADC Input Voltage, Pulse Width (PW) and Cycle. With the application of correction, the cycle values become almost constant or little more than 20ms over PW values from 0.59ms to 2.47ms.

 

'**************************************************

'Servomotor16F688PotG.bas  2015.2.18 2015.2.19 2015.2.24 2015.3.2  2015.4.3  correction applied works OK!

'Division increased up to 100 (2015.3.2)

'16F688 with internal clock 4MHz

'PicBasic Pro V2.50A used

'Servo motor used: SG92R

'Using PBP's PAUSEUS command (2015.2.15)

'******* Pin connection *********

'Pin 1: Vdd (5V)

'Pin 14: Vss (GND)

'Pin 13: PORTA.0: input - connected to VR=100K (analog value input)

'Pin 12: PORTA.1: output - connected to Signal input of Servomotor

'Pin 4: PORTA.3: input - connected to Reset

'All other pins - NC

'******* Program *********

Define ADC_BITS 10     '10-bit ADC used

Define ADC_CLOCK 3     'Internal clock used

Define ADC_SAMPLEUS 50 'Sample timing

ADCON0=%10000011 

'=131 (decimal) 'bit7=1(right justified),bit6=0(volt refer-Vdd),

'bit5-4=0(unimplemented),bit3-2=00(channel0-An0),

'bit1=1(starts right away),bit0=1(on operation)

ANSEL=%00010001   'AN0 of A/D used

CMCON0=7          'turns off comparator function

TRISA=%00001001   '0-5.0V input from divider, input from Reset

TRISC=%00000000   'all are outputs

 

ServOUT var PORTA.1 'Servomotor driving signal out

A0 var PORTA.0

Vin var word

PW var byte  'PW=Pulse width

 

Lp:  'Lp=Loop

Vin=0

Adcin A0, Vin  'Read ADC ch-0 and store it in Vin

IF Vin=<20 Then Jp1  'Jump to Jp1

IF (Vin>20) and (Vin=<30) Then Jp2  '10 interval

IF (Vin>30) and (Vin=<40) Then Jp3   '0 - 5.0V // 1024, 1024 steps/10=102.4 -> 1024 were divided to 100 classes

IF (Vin>40) and (Vin=<50) Then Jp4

IF (Vin>50) and (Vin=<60) Then Jp5

IF (Vin>60) and (Vin=<70) Then Jp6

IF (Vin>70) and (Vin=<80) Then Jp7

IF (Vin>80) and (Vin=<90) Then Jp8

IF (Vin>90) and (Vin=<100) Then Jp9

IF (Vin>100) and (Vin=<110) Then Jp10

IF (Vin>110) and (Vin=<120) Then Jp11

IF (Vin>120) and (Vin=<130) Then Jp12

IF (Vin>130) and (Vin=<140) Then Jp13

IF (Vin>140) and (Vin=<150) Then Jp14

IF (Vin>150) and (Vin=<160) Then Jp15

IF (Vin>160) and (Vin=<170) Then Jp16

IF (Vin>170) and (Vin=<180) Then Jp17

IF (Vin>180) and (Vin=<190) Then Jp18

IF (Vin>190) and (Vin=<200) Then Jp19

IF (Vin>200) and (Vin=<210) Then Jp20

IF (Vin>210) and (Vin=<220) Then Jp21

IF (Vin>220) and (Vin=<230) Then Jp22

IF (Vin>230) and (Vin=<240) Then Jp23

IF (Vin>240) and (Vin=<250) Then Jp24

IF (Vin>250) and (Vin=<260) Then Jp25

IF (Vin>260) and (Vin=<270) Then Jp26

IF (Vin>270) and (Vin=<280) Then Jp27

IF (Vin>280) and (Vin=<290) Then Jp28

IF (Vin>290) and (Vin=<300) Then Jp29

IF (Vin>300) and (Vin=<310) Then Jp30

IF (Vin>310) and (Vin=<320) Then Jp31

IF (Vin>320) and (Vin=<330) Then Jp32

IF (Vin>330) and (Vin=<340) Then Jp33

IF (Vin>340) and (Vin=<350) Then Jp34

IF (Vin>350) and (Vin=<360) Then Jp35

IF (Vin>360) and (Vin=<370) Then Jp36

IF (Vin>370) and (Vin=<380) Then Jp37

IF (Vin>380) and (Vin=<390) Then Jp38

IF (Vin>390) and (Vin=<400) Then Jp39

IF (Vin>400) and (Vin=<410) Then Jp40

IF (Vin>410) and (Vin=<420) Then Jp41

IF (Vin>420) and (Vin=<430) Then Jp42

IF (Vin>430) and (Vin=<440) Then Jp43

IF (Vin>440) and (Vin=<450) Then Jp44

IF (Vin>450) and (Vin=<460) Then Jp45

IF (Vin>460) and (Vin=<470) Then Jp46

IF (Vin>470) and (Vin=<480) Then Jp47

IF (Vin>480) and (Vin=<490) Then Jp48

IF (Vin>490) and (Vin=<500) Then Jp49

IF (Vin>500) and (Vin=<510) Then Jp50

IF (Vin>510) and (Vin=<520) Then Jp51

IF (Vin>520) and (Vin=<530) Then Jp52

IF (Vin>530) and (Vin=<540) Then Jp53

IF (Vin>540) and (Vin=<550) Then Jp54

IF (Vin>550) and (Vin=<560) Then Jp55

IF (Vin>560) and (Vin=<570) Then Jp56

IF (Vin>570) and (Vin=<580) Then Jp57

IF (Vin>580) and (Vin=<590) Then Jp58

IF (Vin>590) and (Vin=<600) Then Jp59

IF (Vin>600) and (Vin=<610) Then Jp60

IF (Vin>610) and (Vin=<620) Then Jp61

IF (Vin>620) and (Vin=<630) Then Jp62

IF (Vin>630) and (Vin=<640) Then Jp63

IF (Vin>640) and (Vin=<650) Then Jp64

IF (Vin>650) and (Vin=<660) Then Jp65

IF (Vin>660) and (Vin=<670) Then Jp66

IF (Vin>670) and (Vin=<680) Then Jp67

IF (Vin>680) and (Vin=<690) Then Jp68

IF (Vin>690) and (Vin=<700) Then Jp69

IF (Vin>700) and (Vin=<710) Then Jp70

IF (Vin>710) and (Vin=<720) Then Jp71

IF (Vin>720) and (Vin=<730) Then Jp72

IF (Vin>730) and (Vin=<740) Then Jp73

IF (Vin>740) and (Vin=<750) Then Jp74

IF (Vin>750) and (Vin=<760) Then Jp75

IF (Vin>760) and (Vin=<770) Then Jp76

IF (Vin>770) and (Vin=<780) Then Jp77

IF (Vin>780) and (Vin=<790) Then Jp78

IF (Vin>790) and (Vin=<800) Then Jp79

IF (Vin>800) and (Vin=<810) Then Jp80

IF (Vin>810) and (Vin=<820) Then Jp81

IF (Vin>820) and (Vin=<830) Then Jp82

IF (Vin>830) and (Vin=<840) Then Jp83

IF (Vin>840) and (Vin=<850) Then Jp84

IF (Vin>850) and (Vin=<860) Then Jp85

IF (Vin>860) and (Vin=<870) Then Jp86

IF (Vin>870) and (Vin=<880) Then Jp87

IF (Vin>880) and (Vin=<890) Then Jp88

IF (Vin>890) and (Vin=<900) Then Jp89

IF (Vin>900) and (Vin=<910) Then Jp90

IF (Vin>910) and (Vin=<920) Then Jp91

IF (Vin>920) and (Vin=<930) Then Jp92

IF (Vin>930) and (Vin=<940) Then Jp93

IF (Vin>940) and (Vin=<950) Then Jp94

IF (Vin>950) and (Vin=<960) Then Jp95

IF (Vin>960) and (Vin=<970) Then Jp96

IF (Vin>970) and (Vin=<980) Then Jp97

IF (Vin>980) and (Vin=<990) Then Jp98

IF (Vin>990) and (Vin=<1000) Then Jp99

IF Vin>1000 Then Jp100

 

Jp1:PW=50:Gosub PM:Goto LP  'increment=2(=0.02ms) added to the base value (PW=50 is equal to 500us=0.50ms(pulse width))

Jp2:PW=52:Gosub PM:Goto LP

Jp3:PW=54:Gosub PM:Goto LP

Jp4:PW=56:Gosub PM:Goto LP

Jp5:PW=58:Gosub PM:Goto LP

Jp6:PW=60:Gosub PM:Goto LP

Jp7:PW=62:Gosub PM:Goto LP

Jp8:PW=64:Gosub PM:Goto LP

Jp9:PW=66:Gosub PM:Goto LP

Jp10:PW=68:Gosub PM:Goto LP

Jp11:PW=70:Gosub PM:Goto LP

Jp12:PW=72:Gosub PM:Goto LP

Jp13:PW=74:Gosub PM:Goto LP

Jp14:PW=76:Gosub PM:Goto LP

Jp15:PW=78:Gosub PM:Goto LP

Jp16:PW=80:Gosub PM:Goto LP

Jp17:PW=82:Gosub PM:Goto LP

Jp18:PW=84:Gosub PM:Goto LP

Jp19:PW=86:Gosub PM:Goto LP

Jp20:PW=88:Gosub PM:Goto LP

Jp21:PW=90:Gosub PM:Goto LP

Jp22:PW=92:Gosub PM:Goto LP

Jp23:PW=94:Gosub PM:Goto LP

Jp24:PW=96:Gosub PM:Goto LP

Jp25:PW=98:Gosub PM:Goto LP

Jp26:PW=100:Gosub PM:Goto LP

Jp27:PW=102:Gosub PM:Goto LP

Jp28:PW=104:Gosub PM:Goto LP

Jp29:PW=106:Gosub PM:Goto LP

Jp30:PW=108:Gosub PM:Goto LP

Jp31:PW=110:Gosub PM:Goto LP

Jp32:PW=112:Gosub PM:Goto LP

Jp33:PW=114:Gosub PM:Goto LP

Jp34:PW=116:Gosub PM:Goto LP

Jp35:PW=118:Gosub PM:Goto LP

Jp36:PW=120:Gosub PM:Goto LP

Jp37:PW=122:Gosub PM:Goto LP

Jp38:PW=124:Gosub PM:Goto LP

Jp39:PW=126:Gosub PM:Goto LP

Jp40:PW=128:Gosub PM:Goto LP

Jp41:PW=130:Gosub PM:Goto LP

Jp42:PW=132:Gosub PM:Goto LP

Jp43:PW=134:Gosub PM:Goto LP

Jp44:PW=136:Gosub PM:Goto LP

Jp45:PW=138:Gosub PM:Goto LP

Jp46:PW=140:Gosub PM:Goto LP

Jp47:PW=142:Gosub PM:Goto LP

Jp48:PW=144:Gosub PM:Goto LP

Jp49:PW=146:Gosub PM:Goto LP

Jp50:PW=148:Gosub PM:Goto LP

Jp51:PW=150:Gosub PM:Goto LP   'PW=150 is equal to 1500us=1.50ms (Central pulse width)

Jp52:PW=152:Gosub PM:Goto LP

Jp53:PW=154:Gosub PM:Goto LP

Jp54:PW=156:Gosub PM:Goto LP

Jp55:PW=158:Gosub PM:Goto LP

Jp56:PW=160:Gosub PM:Goto LP

Jp57:PW=162:Gosub PM:Goto LP

Jp58:PW=164:Gosub PM:Goto LP

Jp59:PW=166:Gosub PM:Goto LP

Jp60:PW=168:Gosub PM:Goto LP

Jp61:PW=170:Gosub PM:Goto LP

Jp62:PW=172:Gosub PM:Goto LP

Jp63:PW=174:Gosub PM:Goto LP

Jp64:PW=176:Gosub PM:Goto LP

Jp65:PW=178:Gosub PM:Goto LP

Jp66:PW=180:Gosub PM:Goto LP

Jp67:PW=182:Gosub PM:Goto LP

Jp68:PW=184:Gosub PM:Goto LP

Jp69:PW=186:Gosub PM:Goto LP

Jp70:PW=188:Gosub PM:Goto LP

Jp71:PW=190:Gosub PM:Goto LP

Jp72:PW=192:Gosub PM:Goto LP

Jp73:PW=194:Gosub PM:Goto LP

Jp74:PW=196:Gosub PM:Goto LP

Jp75:PW=198:Gosub PM:Goto LP

Jp76:PW=200:Gosub PM:Goto LP

Jp77:PW=202:Gosub PM:Goto LP

Jp78:PW=204:Gosub PM:Goto LP

Jp79:PW=206:Gosub PM:Goto LP

Jp80:PW=208:Gosub PM:Goto LP

Jp81:PW=210:Gosub PM:Goto LP

Jp82:PW=212:Gosub PM:Goto LP

Jp83:PW=214:Gosub PM:Goto LP

Jp84:PW=216:Gosub PM:Goto LP

Jp85:PW=218:Gosub PM:Goto LP

Jp86:PW=220:Gosub PM:Goto LP

Jp87:PW=222:Gosub PM:Goto LP

Jp88:PW=224:Gosub PM:Goto LP

Jp89:PW=226:Gosub PM:Goto LP

Jp90:PW=228:Gosub PM:Goto LP

Jp91:PW=230:Gosub PM:Goto LP

Jp92:PW=232:Gosub PM:Goto LP

Jp93:PW=234:Gosub PM:Goto LP

Jp94:PW=236:Gosub PM:Goto LP

Jp95:PW=238:Gosub PM:Goto LP

Jp96:PW=240:Gosub PM:Goto LP

Jp97:PW=242:Gosub PM:Goto LP

Jp98:PW=244:Gosub PM:Goto LP

Jp99:PW=246:Gosub PM:Goto LP

Jp100:PW=248:Gosub PM:Goto LP  'PW=248 is equal to 2480us=2.48ms (pulse width)

 

PM:  'Subroutine  PM=PulseMake

Pulsout ServOUT, PW    'Making a pulse with a width of PW*10us

'Pauseus (20000-PW*10)  'was used, instead of the below, but later found that the cycle became wider (over 20ms) as the pulse width (PW) increased, requiring a correction

Pauseus ((20000-(PW*10))-(PW-50)*47)  ' (PW-50)*47 is a correction that is used to make a cycle equal to almost 20ms. The figure 47 was experimentally determined.

Return

End

'***** End of Program ******

'Note:                                                                                             

     'melabs U2 Programmer's configurations  12F683

'FOSC – INTOSCIO; WDTE - Enable; Power-up Timer – Enable;  'MCLRpin – Reset ; BODEN - Enable; Internal External Switch Over – Enable,

'Fail-Safe Clock Monitor – Enable, Code - Disable; Data EPPROM – Disable

 

 

'上のServomotor16F688PotG.basを多少簡易化したプログラム(2015.3.27追加)

  上のプログラムがあまりに縦に長いのでラベルに使う英文字をできるだけ短くして書き換えた。機能は全く変わらない。PICのメモリ消費数が上のプログラムに比べ減るかと思ったが逆で、内蔵4Kの内3998語を消費していた。

  (プログラムの一部修正2015.4.3)オシロで出力波幅(PW)と周期を検討したところ、ボリュームを回してPWを大きくしていくと周期も設定した20msを超えて30ms近くまで大きくなることが分かった。原因はPWが大きくなるほどプログラムの中の通過するステップ数が増え時間がかかってしまうためである。そこでPWに関連づけた補正が必要になる。具体的には従来のPauseus (20000-PW*10)に補正項PW-50)*51 を加えてPauseus ((20000-(PW*10))-(PW-50)*51)として使用した。ここで51はオシロで波形を見ながら実験的に決めた。PWの全域で周期が20msに保たれることが分かった。

 

Program (Slightly simplified from Servomotor16F688PotG.bas on 2015.3.27)

  This program Servomotor16F688PotGmod.bas is a simplified version of the above 'Servomotor16F688PotG.bas. There is no functional difference between the two. Contrary to my expectations, this program was found to consume 3998 words out of 4K memory of 16F688, more than the above (3508 words). However, the program is shorter and its listing looks tidier.

(Program partly modified on 2015.4.3) It was found that the cycle (supposed to be 20ms) increased up to 30ms as PW was increased by turning VR. To solve this problem, a correction was made to the program by changing the existing Pauseus (20000-PW*10) to Pauseus ((20000-(PW*10))-(PW-50)*51). Here, 51 was experimentally decided. The result is satisfactory, keeping the cycle at about 20ms over the entire PW range.

 

'**************************************************

'Servomotor16F688PotGmod.bas  2015.3.27  2015.4.3 correction applied works OK!

'Division increased up to 100 (2015.3.2)

'16F688 with internal clock 4MHz

'PicBasic Pro V2.50A used

'Servo motor used: SG92R

'Using PBP's PAUSEUS command (2015.2.15)

'******* Pin connection *********

'Pin 1: Vdd (5V)

'Pin 14: Vss (GND)

'Pin 13: PORTA.0: input - connected to VR=100K (analog value input)

'Pin 12: PORTA.1: output - connected to Signal input of Servomotor

'Pin 4: PORTA.3: input - connected to Reset

'All other pins - NC

'******* Program *********

Define ADC_BITS 10     '10-bit ADC used

Define ADC_CLOCK 3     'Internal clock used

Define ADC_SAMPLEUS 50 'Sample timing

ADCON0=%10000011 

'=131 (decimal) 'bit7=1(right justified),bit6=0(volt refer-Vdd),

'bit5-4=0(unimplemented),bit3-2=00(channel0-An0),

'bit1=1(starts right away),bit0=1(on operation)

ANSEL=%00010001   'AN0 of A/D used

CMCON0=7          'turns off comparator function

TRISA=%00001001   '0-5.0V input from divider, input from Reset

TRISC=%00000000   'all are outputs

 

ServOUT var PORTA.1 'Servomotor driving signal out

A0 var PORTA.0

Vin var word

PW var byte  'PW=Pulse width

 

Lp:  'Lp=Loop                    

Vin=0                             

Adcin A0, Vin  'Read ADC ch-0 and store it in Vin       'PW=50 is equal to 500us=0.50ms(pulse width)

IF Vin=<20 Then PW=50:Gosub PM:Goto LP                  'increment of step =10

IF (Vin>20) and (Vin=<30) Then PW=52:Gosub PM:Goto LP   '0 - 5.0V // 1024 steps, -> 1024 were divided to 100 classes

IF (Vin>30) and (Vin=<40) Then PW=54:Gosub PM:Goto LP   'increment of PW =2(=0.02ms) added to the base value

IF (Vin>40) and (Vin=<50) Then PW=56:Gosub PM:Goto LP

IF (Vin>50) and (Vin=<60) Then PW=58:Gosub PM:Goto LP

IF (Vin>60) and (Vin=<70) Then PW=60:Gosub PM:Goto LP

IF (Vin>70) and (Vin=<80) Then PW=62:Gosub PM:Goto LP

IF (Vin>80) and (Vin=<90) Then PW=64:Gosub PM:Goto LP

IF (Vin>90) and (Vin=<100) Then PW=66:Gosub PM:Goto LP

IF (Vin>100) and (Vin=<110) Then PW=68:Gosub PM:Goto LP

IF (Vin>110) and (Vin=<120) Then PW=70:Gosub PM:Goto LP

IF (Vin>120) and (Vin=<130) Then PW=72:Gosub PM:Goto LP

IF (Vin>130) and (Vin=<140) Then PW=74:Gosub PM:Goto LP

IF (Vin>140) and (Vin=<150) Then PW=76:Gosub PM:Goto LP

IF (Vin>150) and (Vin=<160) Then PW=78:Gosub PM:Goto LP

IF (Vin>160) and (Vin=<170) Then PW=80:Gosub PM:Goto LP

IF (Vin>170) and (Vin=<180) Then PW=82:Gosub PM:Goto LP

IF (Vin>180) and (Vin=<190) Then PW=84:Gosub PM:Goto LP

IF (Vin>190) and (Vin=<200) Then PW=86:Gosub PM:Goto LP

IF (Vin>200) and (Vin=<210) Then PW=88:Gosub PM:Goto LP

IF (Vin>210) and (Vin=<220) Then PW=90:Gosub PM:Goto LP

IF (Vin>220) and (Vin=<230) Then PW=92:Gosub PM:Goto LP

IF (Vin>230) and (Vin=<240) Then PW=94:Gosub PM:Goto LP

IF (Vin>240) and (Vin=<250) Then PW=96:Gosub PM:Goto LP

IF (Vin>250) and (Vin=<260) Then PW=98:Gosub PM:Goto LP

IF (Vin>260) and (Vin=<270) Then PW=100:Gosub PM:Goto LP

IF (Vin>270) and (Vin=<280) Then PW=102:Gosub PM:Goto LP

IF (Vin>280) and (Vin=<290) Then PW=104:Gosub PM:Goto LP

IF (Vin>290) and (Vin=<300) Then PW=106:Gosub PM:Goto LP

IF (Vin>300) and (Vin=<310) Then PW=108:Gosub PM:Goto LP

IF (Vin>310) and (Vin=<320) Then PW=110:Gosub PM:Goto LP

IF (Vin>320) and (Vin=<330) Then PW=112:Gosub PM:Goto LP

IF (Vin>330) and (Vin=<340) Then PW=114:Gosub PM:Goto LP

IF (Vin>340) and (Vin=<350) Then PW=116:Gosub PM:Goto LP

IF (Vin>350) and (Vin=<360) Then PW=118:Gosub PM:Goto LP

IF (Vin>360) and (Vin=<370) Then PW=120:Gosub PM:Goto LP

IF (Vin>370) and (Vin=<380) Then PW=122:Gosub PM:Goto LP

IF (Vin>380) and (Vin=<390) Then PW=124:Gosub PM:Goto LP

IF (Vin>390) and (Vin=<400) Then PW=126:Gosub PM:Goto LP

IF (Vin>400) and (Vin=<410) Then PW=128:Gosub PM:Goto LP

IF (Vin>410) and (Vin=<420) Then PW=130:Gosub PM:Goto LP

IF (Vin>420) and (Vin=<430) Then PW=132:Gosub PM:Goto LP

IF (Vin>430) and (Vin=<440) Then PW=134:Gosub PM:Goto LP

IF (Vin>440) and (Vin=<450) Then PW=136:Gosub PM:Goto LP

IF (Vin>450) and (Vin=<460) Then PW=138:Gosub PM:Goto LP

IF (Vin>460) and (Vin=<470) Then PW=140:Gosub PM:Goto LP

IF (Vin>470) and (Vin=<480) Then PW=142:Gosub PM:Goto LP

IF (Vin>480) and (Vin=<490) Then PW=144:Gosub PM:Goto LP

IF (Vin>490) and (Vin=<500) Then PW=146:Gosub PM:Goto LP

IF (Vin>500) and (Vin=<510) Then PW=148:Gosub PM:Goto LP

IF (Vin>510) and (Vin=<520) Then PW=150:Gosub PM:Goto LP  'PW=150 is equal to 1500us=1.50ms (Central pulse width)

IF (Vin>520) and (Vin=<530) Then PW=152:Gosub PM:Goto LP

IF (Vin>530) and (Vin=<540) Then PW=154:Gosub PM:Goto LP

IF (Vin>540) and (Vin=<550) Then PW=156:Gosub PM:Goto LP

IF (Vin>550) and (Vin=<560) Then PW=158:Gosub PM:Goto LP

IF (Vin>560) and (Vin=<570) Then PW=160:Gosub PM:Goto LP

IF (Vin>570) and (Vin=<580) Then PW=162:Gosub PM:Goto LP

IF (Vin>580) and (Vin=<590) Then PW=164:Gosub PM:Goto LP

IF (Vin>590) and (Vin=<600) Then PW=166:Gosub PM:Goto LP

IF (Vin>600) and (Vin=<610) Then PW=168:Gosub PM:Goto LP

IF (Vin>610) and (Vin=<620) Then PW=170:Gosub PM:Goto LP

IF (Vin>620) and (Vin=<630) Then PW=172:Gosub PM:Goto LP

IF (Vin>630) and (Vin=<640) Then PW=174:Gosub PM:Goto LP

IF (Vin>640) and (Vin=<650) Then PW=176:Gosub PM:Goto LP

IF (Vin>650) and (Vin=<660) Then PW=178:Gosub PM:Goto LP

IF (Vin>660) and (Vin=<670) Then PW=180:Gosub PM:Goto LP

IF (Vin>670) and (Vin=<680) Then PW=182:Gosub PM:Goto LP

IF (Vin>680) and (Vin=<690) Then PW=184:Gosub PM:Goto LP

IF (Vin>690) and (Vin=<700) Then PW=186:Gosub PM:Goto LP

IF (Vin>700) and (Vin=<710) Then PW=188:Gosub PM:Goto LP

IF (Vin>710) and (Vin=<720) Then PW=190:Gosub PM:Goto LP

IF (Vin>720) and (Vin=<730) Then PW=192:Gosub PM:Goto LP

IF (Vin>730) and (Vin=<740) Then PW=194:Gosub PM:Goto LP

IF (Vin>740) and (Vin=<750) Then PW=196:Gosub PM:Goto LP

IF (Vin>750) and (Vin=<760) Then PW=198:Gosub PM:Goto LP

IF (Vin>760) and (Vin=<770) Then PW=200:Gosub PM:Goto LP

IF (Vin>770) and (Vin=<780) Then PW=202:Gosub PM:Goto LP

IF (Vin>780) and (Vin=<790) Then PW=204:Gosub PM:Goto LP

IF (Vin>790) and (Vin=<800) Then PW=206:Gosub PM:Goto LP

IF (Vin>800) and (Vin=<810) Then PW=208:Gosub PM:Goto LP

IF (Vin>810) and (Vin=<820) Then PW=210:Gosub PM:Goto LP

IF (Vin>820) and (Vin=<830) Then PW=212:Gosub PM:Goto LP

IF (Vin>830) and (Vin=<840) Then PW=214:Gosub PM:Goto LP

IF (Vin>840) and (Vin=<850) Then PW=216:Gosub PM:Goto LP

IF (Vin>850) and (Vin=<860) Then PW=218:Gosub PM:Goto LP

IF (Vin>860) and (Vin=<870) Then PW=220:Gosub PM:Goto LP

IF (Vin>870) and (Vin=<880) Then PW=222:Gosub PM:Goto LP

IF (Vin>880) and (Vin=<890) Then PW=224:Gosub PM:Goto LP

IF (Vin>890) and (Vin=<900) Then PW=226:Gosub PM:Goto LP

IF (Vin>900) and (Vin=<910) Then PW=228:Gosub PM:Goto LP

IF (Vin>910) and (Vin=<920) Then PW=230:Gosub PM:Goto LP

IF (Vin>920) and (Vin=<930) Then PW=232:Gosub PM:Goto LP

IF (Vin>930) and (Vin=<940) Then PW=234:Gosub PM:Goto LP

IF (Vin>940) and (Vin=<950) Then PW=236:Gosub PM:Goto LP

IF (Vin>950) and (Vin=<960) Then PW=238:Gosub PM:Goto LP

IF (Vin>960) and (Vin=<970) Then PW=240:Gosub PM:Goto LP

IF (Vin>970) and (Vin=<980) Then PW=242:Gosub PM:Goto LP

IF (Vin>980) and (Vin=<990) Then PW=244:Gosub PM:Goto LP

IF (Vin>990) and (Vin=<1000) Then PW=246:Gosub PM:Goto LP

IF Vin>1000 Then PW=248:Gosub PM:Goto LP  'PW=248 is equal to 2480us=2.48ms (pulse width)

                           

PM:  'Subroutine  PM=PulseMake

Pulsout ServOUT, PW    'Making a pulse with a width of PW*10us

'Pauseus (20000-PW*10)  'was used, instead of the below, but later found that the cycle became wider (over 20ms) as the peak width (PW) increased, requiring a correction

Pauseus ((20000-(PW*10))-(PW-50)*51)   ' (PW-50)*51 is a correction that is used to make a cycle equal to almost 20ms. The figure 51 was experimentally determined.

Return

End

'***** End of Program ******

 

Menu Pageへ戻る    (Return to Menu Page)

Topへ戻る    (Return to Top of Page)