'===============================================================================
'Angry Bird Wing Control V2
'
'RC Servosignal und Servoausgabe mit 2 Timer (zitterfrei)
'Servo wobbelt, solange RC Wert im richtigen Bereich ist (ca. 50%-100%)
'Servo geht in Neutralstellung, solange RC Wert im untersten Bereich ist (ca.0%-25%)
'Bei fehlendem RC-Signal wird nach einer Sekunde mittels Watchdog ein Reset durchgeführt (Servo geht in Mittelstellung) ACHTUNG: FUNKTIONIERT NUR MIT EINGEBAUTEM R1 !
'
'RC Eingang 1 an Pin 7 (PB2, INT0)
'Servo 1 an Pin 6 (PB1)
'Kontroll-LED an Pin 3 (PB4), optional
'===============================================================================

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

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


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

Tccr1 = &B00001000                                          'Timer1 für RC-Signal Einlesung wird gestartet (Prescale 128) (geht beim Attiny45 nicht mit "Config"!)
                                                            ' Bei Prescale 128 1ms=63, 1,5ms=94, 2ms=125 (somit keine Timerüberlaufprobleme mehr bei langen RC Eingangssignalen über 2ms
Config Timer0 = Timer , Prescale = 64                       'Timer0 für Servoausgabe, Wert 125 entspricht 1ms, Wert 250 entspricht 2ms
On Timer0 Servoausgabe Nosave                               'Register werden manuel in der ISR gesichert
Enable Timer0


'-------------------------------------------------------------------------------------------------
'Watchdog konfigurieren (1sec) und starten
'-------------------------------------------------------------------------------------------------

Config Watchdog = 1024
Start Watchdog                                              'Watchdog starten, Reset erfogt bei anliegendem RC Signal in der ISR "Rc_eingang"


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

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


'-------------------------------------------------------------------------------------------------------------
'Konstanten definieren
'-------------------------------------------------------------------------------------------------------------
Const Jitterausgleich = 2                                   'Wert in µs um den sich gemessene Impulslaenge aendern darf/muss, bevor Servo gestellt wird (2)

'RC Input Werte (Timer1 Prescale 128)
Const Rc_in_min = 63                                        'Minimaler RC-Wert für die Berechnungen (min Wert 63 für 1ms)
Const Rc_in_max = 125                                       'Maximaler RC-Wert für die Berechnungen (max Wert 125 für 2ms)
Const Neutral = 78                                          'Unterhalb dieses RC-Wertes steht das Servo auf dem Servo_out_neutral (1,25ms = 78)
Const Wobble_off = 94                                       'Zwischen Neutral und diesen Wert (1,5ms = 94) wird nicht gewobbelt, Servo verbleibt auf letzter Position


'Servo Output Werte (Timer0 Prescale 64)
Const Servo_out_min = 125                                   'Minimaler Servowert (125 für 1ms)
Const Servo_out_max = 250                                   'Maximaler Servowert (250 für 2ms)
Const Servo_out_neutral = 187                               'Servowert für die Neutralstellung der Flügel (187 für 1,5ms, wird auch nach einem Reset durch Watchdog gesetzt)


'-------------------------------------------------------------------------------------------------------------
'Variablen definieren
'-------------------------------------------------------------------------------------------------------------
Dim Rc_signal_start As Byte
Dim Rc_signal_stop As Byte
Dim Rc_signal_stop_flag As Byte
Dim Rc_impulslaenge As Byte
Dim Rc_impulslaenge_alt As Byte
Dim Impulslaengenaenderung As Integer                       'wird nur einmal gebraucht, da nur Hilfswariable


'Variablen für Berechnungen
Dim Rc_berechnung_1 As Byte
Dim Wobblespeed As Word
Dim A As Word                                               'Word= bis max 65535
Dim B As Word                                               'Variable für Berechnung Wobblespeed
Dim C As Bit                                                'Marker zum Hoch- oder Runterzählen
Dim D As Bit                                                'Neutralmarker
Dim Servo As Byte


