 |
'**************************************************************
'
' Fish Robot Control Program for PPF-09
'
' 2002/7/9 by Koichi Hirata
'
' ppf09l.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 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
'Amplitude = Vv3 / 16
'Amplitude = Amplitude - 12
'
' Straight or Turn
'
If Vv0 > 600 Then 'Right side
Goto Turningmov1
End If
If Vv0 < 480 Then 'Left side
Goto Turningmov2
End If
T1 = 108 - Amplitude
T2 = 108 + Amplitude
Goto Outputmov 'Central posision
'
Turningmov1: 'Right side
T2 = Vv0 / 6
T2 = T2 + 18
T1 = T2 - Amplitude
T1 = T1 - Amplitude
Goto Outputmov
'
Turningmov2: 'Left side
T1 = Vv0 / 8
T1 = T1 + 41
T2 = T1 + Amplitude
T2 = T2 + Amplitude
Goto Outputmov
'
Outputmov:
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
|
|
[ BACK ] |
 |