'  ***     4SERVO  (c) 2012, gps
'output B: 0=Sstuur/1=Spower
'output D: 0...1 = RS232, 4...7=output dig servo 4...1
'input C: 0...3=data servo 1...4, 5=Stuurkastje aangesloten
$regfile = "m8def.dat" : $crystal = 8000000 : $baud = 8000
$hwstack = 64 : $swstack = 64 : $framesize = 64
'$sim
Const Svoff = 38
Dim Svle(4) As Eram Byte , Svme(4) As Eram Byte , Svhe(4) As Eram Byte
Dim Svse(4) As Eram Byte , Svbe(4) As Eram Byte , Svee(4) As Eram Byte
Dim Svl(4) As Byte , Svm(4) As Byte , Svh(4) As Byte , Svs(4) As Byte
Dim Svb(4) As Byte , Sve(4) As Byte , W As Word , Wa As Word , F As Single
Dim S As String * 20 , Modus As Byte , Svnow As Byte , Svpar As Byte
Dim A As Byte , B As Byte , C As Byte , D As Byte , Sb As Byte , T As Bit
Config Portb = Output : Config Portc = Input : Config Portd = Output
Config Servos = 4 , Servo1 = Portb.2 , Servo2 = Portb.4 , Servo3 = Portb.0 , Servo4 = Portb.6 , Reload = 10
Enable Interrupts : Gosub Initvars
'               *** HOOFDLUS ***
Do
 If Modus > 0 Then                                          '** met Stuurkastje
  If Pinc.5 = 0 Then                                        'uitschakelen?
   Waitms 500 : If Pinc.5 = 0 Then Gosub Startpos
  Else
   Select Case Modus
    Case 1:                                                 'initialisatie-modus
     Gosub Getkey : If C > 0 Then                           'per servo in beeld
      Select Case C
       Case 1:
        Modus = 2 : Gosub Svpshow
       Case 2:                                              'alles opslaan
         A = Svnow : Svle(a) = Svl(a) : Waitms 1 : Svme(a) = Svm(a)
         Waitms 1 : Svhe(a) = Svh(a) : Waitms 1 : Svse(a) = Svs(a)
         Waitms 1 : Svbe(a) = Svb(a) : Waitms 1 : Svee(a) = Sve(a) : Waitms 1
       Case 3 :
        Gosub Svf1 : Gosub Svshow
       Case 6:
        If Sve(svnow) = 0 Then Sve(svnow) = 255 Else Sve(svnow) = 0
        Gosub Svshow
       Case 8:
        Decr Svnow : If Svnow = 0 Then Svnow = 4
        Gosub Svshow
       Case 9 :
        Incr Svnow : If Svnow = 5 Then Svnow = 1
        Gosub Svshow
      End Select : End If
    Case 2:
     Gosub Getkey : If C > 0 Then                           'per functie in beeld
      Select Case C
       Case 2:                                              'opslaan
        Select Case Svpar
         Case 1 : Svle(svnow) = Svl(svnow)
         Case 2 : Svme(svnow) = Svm(svnow)
         Case 3 : Svhe(svnow) = Svh(svnow)
         Case 4 : Svse(svnow) = Svs(svnow)
         Case 5 : Svbe(svnow) = Svb(svnow)
        End Select : Waitms 1
       Case 3 :
        Gosub Svf1 : Gosub Svpshow
       Case 6 :                                             'esc=hogerop
        Modus = 1 : Gosub Svshow
       Case 7:
        Select Case Svpar                                   'hoger zetten
         Case 1:
          Incr Svl(svnow) : If Svl(svnow) >= Svm(svnow) Then Decr Svl(svnow) Else Gosub Svtl
         Case 2:
          Incr Svm(svnow) : If Svm(svnow) >= Svh(svnow) Then Decr Svm(svnow) Else Gosub Svtm
         Case 3:
          Incr Svh(svnow) : If Svh(svnow) > 90 Then Decr Svh(svnow) Else Gosub Svth
         Case 4:
          Incr Svs(svnow) : If Svs(svnow) > 99 Then Decr Svs(svnow)
         Case 5:
          Incr Svb(svnow) : If Svb(svnow) > 14 Then Decr Svb(svnow)
        End Select : Gosub Svpshow
       Case 8 :
        Decr Svpar : If Svpar = 0 Then Svpar = 5
        Gosub Svpshow
       Case 9 :
        Incr Svpar : If Svpar = 6 Then Svpar = 1
        Gosub Svpshow
       Case 10:
        Select Case Svpar                                   'lager zetten
         Case 1:
          Decr Svl(svnow) : If Svl(svnow) < 10 Then Incr Svl(svnow) Else Gosub Svtl
         Case 2:
          Decr Svm(svnow) : If Svm(svnow) <= Svl(svnow) Then Incr Svm(svnow) Else Gosub Svtm
         Case 3:
          Decr Svh(svnow) : If Svh(svnow) <= Svm(svnow) Then Incr Svh(svnow) Else Gosub Svth
         Case 4:
          Decr Svs(svnow) : If Svs(svnow) < 1 Then Incr Svs(svnow)
         Case 5:
          Decr Svb(svnow) : If Svb(svnow) = 255 Then Svb(svnow) = 0
        End Select : Gosub Svpshow
      End Select : End If
   End Select
  End If
 Else                                                       '** zonder stuurkast
  If Pinc.5 = 1 Then
   Waitms 500 : If Pinc.5 = 1 Then                          'kast inschakelen
    Modus = 1 : Print Chr(1) ; Chr(11) ; : For Svnow = 1 To 4
     Gosub Spon : Servo(svnow) = Svm(svnow) + Svoff : Waitms 400
     Gosub Spoff : Next : Svnow = 1 : Gosub Svshow : End If
  Else
   For Svnow = 1 To 4 : If Sve(svnow) = 255 Then            'andere input ?
    Select Case Svnow
     Case 1:
      T = Pinc.0 : If T <> Portd.7 Then
       Gosub Spon : B = Svs(svnow) : Gosub Getslw
       If T = 0 Then Gosub Svdown Else Gosub Svup
       Gosub Spoff : End If
     Case 2:
      T = Pinc.1 : If T <> Portd.6 Then
       Gosub Spon : B = Svs(svnow) : Gosub Getslw
       If T = 0 Then Gosub Svdown Else Gosub Svup
       Gosub Spoff : End If
     Case 3:
      T = Pinc.2 : If T <> Portd.5 Then
       Gosub Spon : B = Svs(svnow) : Gosub Getslw
       If T = 0 Then Gosub Svdown Else Gosub Svup
       Gosub Spoff : End If
     Case 4:
      T = Pinc.3 : If T <> Portd.4 Then
       Gosub Spon : B = Svs(svnow) : Gosub Getslw
       If T = 0 Then Gosub Svdown Else Gosub Svup
       Gosub Spoff : End If
    End Select : End If : Next : Svnow = 1 : End If : End If