'Variablen für Servoausgabe

Dim Kanal As Byte
Dim Servoausgabe_final As Byte


'-------------------------------------------------------------------------------------------------
'Einigen Variablen Startwerte zuweisen
'-------------------------------------------------------------------------------------------------

Kanal = 1
Rc_berechnung_1 = Rc_in_min                                 'Rc_berechnung auf minimum (63 für 1ms)
Servo = 256 - Servo_out_neutral                             'Servo auf Neutralstellung (Mittelstellung)

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

Ddrb = &B00010010                                           'PB1 und PB4 wird Ausgang, restlicher PortB bleibt Eingang


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

Do

'RC Impulslänge berechnen und Grenzwerte festlegen

   If Rc_signal_stop_flag = 1 Then                          'Bearbeitung nur, wenn ISR Pulsende gefunden hat
      Disable Int0                                          'Mögliche Korruption durch ISR vorbeugen
      Rc_impulslaenge = Rc_signal_stop - Rc_signal_start    'Werte zwischen typ. 63 und 125 bei Prescale 128
      Enable Int0
      Rc_signal_stop_flag = 0
      Impulslaengenaenderung = Rc_impulslaenge_alt - Rc_impulslaenge       'SW-Hysterese

         If Abs(impulslaengenaenderung) >= Jitterausgleich Then
            Rc_impulslaenge_alt = Rc_impulslaenge

         If Rc_impulslaenge > Rc_in_max Then Rc_impulslaenge = Rc_in_max
         If Rc_impulslaenge < Rc_in_min Then Rc_impulslaenge = Rc_in_min

         Rc_berechnung_1 = Rc_impulslaenge                  'RC_Impulslaenge hat Werte zwischen 63 und 125
         End If
  End If



'Wobblen, Nichtwobbeln oder Neutralstellung?

   If Rc_berechnung_1 = Neutral Then Goto Wing_neutral
   If Rc_berechnung_1 < Neutral Then Goto Wing_neutral
   If Rc_berechnung_1 > Neutral And Rc_berechnung_1 < Wobble_off Then Goto No_wobbeln



'Wobblespeed berechnen (Werte zwischen ca 500 (schnell) und ca 2000 (langsam)

B = Rc_berechnung_1 - 55
B = B * 50
B = 4000 - B                                                'inventieren
Wobblespeed = B                                             'ergibt Werte zwischen ca 500 und ca 3600. Da aber das Wobbeln erst ab ca Mittelstellung losgeht, stehen ab da die gewünschten Werte von ca. 500 bis 2000 zur Verfügung.



'Hier wird hoch und runter gewobbelt

   If C = 0 Then                                            'hochzählen bis Obergrenze
      Incr Servo
      For A = 0 To Wobblespeed
      Next A
      If Servo > Servo_out_max Then C = 1                   'wenn Obergrenze erreicht, Marker für Runterzählen setzen
   End If

   If C = 1 Then                                            'runterzählen bis Untergrenze
      Decr Servo
      For A = 0 To Wobblespeed
      Next A
      If Servo < Servo_out_min Then C = 0                   'wenn Untergrenze erreicht, Marker für Raufzählen setzen
   End If



'Hier wird nicht gewobbelt, Servo verbleibt auf letzter Position

No_wobbeln:

   If D = 1 Then Servo = Servo_out_neutral                  'falls wir vorher in Neutralstellung waren, beginnt auch da das Wobbeln
   D = 0                                                    'Neutralmarker zurücksetzen

Servoausgabe_final = 256 - Servo                            'Servoausgabe_final enthält Werte zwischen 6 (2ms) und 131 (1ms)
Goto Ende                                                   'Wing_neutral überspringen



'Hier stehen die Wings neutral

Wing_neutral:

