Mehrere RC Signale einlesen und mehrere Servos ausgeben

**Searcher** · 02.01.2014, 14:23

Zitat von R2D2 Bastler

Nur noch ganz selten hörbare und fühlbare Zuckungen, sichtbare Zuckungen waren praktisch nicht vorhanden. Werde morgen nochmal ausgiebig testen und den Code anschließend in mein Programm aufnehmen.

Hört sich schon gut an, aber leider noch nicht semiperfekt.

Zitat von for_ro

Das ist gar nicht so wild.

Du setzt den Bandgap auf 1, dann brauchst du keinen externen positiven Eingang mehr.
Du stoppst den ADC und schaltest den Multiplexer auf den negativen Input.
Du verbindest den AC mit dem ICP von Timer1
Du selektierst den gewünschten Flankenwechsel des AC und auch für den Input Capture

Das reizt mich, aber bevor ich/wir daran gehen noch eine Symptombehandlung. Jitterunterdrückung per SW-Hysterese nach diesem Muster zum Testen.

Hab in der Hauptschleife nun eine SW-Hysterese untergebracht. Die Hysterese kann über die Konstante "Jitterausgleich" in Vielfachen von von 8µs eingestellt werden. Testweise auch kleinere Stufen, aber ich glaube, daß dann das Servo "weglaufen"

könnte. Müßte man mal testen.

Gruß
Searcher

Code:

'===============================================================================
'RC Eingang 1 an Pin 3 (PB1, PCINT9)
'RC Eingang 2 an Pin 6 (PA7, PCINT7)
'RC Eingang 3 an Pin 5 (PB2, INT0)
'Servo 1 an Pin 13 (PA0)
'Servo 2 an Pin 12 (PA1)
'Servo 3 an Pin 11 (PA2)
'Servo 4 an Pin 10 (PA3)
'===============================================================================

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

$hwstack = 50
$swstack = 50
$framesize = 50


'-------------------------------------------------------------------------------------------------
'Timer und konfigurieren
'-------------------------------------------------------------------------------------------------

Config Timer1 = Timer , Prescale = 8    'Timer für Einlesen RC Signale
Start Timer1

Config Timer0 = Timer , Prescale = 64   'Timer für Servoausgabe, Wert 125 entspricht 1ms, Wert 250 entspricht 2ms
Enable Timer0
On Timer0 Servoausgabe Nosave           'Register werden manuell in der ISR gesichert


'-------------------------------------------------------------------------------------------------------------
'Variablen definieren / Konstanten
'-------------------------------------------------------------------------------------------------------------

Const Jitterausgleich = 8               'Wert in µs um den sich gemessene Impulslaenge aendern darf/muss, bevor Servo gestellt wird
                                        'Nur in Vielfachen von 8 veraendern (8, 16, 32, ..)
'Variablen fürs RC Einlesen
Dim Rc_signal_1_start As Word
Dim Rc_signal_2_start As Word
Dim Rc_signal_3_start As Word

Dim Rc_signal_1_stop As Word
Dim Rc_signal_2_stop As Word
Dim Rc_signal_3_stop As Word

Dim Rc_signal_1_stop_flag As Byte
Dim Rc_signal_2_stop_flag As Byte
Dim Rc_signal_3_stop_flag As Byte


Dim Impulslaenge_1 As Word
Dim Impulslaenge_2 As Word
Dim Impulslaenge_3 As Word

Dim Impulslaenge_1_alt As Word
Dim Impulslaenge_2_alt As Word
Dim Impulslaenge_3_alt As Word
Dim Impulslaengenaenderung As Integer   'wird nur einmal gebraucht, da nur Hilfswariable


'Variablen für Berechnungen
Dim Berechnung_1 As Word
Dim Berechnung_2 As Word



'Variablen für Servoausgabe
Dim Kanal As Byte
Dim Servoausgabe_1 As Byte
Dim Servoausgabe_2 As Byte
Dim Servoausgabe_3 As Byte
Dim Servoausgabe_4 As Byte
'Dim Pausen_variable As Byte


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

Kanal = 1
'Pausen_variable = 0

Impulslaenge_1_alt = 1500               'mit Servomittelstellung initialisieren (nicht essentiell)
Impulslaenge_2_alt = 1500               'mit Servomittelstellung initialisieren (nicht essentiell)
Impulslaenge_3_alt = 1500               'mit Servomittelstellung initialisieren (nicht essentiell)