Loop
'              *** SUBROUTINES ***
'   cascade-initroutines
Initvars:
 C = Svbe(4) : If C = 255 Then
  Restore Slow : For A = 1 To 4 : Read B : Svle(a) = B      'eerste gebruik ?
   Waitms 1 : Next : For A = 1 To 4 : Read B : Svme(a) = B
   Waitms 1 : Next : For A = 1 To 4 : Read B : Svhe(a) = B
   Waitms 1 : Next : For A = 1 To 4 : Read B : Svse(a) = B
   Waitms 1 : Next : For A = 1 To 4 : Read B : Svbe(a) = B
   Waitms 1 : Next : For A = 1 To 4 : Read B : Svee(a) = B
   Waitms 1 : Next : End If : For A = 1 To 4 : Svh(a) = Svhe(a)
  Svl(a) = Svle(a) : Svm(a) = Svme(a) : Svs(a) = Svse(a) : Svb(a) = Svbe(a)
  Sve(a) = Svee(a) : Next : Svpar = 1
Startpos:
 For Svnow = 1 To 4 : If Sve(a) = 255 Then                  'snel naar startposities
   B = 1 : Shift B , Left , Svnow : Shift B , Right : C = Pinc And B
   Gosub Spon : If C = 0 Then C = Svl(svnow) Else C = Svh(svnow)
   Servo(svnow) = C + Svoff : Waitms 600 : Gosub Spoff : End If
 Next : Portd.7 = Pinc.0 : Portd.6 = Pinc.1 : Portd.5 = Pinc.2
 Portd.4 = Pinc.3 : Svnow = 1 : Modus = 0 : Return
Getkey:
 C = Inkey() : If C = 0 Then Return
 Do : Waitms 10 : Loop Until Inkey() = 0 : Return
Svshow:
 Gosub Clcslw : Gosub Prservonr : If Sve(svnow) = 255 Then
  Print Svl(svnow) ; Svm(svnow) ; Svh(svnow) ; " " ; Svb(svnow) ;
  Print Chr(9) ; S ; "sec  F1=Test" ;
 Else
  Print "   UIT" ; Chr(9) ; "   Esc = AAN" ;
 End If : Return
Svf1:
 Gosub Clcslw : Print Chr(9) ; S ; "sec  Test..." ;         'F1
 Gosub Svtest : Return
Svpshow:
 Gosub Prservonr : Select Case Svpar
  Case 1 : Print " Onder=" ; Svl(svnow);                    'ondergrens
  Case 2 : Print "Midden=" ; Svm(svnow);                    'middelpunt
  Case 3 : Print " Boven=" ; Svh(svnow);                    'bovengrens
  Case 4:                                                   'snelheid
   Gosub Clcslw : Print "  " ; S ; "sec";
  Case 5 : Print " Zwaai=" ; Svb(svnow);                    'uitzwaai
 End Select : Print Chr(12) ; : Return