D = 1                                                       'Neutralmarker setzen, damit wobbeln wieder aus der Neutralstellung beginnt
Servoausgabe_final = 256 - Servo_out_neutral



Ende:
'(
'======= Nur zur optischen Kontrolle der Variable Rc_berechnung_1 =========
If Rc_berechnung_1 > Wobble_off Then
Portb.4 = 1
Else
Portb.4 = 0
End If
'=========================================================================
')

Loop
End




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


Rc_eingang:
$asm
   push r17                                                 'Register auf Stack sichern
   sbis pinb , 2                                            'Skip next Instr if PINBx = 1
   rjmp Puls_ende1                                          'Springe Puls_ende
   in r17 , tcnt1                                           'Timer1 Wert holen
   sts {Rc_signal_start} , r17                              'Speichere Timer1 nach Rc_signal_start

   rjmp ende1                                               'Springe zum Ende
  Puls_ende1:
   in r17 , tcnt1                                           'Timer1 Wert holen
   sts {Rc_signal_stop} , r17                               'Speichere Timer1 nach Rc_signal_stop

   ldi r17 , 1
   sts {Rc_signal_stop_flag} , r17                          'Setze Flag zur Bearbeitung von Impulslaenge in Hauptschleife
  Ende1:
   pop r17                                                  'Register vom Stack zurückholen
   Wdr                                                      'Watchdog resetten
$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 portB , 1                                           '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_final}                           'Hole aufbereiteten Pulslängenwert für Timer0
   Out Tcnt0 , R17                                          'Setze Timer0 mit Pulslängenwert
   sbi portB , 1                                            'Setze Servosignal HIGH
   rjmp Ende_isr                                            'Springe zum Ende der ISR
  Label_1:
   cbi portB , 1                                            'Setze Servosignal nach LOW
   inc r17                                                  'Erhöhe Kanalnummer

'Pausenauffüllung

   'Pausenanfang

   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_2:                                            'Bearbeitung von Kanal 2 (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

'===============================================================================
'Angry Bird Wing Control V2
'
'RC Servosignal und Servoausgabe mit 2 Timer (zitterfrei)
'Servo wobbelt, solange RC Wert im richtigen Bereich ist (ca. 50%-100%)
'Servo geht in Neutralstellung, solange RC Wert im untersten Bereich ist (ca.0%-25%)
'Bei fehlendem RC-Signal wird nach einer Sekunde mittels Watchdog ein Reset durchgeführt (Servo geht in Mittelstellung) ACHTUNG: FUNKTIONIERT NUR MIT EINGEBAUTEM R1 !
'
'RC Eingang 1 an Pin 7 (PB2, INT0)
'Servo 1 an Pin 6 (PB1)
'Kontroll-LED an Pin 3 (PB4), optional
'===============================================================================

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

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


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

Tccr1 = &B00001000                                          'Timer1 für RC-Signal Einlesung wird gestartet (Prescale 128) (geht beim Attiny45 nicht mit "Config"!)
                                                            ' Bei Prescale 128 1ms=63, 1,5ms=94, 2ms=125 (somit keine Timerüberlaufprobleme mehr bei langen RC Eingangssignalen über 2ms
Config Timer0 = Timer , Prescale = 64                       'Timer0 für Servoausgabe, Wert 125 entspricht 1ms, Wert 250 entspricht 2ms
On Timer0 Servoausgabe Nosave                               'Register werden manuel in der ISR gesichert
Enable Timer0


'-------------------------------------------------------------------------------------------------
'Watchdog konfigurieren (1sec) und starten
'-------------------------------------------------------------------------------------------------

Config Watchdog = 1024
Start Watchdog                                              'Watchdog starten, Reset erfogt bei anliegendem RC Signal in der ISR "Rc_eingang"


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

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


'-------------------------------------------------------------------------------------------------------------
'Konstanten definieren
'-------------------------------------------------------------------------------------------------------------
Const Jitterausgleich = 2                                   'Wert in µs um den sich gemessene Impulslaenge aendern darf/muss, bevor Servo gestellt wird (2)

