Servo-Programm für 6 Servos mit 1000 Schritten

[themaddin]

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?




'(
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* 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

[kater]

Ich kann zwar nicht wirklich BAscom lesen, aber dein Programm scheint nach dem selben Prinzip zu funktionieren wie meins das ich erst kuerzlich geschrieben habe :)

Ich finde aber irgendwie die Stelle nicht wo du die Daten von der Seriellen empfaengst.

[themaddin]

Moin!

Es funktioniert so, dass zuerst Servo1 auf high gesetzt wird, dann nach 1,5ms (für mittelstellung z.B.) Servo1 auf low und Servo2 auf high usw. zum Schluss werden dann alle Zeiten der positiven Impulse von 20ms abgezogen, die zeit wird gewartet und es geht wieder bei 1 los.
Der Interrupt für die seriellen Schnittstelle befindet sich ganz unten, im oberen Teil werden diese ausgewertet.

MfG
Martin

[kater]

Jup, das Prinzip hab ich auch. Du wertest den String im Hauptprogramm aus, ich mach das in der INT Routine. Ich glaub deine Loesung ist besser.