'-------------------------------------------------------------------------------------------------------------
'Ein - Und Ausgang Festlegen
'-------------------------------------------------------------------------------------------------------------

Ddra = &B00001111                       'PA0 - PA3 werden Ausgänge
Ddrb = &B00000000                       'PortB bleibt Eingang


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

'Info:
'Alle Porta Pinchangeinterrupts sind in Bascom "PCINT0" zugeordnet.
'Alle Portb Pinchangeinterrupts sind in Bascom "PCINT1" zugeordnet.


Pcmsk1.pcint9 = 1                       'beim Flankenwechsel an PB1/PCINT9 (RC Eingang 1) Pinchangeinterrupt1 auslösen und in die Subroutine springen
Enable Pcint1                           'Pinchangeinterrupt1 (1 weil auf PortB) zulassen
On Pcint1 Rc_eingang_1 Nosave           'Register werden manuel in der ISR gesichert


Pcmsk0.pcint7 = 1                       'beim Flankenwechsel an PA7/PCINT6 (RC Eingang 2) Pinchangeinterrupt0 auslösen und in die Subroutine springen
Enable Pcint0                           'Pinchangeinterrupt0 (0 weil auf PortA) zulassen
On Pcint0 Rc_eingang_2 Nosave           'Register werden manuel in der ISR gesichert


Config Int0 = Change                    'beim Flankenwechsel an PB2/INT0 (RC Eingang 3) Int0 auslösen und in die Subroutine springen
Enable Int0
On Int0 Rc_eingang_3 Nosave             'Register werden manuel in der ISR gesichert

Enable Interrupts


'======================================================
'Hauptprogramm
'======================================================

Do

  If Rc_signal_1_stop_flag = 1 Then     'Bearbeitung nur, wenn ISR Pulsende gefunden hat
      Disable Pcint1                    'Mögliche Korruption durch ISR vorbeugen
      Impulslaenge_1 = Rc_signal_1_stop - Rc_signal_1_start
      Enable Pcint1
      Rc_signal_1_stop_flag = 0

      Impulslaengenaenderung = Impulslaenge_1_alt - Impulslaenge_1       'SW-Hysterese
      If Abs(impulslaengenaenderung) >= Jitterausgleich Then
          Impulslaenge_1_alt = Impulslaenge_1

          Berechnung_1 = Impulslaenge_1
          Shift Berechnung_1 , Right , 3       'Division von Berechnung_1 durch 8
          If Berechnung_1 > 255 Then Berechnung_1 = 255       'zu hohe Werte abfangen
          If Berechnung_1 < 120 Then Berechnung_1 = 120       'zu kleine Werte abfangen
          Servoausgabe_1 = 256 - Berechnung_1       'Servoausgabe_ zum direkten Laden in TCNT0 vorbereiten (Cmd Load umgangen)
      End If
  End If


  If Rc_signal_2_stop_flag = 1 Then     'Bearbeitung nur, wenn ISR Pulsende gefunden hat
      Disable Pcint0                    'Mögliche Korruption durch ISR vorbeugen
      Impulslaenge_2 = Rc_signal_2_stop - Rc_signal_2_start
      Enable Pcint0
      Rc_signal_2_stop_flag = 0

      Impulslaengenaenderung = Impulslaenge_2_alt - Impulslaenge_2       'SW-Hysterese
      If Abs(impulslaengenaenderung) >= Jitterausgleich Then
          Impulslaenge_2_alt = Impulslaenge_2

          Berechnung_2 = Impulslaenge_2
          Shift Berechnung_2 , Right , 3       'Division von Berechnung_1 durch 8
          If Berechnung_2 > 255 Then Berechnung_2 = 255       'zu hohe Werte abfangen
          If Berechnung_2 < 120 Then Berechnung_2 = 120       'zu kleine Werte abfangen
          Servoausgabe_2 = 256 - Berechnung_2       'Servoausgabe_ zum direkten Laden in TCNT0 vorbereiten (Cmd Load umgangen)
      End If
  End If

  Servoausgabe_3 = 190
  Servoausgabe_4 = 190

Loop



'======================================================
'ISR
'======================================================