'RC Input Werte (Timer1 Prescale 128)
Const Rc_in_min = 63                                        'Minimaler RC-Wert für die Berechnungen (min Wert 63 für 1ms)
Const Rc_in_max = 125                                       'Maximaler RC-Wert für die Berechnungen (max Wert 125 für 2ms)
Const Neutral = 78                                          'Unterhalb dieses RC-Wertes steht das Servo auf dem Servo_out_neutral (1,25ms = 78)
Const Wobble_off = 94                                       'Zwischen Neutral und diesen Wert (1,5ms = 94) wird nicht gewobbelt, Servo verbleibt auf letzter Position


'Servo Output Werte (Timer0 Prescale 64)
Const Servo_out_min = 125                                   'Minimaler Servowert (125 für 1ms)
Const Servo_out_max = 250                                   'Maximaler Servowert (250 für 2ms)
Const Servo_out_neutral = 187                               'Servowert für die Neutralstellung der Flügel (187 für 1,5ms, wird auch nach einem Reset durch Watchdog gesetzt)


'-------------------------------------------------------------------------------------------------------------
'Variablen definieren
'-------------------------------------------------------------------------------------------------------------
Dim Rc_signal_start As Byte
Dim Rc_signal_stop As Byte
Dim Rc_signal_stop_flag As Byte
Dim Rc_impulslaenge As Byte
Dim Rc_impulslaenge_alt As Byte
Dim Impulslaengenaenderung As Integer                       'wird nur einmal gebraucht, da nur Hilfswariable


'Variablen für Berechnungen
Dim Rc_berechnung_1 As Byte
Dim Wobblespeed As Word
Dim A As Word                                               'Word= bis max 65535
Dim B As Word                                               'Variable für Berechnung Wobblespeed
Dim C As Bit                                                'Marker zum Hoch- oder Runterzählen
Dim D As Bit                                                'Neutralmarker
Dim Servo As Byte


'Variablen für Servoausgabe

Dim Kanal As Byte
Dim Servoausgabe_final As Byte


'-------------------------------------------------------------------------------------------------
'Einigen Variablen Startwerte zuweisen
'-------------------------------------------------------------------------------------------------

Kanal = 1
Rc_berechnung_1 = Rc_in_min                                 'Rc_berechnung auf minimum (63 für 1ms)
Servo = 256 - Servo_out_neutral                             'Servo auf Neutralstellung (Mittelstellung)

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

Ddrb = &B00010010                                           'PB1 und PB4 wird Ausgang, restlicher PortB bleibt Eingang


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

Do

'RC Impulslänge berechnen und Grenzwerte festlegen

   If Rc_signal_stop_flag = 1 Then                          'Bearbeitung nur, wenn ISR Pulsende gefunden hat
      Disable Int0                                          'Mögliche Korruption durch ISR vorbeugen
      Rc_impulslaenge = Rc_signal_stop - Rc_signal_start    'Werte zwischen typ. 63 und 125 bei Prescale 128
      Enable Int0
      Rc_signal_stop_flag = 0
      Impulslaengenaenderung = Rc_impulslaenge_alt - Rc_impulslaenge       'SW-Hysterese
      Impulslaengenaenderung = Abs(impulslaengenaenderung)

         If Impulslaengenaenderung >= Jitterausgleich Then
            Rc_impulslaenge_alt = Rc_impulslaenge

         If Rc_impulslaenge > Rc_in_max Then Rc_impulslaenge = Rc_in_max
         If Rc_impulslaenge < Rc_in_min Then Rc_impulslaenge = Rc_in_min

         Rc_berechnung_1 = Rc_impulslaenge                  'RC_Impulslaenge hat Werte zwischen 63 und 125
         End If
  End If