Prservonr:
 Print Chr(10) ; "Servo" ; Svnow ; " " ; : Return
Getslw:
 Restore Slowtab : For A = 1 To B : Read W : Next : Return
Clcslw:
 B = Svs(svnow) : Gosub Getslw : F = W / 530 : A = Svh(svnow) -svl(svnow)
 B = Svb(svnow) : For C = 1 To B : A = A + C : Next
 F = F * A : F = F / 98 : S = Str(f) : S = Left(s , 4) : Return
Spon:
 Select Case Svnow:
  Case 1 : Portb.5 = 1
  Case 2 : Portb.3 = 1
  Case 3 : Portb.1 = 1
  Case 4 : Portb.7 = 1
 End Select : Return
Svtl:
 Gosub Spon : Servo(svnow) = Svl(svnow) + Svoff : Waitms 300 : Goto Spoff
Svtm:
 Gosub Spon : Servo(svnow) = Svm(svnow) + Svoff : Waitms 300 : Goto Spoff
Svth:
 Gosub Spon : Servo(svnow) = Svh(svnow) + Svoff : Waitms 300
Spoff:
 Select Case Svnow:
  Case 1 : Portb.5 = 0
  Case 2 : Portb.3 = 0
  Case 3 : Portb.1 = 0
  Case 4 : Portb.7 = 0
 End Select : Return
Svtest:
 Gosub Spon : Servo(svnow) = Svl(svnow) + Svoff : B = Svs(svnow)
 Gosub Getslw : Waitms 600 : Gosub Svup : Waitms 600 : Gosub Svdown
 Waitms 600 : Servo(svnow) = Svm(svnow) + Svoff : Waitms 600 : Gosub Spoff : Return
Svup:
 Sb = Svb(svnow) : A = Sb / 2 : D = Svl(svnow) : C = Svh(svnow) - A : Gosub Svbup
 A = Sb And 1 : While Sb > 0 : B = Sb And 1 : If A = B Then Gosub Svbdown Else Gosub Svbup
  Decr Sb : Wend : Servo(svnow) = Svh(svnow) + Svoff : Return
Svdown:
 Sb = Svb(svnow) : A = Sb / 2 : D = Svh(svnow) : C = Svl(svnow) + A : Gosub Svbdown
 A = Sb And 1 : While Sb > 0 : B = Sb And 1 : If A = B Then Gosub Svbup Else Gosub Svbdown
  Decr Sb : Wend : Servo(svnow) = Svl(svnow) + Svoff : Return
Svbup:
 B = D + 1 : C = C + Sb : For D = B To C : Gosub Svset
  If D = Svm(svnow) Then
   Select Case Svnow
    Case 1 : Portd.7 = 1
    Case 2 : Portd.6 = 1
    Case 3 : Portd.5 = 1
    Case 4 : Portd.4 = 1
   End Select : End If : Next : Return
Svbdown:
 B = D -1 : C = C -sb : For D = B To C Step -1 : Gosub Svset
 If D = Svm(svnow) Then
   Select Case Svnow
    Case 1 : Portd.7 = 0
    Case 2 : Portd.6 = 0
    Case 3 : Portd.5 = 0
    Case 4 : Portd.4 = 0
   End Select : End If : Next : Return
Svset:
 Servo(svnow) = D + Svoff : For Wa = 1 To W : Waitus Traag : Next : Return
End
Slow:
Data 35 , 35 , 35 , 35                                      'laag-punt
Data 50 , 50 , 50 , 50                                      'middelpunt
Data 65 , 65 , 65 , 65                                      'hoog-punt
Data 50 , 50 , 50 , 50                                      'snelheid
Data 0 , 0 , 0 , 0                                          'uitzwengelen
Data 255 , 255 , 255 , 255                                  'aan
Slowtab:
Data 50% , 55% , 60% , 65% , 70% , 76% , 82% , 89% , 96% , 104%
Data 112% , 121% , 131% , 141% , 152% , 163% , 176% , 189% , 203% , 218%
Data 234% , 252% , 270% , 290% , 312% , 334% , 359% , 385% , 413% , 443%
Data 475% , 509% , 546% , 585% , 627% , 671% , 719% , 771% , 826% , 884%
Data 947% , 1015% , 1087% , 1164% , 1246% , 1334% , 1429% , 1530% , 1638% , 1753%
Data 1877% , 2010% , 2151% , 2303% , 2465% , 2638% , 2824% , 3023% , 3235% , 3463%
Data 3706% , 3967% , 4245% , 4543% , 4862% , 5204% , 5569% , 5960% , 6378% , 6825%
Data 7304% , 7816% , 8365% , 8951% , 9579% , 10250% , 10969% , 11737% , 12560% , 13440%
Data 14382% , 15390% , 16468% , 17622% , 18856% , 20177% , 21590% , 23103% , 24721% , 26452%
Data 28305% , 30287% , 32408% , 34678% , 37106% , 39705% , 42485% , 45460% , 48643% , 52049%