Rc_eingang_1:
 $asm
   push r17                             'Register auf Stack sichern
   sbis pinb , 1                        'Skip next Instr if PINBx = 1
   rjmp Puls_ende1                      'Springe Puls_ende
   in r17 , tcnt1l                      'Timer1 low Byte holen
   sts {Rc_signal_1_start} , r17        'Speichere Timer1 low Byte nach Rc_signal_x_start low Byte
   in r17 , tcnt1h                      'Timer1 high Byte holen
   sts {Rc_signal_1_start} + 1 , r17    'Speichere Timer1 high Byte nach Rc_signal_x_start high Byte
   rjmp ende1                           'Springe zum Ende
  Puls_ende1:
   in r17 , tcnt1l                      'Timer1 low Byte holen
   sts {Rc_signal_1_stop} , r17         'Speichere Timer1 low Byte nach Rc_signal_x_stop low Byte
   in r17 , tcnt1h                      'Timer1 high Byte holen
   sts {Rc_signal_1_stop} + 1 , r17     'Speichere Timer1 high Byte nach Rc_signal_x_stop high Byte
   ldi r17 , 1
   sts {Rc_signal_1_stop_flag} , r17    'Setze Flag zur Bearbeitung von Impulslaenge in Hauptschleife
  Ende1:
   pop r17                              'Register vom Stack zurückholen
 $end Asm
Return

Rc_eingang_2:
 $asm
   push r17                             'Register auf Stack sichern
   sbis pinA , 7                        'Skip next Instr if PINBx = 1
   rjmp Puls_ende2                      'Springe Puls_ende
   in r17 , tcnt1l                      'Timer1 low Byte holen
   sts {Rc_signal_2_start} , r17        'Speichere Timer1 low Byte nach Rc_signal_x_start low Byte
   in r17 , tcnt1h                      'Timer1 high Byte holen
   sts {Rc_signal_2_start} + 1 , r17    'Speichere Timer1 high Byte nach Rc_signal_x_start high Byte
   rjmp ende2                           'Springe zum Ende
  Puls_ende2:
   in r17 , tcnt1l                      'Timer1 low Byte holen
   sts {Rc_signal_2_stop} , r17         'Speichere Timer1 low Byte nach Rc_signal_x_stop low Byte
   in r17 , tcnt1h                      'Timer1 high Byte holen
   sts {Rc_signal_2_stop} + 1 , r17     'Speichere Timer1 high Byte nach Rc_signal_x_stop high Byte
   ldi r17 , 1
   sts {Rc_signal_2_stop_flag} , r17    'Setze Flag zur Bearbeitung von Impulslaenge in Hauptschleife
  Ende2:
   pop r17                              'Register vom Stack zurückholen
 $end Asm
Return

Rc_eingang_3:
 $asm
   push r17                             'Register auf Stack sichern
   sbis pinB , 2                        'Skip next Instr if PINBx = 1
   rjmp Puls_ende3                      'Springe Puls_ende
   in r17 , tcnt1l                      'Timer1 low Byte holen
   sts {Rc_signal_3_start} , r17        'Speichere Timer1 low Byte nach Rc_signal_x_start low Byte
   in r17 , tcnt1h                      'Timer1 high Byte holen
   sts {Rc_signal_3_start} + 1 , r17    'Speichere Timer1 high Byte nach Rc_signal_x_start high Byte
   rjmp ende3                           'Springe zum Ende
  Puls_ende3:
   in r17 , tcnt1l                      'Timer1 low Byte holen
   sts {Rc_signal_3_stop} , r17         'Speichere Timer1 low Byte nach Rc_signal_x_stop low Byte
   in r17 , tcnt1h                      'Timer1 high Byte holen
   sts {Rc_signal_3_stop} + 1 , r17     'Speichere Timer1 high Byte nach Rc_signal_x_stop high Byte
   ldi r17 , 1
   sts {Rc_signal_3_stop_flag} , r17    'Setze Flag zur Bearbeitung von Impulslaenge in Hauptschleife
  Ende3:
   pop r17                              'Register vom Stack zurückholen
 $end Asm
Return