'Wobblen, Nichtwobbeln oder Neutralstellung?

   If Rc_berechnung_1 = Neutral Then Goto Wing_neutral
   If Rc_berechnung_1 < Neutral Then Goto Wing_neutral
   If Rc_berechnung_1 > Neutral And Rc_berechnung_1 < Wobble_off Then Goto No_wobbeln



'Wobblespeed berechnen (Werte zwischen ca 500 (schnell) und ca 2000 (langsam)

B = Rc_berechnung_1 - 55
B = B * 50
B = 4000 - B                                                'inventieren
Wobblespeed = B                                             'ergibt Werte zwischen ca 500 und ca 3600. Da aber das Wobbeln erst ab ca Mittelstellung losgeht, stehen ab da die gewünschten Werte von ca. 500 bis 2000 zur Verfügung.



'Hier wird hoch und runter gewobbelt

   If C = 0 Then                                            'hochzählen bis Obergrenze
      Incr Servo
      For A = 0 To Wobblespeed
      Next A
      If Servo > Servo_out_max Then C = 1                   'wenn Obergrenze erreicht, Marker für Runterzählen setzen
   End If

   If C = 1 Then                                            'runterzählen bis Untergrenze
      Decr Servo
      For A = 0 To Wobblespeed
      Next A
      If Servo < Servo_out_min Then C = 0                   'wenn Untergrenze erreicht, Marker für Raufzählen setzen
   End If



'Hier wird nicht gewobbelt, Servo verbleibt auf letzter Position

No_wobbeln:

   If D = 1 Then Servo = Servo_out_neutral                  'falls wir vorher in Neutralstellung waren, beginnt auch da das Wobbeln
   D = 0                                                    'Neutralmarker zurücksetzen

Servoausgabe_final = 256 - Servo                            'Servoausgabe_final enthält Werte zwischen 6 (2ms) und 131 (1ms)
Goto Ende                                                   'Wing_neutral überspringen



'Hier stehen die Wings neutral

Wing_neutral:

D = 1                                                       'Neutralmarker setzen, damit wobbeln wieder aus der Neutralstellung beginnt
Servoausgabe_final = 256 - Servo_out_neutral



Ende:
'(
'======= Nur zur optischen Kontrolle der Variable Rc_berechnung_1 =========
If Rc_berechnung_1 > Wobble_off Then
Portb.4 = 1
Else
Portb.4 = 0
End If
'=========================================================================
')

Loop
End




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


Rc_eingang:
$asm
   push r17                                                 'Register auf Stack sichern
   sbis pinb , 2                                            'Skip next Instr if PINBx = 1
   rjmp Puls_ende1                                          'Springe Puls_ende
   in r17 , tcnt1                                           'Timer1 Wert holen
   sts {Rc_signal_start} , r17                              'Speichere Timer1 nach Rc_signal_start

   rjmp ende1                                               'Springe zum Ende
  Puls_ende1:
   in r17 , tcnt1                                           'Timer1 Wert holen
   sts {Rc_signal_stop} , r17                               'Speichere Timer1 nach Rc_signal_stop

   ldi r17 , 1
   sts {Rc_signal_stop_flag} , r17                          'Setze Flag zur Bearbeitung von Impulslaenge in Hauptschleife
  Ende1:
   pop r17                                                  'Register vom Stack zurückholen
   Wdr                                                      'Watchdog resetten
$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 portB , 1                                           '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_final}                           'Hole aufbereiteten Pulslängenwert für Timer0
   Out Tcnt0 , R17                                          'Setze Timer0 mit Pulslängenwert
   sbi portB , 1                                            'Setze Servosignal HIGH
   rjmp Ende_isr                                            'Springe zum Ende der ISR
  Label_1:
   cbi portB , 1                                            'Setze Servosignal nach LOW
   inc r17                                                  'Erhöhe Kanalnummer

'Pausenauffüllung

   'Pausenanfang

   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_2:                                            'Bearbeitung von Kanal 2 (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