'
'*********************************************************
'
' R/C Servo Motor Operating Program for PPF-08i Ver.0.2
'
'                 ppf08_01.bas  2000/7/8 by K. Hirata
'
'*********************************************************
'
' PIC16F84 20MHz用
'
'***********************
' 初期設定
'***********************
'
Symbol ONtime=W0
Symbol T1=W1
Symbol T2=W2
Symbol T3=W3
Symbol Freq=W4
Symbol LL=W5
Symbol RR=W6
Symbol SpeedStep=W7
Symbol ModeNum=W8
Symbol Amplitude=W9
Symbol ModeSW=W10
Symbol ModeType=W11
Symbol i=W13
Symbol Ptime=20 'パルス周期(20ms)
Symbol ONtime_min=375 'ONの時間の最小値(375/(20/4)/100=0.75ms)
'Symbol ONtime_min=500 'ONの時間の最小値(500/(20/4)/100=1.00ms)
Symbol ONtime_max=1125 'ONの時間の最大値(562/(20/4)/100=2.25ms)
'
input 0: input 1: input 2: input 3
input 4: input 5: output 6: output 7
'
Poke $81,0 'プルアップ抵抗の設定
'
'***********************
' モード選択
'***********************
'
If pin0=0 then Loop_random 'ランダムモード
'If pin1=0 then Loop_sensor 'センサ感知モード
Goto Loop '運動設定モード
'
'************************************
' メインルーチン（運動設定モード）
'************************************
'
Loop:
ModeType=3
'
' 振幅，周波数の設定
'
ModeSW=0
If pin5=1 then Mode_pin5
ModeSW=1
Mode_pin5:
If pin4=1 then Mode_pin4
ModeSW=ModeSW+2
Mode_pin4:
If pin3=1 then Mode_pin3:
ModeSW=ModeSW+4
Mode_pin3:
'If pin2=1 then Mode_pin2:
'ModeSW=ModeSW+8
'Mode_pin2:
Branch ModeSW,(Mode0,Mode1,Mode2,Mode3,Mode4,Mode5,Mode6,Mode7)
Goto Mode0
Mode_end:
'
' 周波数の設定
'
If pin2=1 then LowSpeed
If pin2=0 then HighSpeed
Speed:
'
' 傾きの設定
'
If pin0=0 then LeftL
If pin1=0 then RightR
Goto Center
Leaning:
'
' 尾ひれの往復運動
'
For ONtime=T1 to T2 step SpeedStep 'ONの時間(/2.5usec)
  Pulsout 7, ONtime
  Pause 94 'OFFtime 'OFFの時間(94/5=18.8msec)
Next ONtime
T2=T2-1: T1=T1+1
For ONtime=T2 to T1 step -SpeedStep 'ONの時間(/2.5usec)
  Pulsout 7, ONtime
  Pause 94 'OFFtime 'OFFの時間(94/5=18.8msec)
Next ONtime
T2=T2+1: T1=T1-1
Goto Loop
End
'
'************************************
' メインルーチン（ランダムモード）
'************************************
'
Loop_random:
ModeType=1
'
' 振幅，周波数の設定
'
Random ModeNum
ModeSW=0
If ModeNum<50000 then Mode_pin5_rd
ModeSW=1
Mode_pin5_rd:
If ModeNum<40000 then Mode_pin4_rd
ModeSW=ModeSW+1
Mode_pin4_rd:
If ModeNum<30000 then Mode_pin3_rd:
ModeSW=ModeSW+1
Mode_pin3_rd:
If ModeNum<20000 then Mode_pin2_rd:
ModeSW=ModeSW+1
Mode_pin2_rd:
If ModeNum<10000 then Mode_pin1_rd:
ModeSW=ModeSW+1
Mode_pin1_rd:
Branch ModeSW,(Mode0,Mode1,Mode2,Mode3,Mode4,Mode5)
Goto Mode0
Mode_end_rd:
'
' 周波数の設定
'
Random ModeNum
If ModeNum>40000 then LowSpeed
Goto HighSpeed
Speed_rd:
'
' 傾きの設定
'
Random ModeNum
If ModeNum>40000 then LeftL
If ModeNum<20000 then RightR
Goto Center
Leaning_rd:
'
' 尾ひれの往復運動
'
For i=1 to 5
For ONtime=T1 to T2 step SpeedStep 'ONの時間(/2.5usec)
  Pulsout 7, ONtime
  Pause 94 'OFFtime 'OFFの時間(94/5=18.8msec)
Next ONtime
T2=T2-1: T1=T1+1
For ONtime=T2 to T1 step -SpeedStep 'ONの時間(/2.5usec)
  Pulsout 7, ONtime
  Pause 94 'OFFtime 'OFFの時間(94/5=18.8msec)
Next ONtime
T2=T2+1: T1=T1-1
Next i
Goto Loop_random
End
'
'******************************
' 振幅設定・サブルーチン
'******************************
'
' モード0
'
Mode0:
Amplitude=20 '振幅(0-100)
Freq=100 '周波数(/100Hz)
If ModeType=1 then Mode_end_rd
Goto Mode_end
'
' モード1
'
Mode1:
Amplitude=30 '振幅(0-100)
If ModeType=1 then Mode_end_rd
Goto Mode_end
'
' モード2
'
Mode2:
Amplitude=40 '振幅(0-100)
If ModeType=1 then Mode_end_rd
Goto Mode_end
'
' モード3
'
Mode3:
Amplitude=50 '振幅(0-100)
If ModeType=1 then Mode_end_rd
Goto Mode_end
'
' モード4
'
Mode4:
Amplitude=60 '振幅(0-100)
If ModeType=1 then Mode_end_rd
Goto Mode_end
'
' モード5
'
Mode5:
Amplitude=70 '振幅(0-100)
If ModeType=1 then Mode_end_rd
Goto Mode_end
'
' モード6
'
Mode6:
Amplitude=80 '振幅(0-100)
If ModeType=1 then Mode_end_rd
Goto Mode_end
'
' モード7
'
Mode7:
Amplitude=90 '振幅(0-100)
If ModeType=1 then Mode_end_rd
Goto Mode_end
'
'******************************
' 周波数設定・サブルーチン
'******************************
'
LowSpeed:
Freq=100 '周波数(/100Hz)
If ModeType=1 then Speed_rd
Goto Speed
'
HighSpeed:
Freq=250 '周波数(/100Hz)
If ModeType=1 then Speed_rd
Goto Speed
'
'******************************
' 傾き設定・サブルーチン
'******************************
'
LeftL:
RR=Amplitude
LL=0
Gosub Mode_set
If ModeType=1 then Leaning_rd
Goto Leaning
'
RightR:
RR=100
LL=100-Amplitude
Gosub Mode_set
If ModeType=1 then Leaning_rd
Goto Leaning

'
Center:
RR=Amplitude/2
RR=50+RR
LL=Amplitude/2
LL=50-LL
Gosub Mode_set
If ModeType=1 then Leaning_rd
Goto Leaning
'
'******************************
' モードセット・サブルーチン
'******************************
'
Mode_set:
T1=ONtime_max-Ontime_min
T1=T1/100
T1=T1*LL
T1=ONtime_min+T1
'
T2=ONtime_max-Ontime_min
T2=T2/100
T2=T2*RR
T2=ONtime_min+T2
'
T3=T2-T1 '1/2周期のデータ数
T3=2*T3 '1周期のデータ数（4の倍数）
SpeedStep=10000/Freq/Ptime*10 '設定した周波数に対応するデータ数
SpeedStep=T3/SpeedStep '刻み幅（ステップ）
Return