Servoausgabe:

 $asm
   push r16                             'Register auf Stack sichern
   in r16,sreg                          'Statusregister holen und halten
   push r17                             'Register auf Stack sichern
   lds r17 , {kanal}                    'hole Kanalnummer

   cpi r17 , 1                          'check Kanal und ...
   brne LABEL_KANAL_2                   '... "wenn nicht gleich" verzweige zum nächsten Kanal
   sbic porta , 0                       'Skip next instr. wenn PORTA.0 = 0 ist
   rjmp label_1                         'Springe zum LOW-setzen des Servosignals
   sts {kanal} , r17                    'Sichere Kanalnummer
   lds r17 , {Servoausgabe_1}           'Hole aufbereiteten Pulslängenwert für Timer0
   Out Tcnt0 , R17                      'Setze Timer0 mit Pulslängenwert
   sbi porta , 0                        'Setze Servosignal HIGH
   rjmp Ende_isr                        'Springe zum Ende der ISR
  Label_1:
   cbi porta , 0                        'Setze Servosignal nach LOW
   inc r17                              'Erhöhe Kanalnummer


  Label_kanal_2:                        'Bearbeitung von Kanal 2
   cpi r17 , 2                          'check Kanal und ...
   brne LABEL_KANAL_3                   '... "wenn nicht gleich" verzweige zum nächsten Kanal
   sbic porta , 1                       'Skip next instr. wenn PORTA.1 = 0 ist
   rjmp label_2                         'Springe zum LOW-setzen des Servosignals
   sts {kanal} , r17                    'Sichere Kanalnummer
   lds r17 , {Servoausgabe_2}           'Hole aufbereiteten Pulslängenwert für Timer0
   Out Tcnt0 , R17                      'Setze Timer0 mit Pulslängenwert
   sbi porta , 1                        'Setze Servosignal HIGH
   rjmp Ende_isr                        'Springe zum Ende der ISR
  Label_2:
   cbi porta , 1                        'Setze Servosignal nach LOW
   inc r17                              'Erhöhe Kanalnummer


  Label_kanal_3:                        'Bearbeitung von Kanal 3
   cpi r17 , 3                          'check Kanal und ...
   brne LABEL_KANAL_4                   '... "wenn nicht gleich" verzweige zum nächsten Kanal
   sbic porta , 2                       'Skip next instr. wenn PORTA.2 = 0 ist
   rjmp label_3                         'Springe zum LOW-setzen des Servosignals
   sts {kanal} , r17                    'Sichere Kanalnummer
   lds r17 , {Servoausgabe_3}           'Hole aufbereiteten Pulslängenwert für Timer0
   Out Tcnt0 , R17                      'Setze Timer0 mit Pulslängenwert
   sbi porta , 2                        'Setze Servosignal HIGH
   rjmp Ende_isr                        'Springe zum Ende der ISR
  Label_3:
   cbi porta , 2                        'Setze Servosignal nach LOW
   inc r17                              'Erhöhe Kanalnummer


  Label_kanal_4:                        'Bearbeitung von Kanal 4 mit anschließendem Pausenanfang
   cpi r17 , 4                          'check Kanal und ...
   brne LABEL_KANAL_5                   '... "wenn nicht gleich" verzweige zum nächsten Kanal
   sbic porta , 3                       'Skip next instr. wenn PORTA.3 = 0 ist
   rjmp label_4                         'Springe zum LOW-setzen des Servosignals
   sts {kanal} , r17                    'Sichere Kanalnummer
   lds r17 , {Servoausgabe_4}           'Hole aufbereiteten Pulslängenwert für Timer0
   Out Tcnt0 , R17                      'Setze Timer0 mit Pulslängenwert
   sbi porta , 3                        'Setze Servosignal HIGH
   rjmp Ende_isr                        'Springe zum Ende der ISR
  Label_4:
   cbi porta , 3                        'Setze Servosignal nach LOW

'Pausenauffüllung

   'Pausenanfang
   inc r17
   sts {kanal} , r17                    'Sichere Kanalnummer
   ldi r17 , &B00000101                 'CS00 und CS02 für prescaler 1024 in Timer0 vorbereiten
   Out Tccr0b , R17                     'Timer0 läuft mit 128µs Auflösung
   ldi r17 , 162                        'Wert für ca 12ms bis zum nächsten OVF (162=256-94) bei prescaler 1024
   Out Tcnt0 , R17
   rjmp Ende_isr                        'Springe zum Ende der ISR

  'Pausenende
  Label_kanal_5:                        'Bearbeitung von Kanal 5 (Pausenende)
   ldi r17 , &B00000011                 'CS00 und CS01 für prescaler 64 in Timer0 vorbereiten
   Out Tccr0b , R17                     'Timer0 läuft wieder mit 8µs Auflösung
   ldi r17 , 1                          'Kanalnummer auf 1 setzen
   sts {kanal} , r17                    'Sichere Kanalnummer
  Ende_isr:
   pop r17                              'Register vom Stack zurückholen
   Out Sreg , R16                       'Statusregister zurückspeichern
   pop r16                              'Register vom Stack zurückholen
 $end Asm
Return