Servo-Programm für 6 Servos mit 1000 Schritten
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
