'
'**************************************************
'
' R/C Servo Motor Operating Program for PPF-01i
'
'**************************************************
'
Symbol ONtime1=W0
Symbol ONtime2=W1
Symbol T1=W2
Symbol T2=W3
Symbol T3=W4
Symbol T4=W5
Symbol Freq=W6
Symbol LL=W7
Symbol RR=W8
Symbol SpeedRep=W9
Symbol SpeedStep=W10
Symbol i=B24
Symbol ii=B25
Symbol z=B26
Symbol k=B27
Symbol Ptime=20 'パルス周期(20ms)
Symbol ONtime_min=188 'ONの時間の最小値(188/250=0.75ms)
Symbol Ontime_cent=375 'ニュートラル時のONの時間(375/250=1.50ms)
Symbol ONtime_max=562 'ONの時間の最大値(562/250=2.25ms)
Symbol Clk=10 'クロック(10MHz)
z=0 'モード番号（初期値）
k=1 'スピードの倍率
'
input 0: input 1: input 2: input 3
input 4: input 5: output 6: output 7
'
Loop:
Branch z,(Mode0, Mode1, Mode2, Mode3, Mode4, Mode4, z_reset) 'モード設定
Mode_end:
z=z+1
'
For ii=1 to 3
  if Pin0=0 then LowSpeed
  if Pin1=0 then HighSpeed
  Speed_end:
  SpeedStep=SpeedStep+k
  '
  For ONtime1=T1 to T2 step SpeedStep 'ONの時間(/2.5usec)
    T4=T3/4+T1 '90degの位置
    If ONtime1=T4 then NextONtime2 '90deg〜180deg
    NextONtime1: '0〜90deg
      T4=T3/4 '1/4周期
      ONtime2=T4+T1-ONtime1+T1 '逆向きに動く
      Goto NextONtime3
    NextONtime2: '90deg〜180deg
      T4=T3/4 '1/4周期
      ONtime2=ONtime1-T4 '1/4周期引く
    NextONtime3:
    For i=1 to SpeedRep
      Pulsout 6, ONtime1
      Pulsout 7, ONtime2
      Pause 47 'OFFtime 'OFFの時間(47/2.5=18.8msec)
    Next i
  Next ONtime1
  T2=T2-1: T1=T1+1
  For ONtime1=T2 to T1 step -SpeedStep 'ONの時間(/2.5usec)
    T4=T3/4+T1 '270degの位置
    If ONtime1=T4 then NextONtime5
    NextONtime4: '180〜270deg
      T4=T3/4 '1/4周期
      ONtime2=ONtime1+T4 '1/4周期加える
      Goto NextONtime6
    NextONtime5: '270deg〜360deg
      T4=T3/4 '3/4周期
      ONtime2=T4+T2-ONtime1+T1
    NextONtime6:
    For i=1 to SpeedRep
      Pulsout 6, ONtime1
      Pulsout 7, ONtime2
      Pause 47 'OFFtime 'OFFの時間(47/2.5=18.8msec)
    Next i
  Next ONtime1
  T2=T2+1: T1=T1-1
Next ii
Goto Loop
End
'
'************************************************** サブルーチン
'
LowSpeed:
  If k=1 then Speed_end
  k=k-1
  Goto Speed_end
HighSpeed:
  k=k+1
  Goto Speed_end
'
' モード0
'
Mode0:
LL=80 '左偏り(%)
RR=80 '右偏り(%)
Freq=100 '周波数(/100Hz)
Gosub Mode_set
Goto Mode_end
'
' モード1
'
Mode1:
LL=70 '左偏り(%)
RR=10 '右偏り(%)
Freq=100 '周波数(/100Hz)
Gosub Mode_set
Goto Mode_end
'
' モード2
'
Mode2:
LL=10 '左偏り(%)
RR=70 '右偏り(%)
Freq=100 '周波数(/100Hz)
Gosub Mode_set
Goto Mode_end
'
' モード3
'
Mode3:
LL=60 '左偏り(%)
RR=60 '右偏り(%)
Freq=150 '周波数(/100Hz)
Gosub Mode_set
Goto Mode_end
'
' モード4
'
Mode4:
LL=40 '左偏り(%)
RR=40 '右偏り(%)
Freq=200 '周波数(/100Hz)
Gosub Mode_set
Goto Mode_end
'
' 変数zを0に戻すサブルーチン
'
z_reset:
z=0
Goto Loop
'
' モードセットサブルーチン
'
Mode_set:
T1=ONtime_cent-Ontime_min
T1=T1*LL/100
T1=ONtime_cent-T1
T2=ONtime_max-Ontime_cent
T2=T2*RR/100
T2=ONtime_cent+T2
T3=T2-T1
T3=2*T3 '1周期のデータ数（4の倍数）
'SpeedStep=10000/Ptime/T3*10 'ダミー（ステップ1，リピート1の場合の周波数）
'If Freq>SpeedStep Then Set_SpeedStep
'  SpeedStep=1 '最も遅く設定
'  SpeedRep=10000/Freq/Ptime*10 '設定した周波数に対応するデータ数
'  SpeedRep=SpeedRep/T3 '繰り返し数（リピート）
'Return
'Set_SpeedStep:
  SpeedRep=1 '最速に設定
  SpeedStep=10000/Freq/Ptime*10 '設定した周波数に対応するデータ数
  SpeedStep=T3/SpeedStep '刻み幅（ステップ）
Return