'**************************************************************
'
'  Fish Robot Control Program for PPF-09
'
'    2002/7/17 by Koichi Hirata
'
'    ppf09m.bas
'
'**************************************************************
'
' for AT90S8535-8PC
' Futaba, T4VF, SKYSPORT4
'
'*********************
' Initial set
'*********************
'
'
Config Adc = Single , Prescaler = Auto                        'For A/D conv.
Config Pinb.0 = Output                                        'For Pulse Output
Dim Vv0 As Word
Dim Vv1 As Word
Dim Vv2 As Word
Dim Vv3 As Word
Dim V0 As Single
Dim V1 As Single
Dim V2 As Single
Dim V3 As Single
Dim I As Integer
Dim Ontime1 As Integer
Dim Ontime2 As Integer
Dim Ontime3 As Integer
Dim Speedstep As Integer
Dim Amplitude As Integer
Dim T0 As Integer
Dim T1 As Integer
Dim T2 As Integer
Dim T3 As Integer
'
'*********************
' Main Routine (Loop)
'*********************
'
Do
'
' Read A/D value
'
Start Adc
   Vv0 = Getadc(0)
   V0 = Vv0 * 5
   V0 = V0 / 1024
   Vv1 = Getadc(1)
   V1 = Vv1 * 5
   V1 = V1 / 1024                                             'Voltage (V)
   Vv2 = Getadc(2)
   V2 = Vv2 * 5
   V2 = V2 / 1024                                             'Voltage (V)
   Vv3 = Getadc(3)
   V3 = Vv3 * 5
   V3 = V3 / 1024                                             'Voltage (V)
Stop Adc
'
' Up-down Motion
'
Ontime3 = Vv2 / 6
Ontime3 = 198 - Ontime3
'
' Stationary or Moving
'
If Vv1 > 690 Then
  Goto Stationary
Else
  Goto Moving
End If
'
'**********************
' Moving Mode
'**********************
'
Moving:
'
' Set to Frequency
'
Speedstep = Vv1 * 7
Speedstep = Speedstep / 400
Speedstep = 13 - Speedstep
If Speedstep < 1 Then Speedstep = 1
'
' Set to Amplitude
'
Amplitude = Vv3 / 25
T0 = Vv0 / 8
T0 = T0 + 41
'
T1 = T0 - Amplitude
T2 = T0 + Amplitude
T3 = T1 + T2                                                  'Data Number of 1/2 cycle
T3 = T3 / 2                                                   'Locayion of 1/4 cycle
'
Ontime1 = T1
Move1:
  If Ontime1 > T3 Then
    Goto Nextontime2
  End If
Nextontime1:                                                  '0-90deg
  Ontime2 = T3 - Ontime1
  Ontime2 = Ontime2 + T1                                      'Oposite motion
  Goto Nextontime3
Nextontime2:                                                  '90-180deg
  Ontime2 = Ontime1 - T3
  Ontime2 = Ontime2 + T1                                      '1/4 cycle of phase
Nextontime3:
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  Waitms 10 : Waitus 200 : Waitus 200 : Waitus 200            'ON time(10.6msec)
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  For I = 0 To Ontime1
    Waitus 4                                                  'For ch1
  Next I
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  For I = 0 To Ontime2
    Waitus 4                                                  'For ch2
  Next I
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  For I = 0 To Ontime3
    Waitus 4                                                  'For ch3
  Next I
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  Waitms 1 : Waitus 100                                       'ch4(1.1msec)
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  Waitus 200 : Waitus 200 : Waitus 200                        'ON time(0.6msec)
'
If Ontime1 > T2 Then
  Goto Move2
End If
Ontime1 = Ontime1 + Speedstep
Goto Move1
'
Move2:
  If Ontime1 < T3 Then
    Goto Nextontime5
  End If
Nextontime4:                                                  '180-270deg
  Ontime2 = T2 - Ontime1
  Ontime2 = Ontime2 + T3                                      '1/4 cycle of phase
  Goto Nextontime6
Nextontime5:                                                  '270-360deg
  Ontime2 = Ontime1 + T2
  Ontime2 = Ontime2 - T3
Nextontime6:
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  Waitms 10 : Waitus 200 : Waitus 200 : Waitus 200            'ON time(10.6msec)
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  For I = 0 To Ontime1
    Waitus 4                                                  'For ch1
  Next I
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  For I = 0 To Ontime2
    Waitus 4                                                  'For ch2
  Next I
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  For I = 0 To Ontime3
    Waitus 4                                                  'For ch3
  Next I
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  Waitms 1 : Waitus 100                                       'ch4(1.1msec)
  Portb.0 = 0
  Waitus 200 : Waitus 200                                     'OFF time(0.4msec)
  Portb.0 = 1
  Waitus 200 : Waitus 200 : Waitus 200                        'ON time(0.6msec)
'
If Ontime1 < T1 Then
  Goto Loopend
End If
Ontime1 = Ontime1 - Speedstep
Goto Move2
'
'**********************
' Stationary Mode
'**********************
'
Stationary:
'If Vv0 > 600 Then                                             'Right side
'  Goto Turningpos
'End If
'If Vv0 < 480 Then                                             'Left side
'  Goto Turningpos
'End If
'Ontime1 = 108                                                 'Central Position
'Goto Outputpos:
'
'Turningpos:
Ontime1 = Vv0 / 8
Ontime1 = Ontime1 + 41
'
Outputpos:
Portb.0 = 0
Waitus 200 : Waitus 200                                       'OFF time(0.4msec)
Portb.0 = 1
Waitms 10 : Waitus 200 : Waitus 200 : Waitus 200              'ON time(10.6msec)
Portb.0 = 0
Waitus 200 : Waitus 200                                       'OFF time(0.4msec)
Portb.0 = 1
For I = 0 To Ontime1
  Waitus 4                                                    'For ch1
Next I
Portb.0 = 0
Waitus 200 : Waitus 200                                       'OFF time(0.4msec)
Portb.0 = 1
For I = 0 To Ontime1
  Waitus 4                                                    'For ch2
Next I
Portb.0 = 0
Waitus 200 : Waitus 200                                       'OFF time(0.4msec)
Portb.0 = 1
  For I = 0 To Ontime3
    Waitus 4                                                  'For ch3
  Next I
Portb.0 = 0
Waitus 200 : Waitus 200                                       'OFF time(0.4msec)
Portb.0 = 1
Waitms 1 : Waitus 100                                         'ON time(1.1msec)
Portb.0 = 0
Waitus 200 : Waitus 200                                       'OFF time(0.4msec)
Portb.0 = 1
Waitus 200 : Waitus 200 : Waitus 200                          'ON time(0.6msec)
'
Loopend:
Loop

End