Mehrere RC Signale einlesen und mehrere Servos ausgeben

[radbruch]

Ungetestet:

'===============================================================================
'Baggersteuerung (ACHTUNG FUNKTIONIERT NICHT, SERVO RUCKELT EXTREM)

'RC Empfangskanal 1 an Pin 6 (PA7)
'RC Empfangskanal 2 an Pin 7 (PA6)
'RC Empfangskanal 3 (Mode-Schalter) an Pin 5 (PB2)

'Werte zwischen 100 und 200 entsprechen dem kompletten Knüppelweg
'Der Wert 150 entspricht dabei ca. Mittelstellung, der Wert 200 ca. Vollausschlag

'Poti Eingang 1 Pin 9 (PA4)
'Poti Eingang 1 Pin 8 (PA5)

'Servoausgang 1 Pin13 (PA0)
'Servoausgang 2 Pin12 (PA1)
'Servoausgang 3 Pin11 (PA2)
'Servoausgang 4 Pin10 (PA3)

'Made by Robert (www.roberts-r2d2-bau.de)

'Angepasst von radbruch 21.12.2013
'https://www.roboternetz.de/community/threads/63750-Mehrere-RC-Signale-einlesen-und-mehrere-Servos-ausgeben?
'===============================================================================


$regfile = "attiny24.dat"
$crystal = 8000000                      'FuseBit CKDIV8 deaktivieren


$hwstack = 32
$swstack = 20
$framesize = 40



'-------------------------------------------------------------------------------------------------------------
'Variablen definieren
'-------------------------------------------------------------------------------------------------------------


Dim Empfangskanal(3) As Word
Dim Empfangskanal_temp_ein(3) As Word
Dim Empfangskanal_temp_aus(3) As Word

Dim Kanal As Byte
Dim Servo(4) As Word


'-------------------------------------------------------------------------------------------------------------
'Ein- und Ausgang festlegen
'-------------------------------------------------------------------------------------------------------------

Ddra = &B00001111                       'PA0, PA1, PA2 und PA3 werden Ausgänge, der restliche Port A bleibt Eingang
Ddrb = &B00000000                       'PortB bleibt Eingang


'-------------------------------------------------------------------------------------------------
'Timer und Interrupt-Service-Routinen konfigurieren und freigeben
'-------------------------------------------------------------------------------------------------

Config Timer1 = Timer , Prescale = 8    'Timer für Servos, läuft mit 1MHz, ergibt 1000 Schritte Auflösung pro ms
Compare1a = 1000                        ' Nach 1ms wird die ISR initialisiert
Timer1 = 1
Enable Compare1a                        'schaltet den Timer1 Compare1a Match-Interrupt ein

On Compare1a Servoausgabe               'Springt bei Timer1 Compare1a Match in die ISR
Enable Interrupts                       ' Interrupts zulassen


'-------------------------------------------------------------------------------------------------------------
'Einigen Variablen Werte zuweisen
'-------------------------------------------------------------------------------------------------------------

Kanal = 0                               ' Kanal startet jetzt bei 0 wegen porta.kanal in der ISR!

Servo(1) = 1500
Servo(2) = 1500
Servo(3) = 1500
Servo(4) = 1500
Empfangskanal(1) = 0
Empfangskanal(2) = 0
Empfangskanal(3) = 0
Empfangskanal_temp_ein(1) = 0
Empfangskanal_temp_ein(2) = 0
Empfangskanal_temp_ein(3) = 0
Empfangskanal_temp_aus(1) = 0
Empfangskanal_temp_aus(2) = 0
Empfangskanal_temp_aus(3) = 0


'-------------------------------------------------------------------------------------------------------------
'Hauptprogramm starten
'-------------------------------------------------------------------------------------------------------------

Do

If Pina.7 = 1 And Empfangskanal_temp_ein(1) = 0 Then
   Empfangskanal_temp_ein(1) = Timer1
End If

If Pina.7 = 0 And Empfangskanal_temp_ein(1) <> 0 Then
      Empfangskanal_temp_aus(1) = Timer1
      If Empfangskanal_temp_aus(1) < Empfangskanal_temp_ein(1) Then
         Empfangskanal_temp_aus(1) = Empfangskanal_temp_aus(1) + 20000
      End If
      Empfangskanal(1) = Empfangskanal_temp_aus(1) - Empfangskanal_temp_ein(1)
      Empfangskanal_temp_ein(1) = 0
End If

If Pina.6 = 1 And Empfangskanal_temp_ein(2) = 0 Then
   Empfangskanal_temp_ein(2) = Timer1
End If

If Pina.6 = 0 And Empfangskanal_temp_ein(2) <> 0 Then
      Empfangskanal_temp_aus(2) = Timer1
      If Empfangskanal_temp_aus(2) < Empfangskanal_temp_ein(2) Then
         Empfangskanal_temp_aus(2) = Empfangskanal_temp_aus(2) + 20000
      End If
      Empfangskanal(2) = Empfangskanal_temp_aus(2) - Empfangskanal_temp_ein(2)
      Empfangskanal_temp_ein(2) = 0
End If

If Empfangskanal(1) <> 0 Then Servo(1) = Empfangskanal(1) End If
If Empfangskanal(2) <> 0 Then Servo(2) = Empfangskanal(2) End If

Loop

End

'(

Der Timer startet beim ersten Servo mit dem Zählwert 1, Das Compare-Register wird auf den Wert
für die Impulslänge des ersten Servo gesetzt.In der Compere-ISR (wenn der Zählwert den Wert im
Compareregister erreicht hat) wird dann ausgehend vom aktuelen Timerwert der Vergleichswert für
die Impulslänge des nächsten Servo berechnet und gesetzt. Nach dem letzten Servo wird 20000 für
die Wiederholfrequenz des Signals gesetzt

')

Servoausgabe:

Porta.0 = 0                             'alle Servosignale ausschalten
Porta.1 = 0
Porta.2 = 0
Porta.3 = 0

If Kanal < 4 Then                       ' Zeitpunkt für nächstes Servo setzen
   If Kanal = 0 Then
      Timer1 = 1                        ' beim ersten Servo Timer Zähler zurücksetzen
   End If
   Compare1a = Timer1 + Servo(kanal + 1)       ' aktueller Timerwert + Servo Impulslänge
   Porta.kanal = 1                      ' Impuls ausgeben
   Incr Kanal                           ' nächstes Servo vormerken
Else
   Compare1a = 20000                    ' 50Hz Wiederholfrequenz setzen
   Kanal = 0
End If

Return