Moin!

Hier mein selbsterstelltes Programm zum Steuern von 6 Servos über die serielle Schnittstelle. Es kann auch auf noch mehr Servos einfach erweitert werden.
Was haltet ihr davon? Habt ihr Verbesserungsvorschläge?



Code:
'(
          * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
          * Programm Zum Steuern Von 6 Servos über Die Serielle Schnitstelle
          * Für 8mhz , es wird Timer1 verwendet
          * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *

')
$crystal = 8000000
$baud = 9600
Config Timer1 = Timer , Prescale = 8                        'Timer 0 für die Servoroutine

On Timer1 T1isr                                             'Timer0_isr
Ddrb = &B11111111
Ddrc = &B11111111
Enable Interrupts
Enable Timer1
On Urxc Onrxd
Enable Urxc



Dim Wort As String * 10
Dim Restdauer As Word
Dim Position As Long
Led Alias Portc.0
Dim Servo(6) As Word                                        '6-Byte-Array für 6 Servos für angestrebte Position
Dim Servopos(6) As Word                                     'für tatsächliche Position
Dim Zaehler As Byte                                         'Hilfsvariable
Servo1port Alias Portb.0
Servo2port Alias Portb.1
Servo3port Alias Portb.2
Servo4port Alias Portb.3
Servo5port Alias Portb.4
Servo6port Alias Portb.5

Servo(1) = 1500
Servo(2) = 1500
Servo(3) = 1000
Servo(4) = 2000
Servo(5) = 1500
Servo(6) = 2000
Servopos(1) = 1500
Servopos(2) = 1500
Servopos(3) = 1000
Servopos(4) = 2000
Servopos(5) = 1500
Servopos(6) = 2000

Zaehler = 0

Timer1 = 0
Start Timer1

Do


If Len(wort) > 4 Then
  Position = Val(wort)
  Wort = ""
   Select Case Position
    Case 11000 To 12000 : Servo(1) = Position - 10000
    Case 21000 To 22000 : Servo(2) = Position - 20000
    Case 31000 To 32000 : Servo(3) = Position - 30000
    Case 41000 To 42000 : Servo(4) = Position - 40000
    Case 51000 To 52000 : Servo(5) = Position - 50000
    Case 61000 To 62000 : Servo(6) = Position - 60000
   End Select
End If

Loop


T1isr:
If Zaehler = 0 Then
Servo1port = 1
Servopos(1) = 65535 - Servo(1)
Timer1 = Servopos(1)                                        '65535 - 1500 Takte x 1us gleich 1,5ms
Incr Zaehler

Elseif Zaehler = 1 Then
Servo1port = 0
Servo2port = 1
Servopos(2) = 65535 - Servo(2)
Timer1 = Servopos(2)                                        '65535 - 1500 Takte x 1us gleich 1,5ms
Incr Zaehler

Elseif Zaehler = 2 Then
Servo2port = 0
Servo3port = 1
Servopos(3) = 65535 - Servo(3)
Timer1 = Servopos(3)
Incr Zaehler


Elseif Zaehler = 3 Then
Servo3port = 0
Servo4port = 1
Servopos(4) = 65535 - Servo(4)
Timer1 = Servopos(4)                                        '65535 - 1500 Takte x 1us gleich 1,5ms
Incr Zaehler


Elseif Zaehler = 4 Then
Servo4port = 0
Servo5port = 1
Servopos(5) = 65535 - Servo(5)
Timer1 = Servopos(5)                                        '65535 - 1500 Takte x 1us gleich 1,5ms
Incr Zaehler

Elseif Zaehler = 5 Then
Servo5port = 0
Servo6port = 1
Servopos(6) = 65535 - Servo(6)
Timer1 = Servopos(6)                                        '65535 - 1500 Takte x 1us gleich 1,5ms
Incr Zaehler                                                '65535 - 1500 Takte x 1us gleich 1,5ms


Elseif Zaehler = 6 Then
Servo6port = 0

Restdauer = 20000 - Servo(1)
Restdauer = Restdauer - Servo(2)
Restdauer = Restdauer - Servo(3)
Restdauer = Restdauer - Servo(4)
Restdauer = Restdauer - Servo(5)
Restdauer = Restdauer - Servo(6)
Restdauer = 65535 - Restdauer
Timer1 = Restdauer
Zaehler = 0
Toggle Led

End If

Return

Onrxd:
  Wort = Wort + Chr(udr)
Return
MfG
Martin