'
' Alle Berechnungen gehen von Folgendem aus:
' Der Bot ist an einer Ebene befestigt, die parallel zur xy-Ebene liegt
' Der Arm 1 liegt in y-Richtung.
' Der Arm 2 liegt, von oben gesehen, 120° links gedreht (Negative y- und x-Richtung).
' Der Arm 3 liegt, von oben gesehen, 120° rechts gedreht (Negative y-, positive x-Richtung).
' Alle Werte für Längen, Abstände und Positionen werden in Nanometern (nm) angegeben!
'


Declare Function Tastenabfrage() As Byte
Declare Sub Eins()
Declare Sub Zwei()
Declare Sub Drei()
Declare Sub Vier()
Declare Sub Fuenf()


$regfile = "m32def.dat"
$framesize = 32
$swstack = 32
$hwstack = 32
$crystal = 16000000                                         'Quarzfrequenz
$baud = 9600

Config Adc = Single , Prescaler = Auto                      'Für Tastenabfrage und Spannungsmessung
Config Pina.7 = Input                                       'Für Tastenabfrage
Porta.7 = 1                                                 'Pullup Widerstand ein

Dim Taste As Byte

' Deklaration der Variablen, die die Proportionen des Roboters beschreiben
' Alle Angaben in nm und als "Long" (32bit genauigkeit)
' Zulässig sind Werte zwischen -2147483648 und +2147483647

Print
Print "= = = = Delta-Steuerung = = = ="
Print
Print "Lade Konstruktionsparameter..."

' Fixer Befestigungspunkt B im Raum
Dim B_x As Long
Dim B_y As Long
Dim B_z As Long
                           B_x = 0
                           B_y = 0
                           B_z = 0


' Länge des Servoseitigen Armteils ("Oberer Armteil" -> Oa)
Dim Oa As Long
                           Oa = 100000000                   '=10cm


' Länge des Servofernen Armteils ("Unterer Armteil" -> Ua)
Dim Ua As Long
                           Ua = 200000000                   '=20cm


' Abstand der Armservoachsen vom Befestigungspunkt B auf der xy-Ebene
Dim Abst_b_aa_xy As Long
                           Abst_b_aa_xy = 70000000          '=7cm


' Abstand der Armservoachsenvom von der xy-Ebene
Dim Abst_b_aa_z As Long
                           Abst_b_aa_z = 30000000           '=3cm


' Abstand der Armenden vom Tool auf der xy-Ebene
Dim Abst_t_ae_xy As Long
                           Abst_t_ae_xy = 40000000          '=4cm

' Abstand der Armenden von der xy-Ebene
Dim Abst_t_ae_z As Long
                           Abst_t_ae_z = 10000000           '=1cm

Print "Konstruktionsparameter geladen"
Print
Print "Montagepunkt = (" ; B_x ; "," ; B_y ; "," ; B_z ; ")"
Print "Armanfangs-Versatz:"
Print Abst_b_aa_xy ; "nm vom Montagepunkt in xy-Ebene"
Print "und " ; Abst_b_aa_z ; "nm in z-Richtung"
Print "Armende-Versatz:"
Print Abst_t_ae_xy ; "nm vom Toolpunkt in xy-Ebene"
Print "und " ; Abst_t_ae_z ; "nm in z-Richtung"
Print
Print "Berechne Konstruktionsabhaengige Fixwerte"

' Arm-Anfangspunkte:
' Arm1: (Der von oben gesehen hintere Arm, genau in y-Richtung)
Dim A1a_x As Long
Dim A1a_y As Long
Dim A1a_z As Long
A1a_x = B_x
A1a_y = B_y + Abst_b_aa_xy
A1a_z = B_z - Abst_b_aa_z

'Arm2: (Der von oben gesehen linke Arm)
Dim A2a_x As Long
Dim A2a_y As Long
Dim A2a_z As Long
Dim Hilf1 As Single                                         'Hilfsvariable für Zwischenergebnisse
Dim Hilf2 As Long
Dim Hilf3 As Long
Hilf1 = 0.866025403784435                                   '=Cos(30°)
Hilf2 = Hilf1 * Abst_b_aa_xy
A2a_x = B_x - Hilf2
Hilf3 = Abst_b_aa_xy / 2                                    '=Sin(30°)*Abst_b_aa_xy = Abst_b_aa_xy / 2
A2a_y = B_y - Hilf3
A2a_z = B_z - Abst_b_aa_z

'Arm3: (Der von oben gesehen rechte Arm)
Dim A3a_x As Long
Dim A3a_y As Long
Dim A3a_z As Long
A3a_x = B_x + Hilf2
A3a_y = B_y - Hilf3
A3a_z = B_z - Abst_b_aa_z

Print "Fixwert-Berechnung abgeschlossen."
Print "Arm1-Anfang = ( " ; A1a_x ; " , " ; A1a_y ; " , " ; A1a_z ; " )"
Print "Arm2-Anfang = ( " ; A2a_x ; " , " ; A2a_y ; " , " ; A2a_z ; " )"
Print "Arm3-Anfang = ( " ; A3a_x ; " , " ; A3a_y ; " , " ; A3a_z ; " )"

Do
   Taste = Tastenabfrage()
   If Taste <> 0 Then

      Select Case Taste
         Case 1
            Call Eins
         Case 2
            Call Zwei
         Case 3
            Call Drei
         Case 4
            Call Vier
         Case 5
            Call Fuenf
      End Select
   End If
   Waitms 1000
Loop

End

Sub Eins()
Print "Sub 1"
End Sub


Sub Zwei()
Print "Sub 2"
End Sub


Sub Drei()
Print "Sub 3"
End Sub


Sub Vier()
Print "Sub 4"
End Sub


Sub Fuenf()
Print "Sub 5"
End Sub


'Diese Unterfunktion fragt die Tastatur am analogen Port ab
Function Tastenabfrage() As Byte
Local Ws As Word

   Tastenabfrage = 0
   Start Adc
   Ws = Getadc(7)
  ' Print "Tastenabfrage anpassen!ADC Wert ws=" ; Ws
   If Ws < 500 Then
      Select Case Ws
         Case 400 To 450
            Tastenabfrage = 1
         Case 330 To 380
            Tastenabfrage = 2
         Case 260 To 305
            Tastenabfrage = 3
         Case 180 To 220
            Tastenabfrage = 4
         Case 90 To 130
            Tastenabfrage = 5
      End Select
   End If
End Function

// Deltaarm ansteuern mit dem RP6                                  3.2.09  mic

#include "rblib.h"
#include "rblib.c"

volatile uint16_t p=0;
uint8_t  xpos=120, ypos=120, zpos=120, demo;
uint16_t speed;

void servo_init(void);
void pause(uint16_t p_10ms);
void moveto(uint8_t x, uint8_t y, uint8_t z, uint8_t pause_10ms);
void holen1(void);
void holen2(void);
void holen3(void);
void bringen1(void);
void bringen2(void);
void bringen3(void);
int main(void)
{
   rblib_init();
   servo_init();
   pause(100);
/*   for(demo=120; demo<190; demo++) moveto(demo,demo,demo,5);
   for(demo=189; demo>120; demo--) moveto(demo,demo,demo,4);
   for(demo=121; demo<190; demo++) moveto(demo,demo,demo,3);
   for(demo=189; demo>120; demo--) moveto(demo,demo,demo,2);
   for(demo=121; demo<190; demo++) moveto(demo,demo,demo,2);
   for(demo=189; demo>120; demo--) moveto(demo,demo,demo,1);
   moveto(190,190,190,50);
   moveto(120,120,120,50);
*/

   // heben und legen
   holen1(); bringen1();
   holen3(); bringen3();

   while(1)
   {
   // tauschen
      holen1(); bringen2();
      holen3(); bringen1();
      holen2(); moveto(90,160,90,40); bringen3();
   }
   return(0);
}
ISR(TIMER0_COMP_vect)
{
   static uint16_t count=0;
   if(count>xpos) PORTA &= ~16; else PORTA |= 16;   // E_INT1 (Pin8)
   if(count>ypos) PORTC &= ~1;  else PORTC |= 1;   // SCL (Pin10)
   if(count>zpos) PORTC &= ~2;  else PORTC |= 2;   // SDA (Pin12)
   if(count<1000)count++; else { count=0; if(p) p--; }
}
void servo_init(void)
{
   DDRA |= 16;            // E_INT1 als Ausgang
   DDRC |= 3;            // SCL und SDA als Ausgang

   TCCR0 =  (0 << WGM00) | (1 << WGM01);               // CTC-Mode
   TCCR0 |= (0 << COM00) | (0 << COM01);               // ohne OCR-Pin
   TCCR0 |=   (0 << CS02)  | (1 << CS01) | (0 << CS00);   // prescaler /8
   TIMSK =  (1 << OCIE0);                               // Interrupt ein
   OCR0  = 10; // 100kHz?
}
void pause(uint16_t p_10ms)
{
   p=p_10ms;
   while(p);
}
void moveto(uint8_t x, uint8_t y, uint8_t z, uint8_t p_10ms)
{
   xpos=x;
   ypos=y;
   zpos=z;
   p=p_10ms;
   while(p);
}
void holen1(void)
{
      moveto(99,104,91,50);
      moveto(80,90,73,20);
      moveto(99,104,91,30);
}
void holen2(void)
{
      moveto(60,111,83,50);
      moveto(50,95,65,20);
      moveto(60,111,83,30);
}
void holen3(void)
{
      moveto(84,110,52,50);
      moveto(69,95,47,20);
      moveto(84,110,52,30);
}
void bringen1(void)
{
      moveto(99,104,91,50);
      pause(200);
}
void bringen2(void)
{
      moveto(60,111,83,50);
      pause(200);
}
void bringen3(void)
{
      moveto(81,107,50,50);
      pause(200);
}

'(
################################################################################
Name : Delta-Ansteuerung.bas
Zweck : Stellt Eine Rückwärtskinematik Für Einen 3 -armigen
                               "Delta" -roboter Zur Verfügung.
Mikroprozessor : : Atmega32 Auf Rn -control V1.4

 Alle Berechnungen Gehen Von Folgendem Aus:
 Der Bot Ist An Einer Ebene Befestigt , Die Parallel Zur Xy -ebene Liegt
 Der Arm 1 Liegt In Y -richtung.
 Der Arm 2 Liegt , Von Oben Gesehen , 120° Links Gedreht(negative Y - Und X -richtung).
 Der Arm 3 Liegt , Von Oben Gesehen , 120° Rechts Gedreht(negative Y - , Positive X -richtung).
 Alle Werte Für Längen , Abstände Und Positionen Werden In Nanometern(nm) Angegeben!
################################################################################
')
Declare Function Tastenabfrage() As Byte
Declare Sub Update_ae()
Declare Sub Change_t()
Declare Sub Nix()


$regfile = "m32def.dat"
$framesize = 32
$swstack = 32
$hwstack = 32
$crystal = 16000000                                         'Quarzfrequenz
$baud = 9600

Config Adc = Single , Prescaler = Auto                      'Für Tastenabfrage und Spannungsmessung
Config Pina.7 = Input                                       'Für Tastenabfrage
Porta.7 = 1                                                 'Pullup Widerstand ein

Dim Taste As Byte

' Deklaration der Variablen, die die Proportionen des Roboters beschreiben
' Alle Angaben in nm und als "Long" (32bit genauigkeit)
' Zulässig sind Werte zwischen -2147483648 und +2147483647

Print
Print "= = = = Delta-Steuerung = = = ="

' Fixer Befestigungspunkt B im Raum
Dim B_x As Long
Dim B_y As Long
Dim B_z As Long
                           B_x = 0
                           B_y = 0
                           B_z = 0


' Länge des Servoseitigen Armteils ("Oberer Armteil" -> Oa)
Dim Oa As Long
                           Oa = 100000000                   '=10cm


' Länge des Servofernen Armteils ("Unterer Armteil" -> Ua)
Dim Ua As Long
                           Ua = 200000000                   '=20cm


' Abstand des Armanfangs (aa) vom Befestigungspunkt B auf der Befestigungsebene (parallel zur xy-Ebene)
Dim Abst_b_aa_xy As Long
                           Abst_b_aa_xy = 70000000          '=7cm


' Abstand des Armanfangs (aa) von der Befestigungsebene
Dim Abst_b_aa_z As Long
                           Abst_b_aa_z = 30000000           '=3cm


' Abstand des Armendes (ae) vom Tool auf der xy-Parallel-Ebene
Dim Abst_t_ae_xy As Long
                           Abst_t_ae_xy = 40000000          '=4cm

' Abstand des Armendes (ae) von der xy-Parallel-Ebene
Dim Abst_t_ae_z As Long
                           Abst_t_ae_z = 20000000           '=2cm

'##### Berechne nun Konstruktionsabhängige Fixwerte #####

' Arm-Anfangspunkte:
' Arm1: (Der von oben gesehen hintere Arm, genau in y-Richtung)
Dim A1a_x As Long
Dim A1a_y As Long
Dim A1a_z As Long
A1a_x = B_x
A1a_y = B_y + Abst_b_aa_xy
A1a_z = B_z - Abst_b_aa_z

'Arm2: (Der von oben gesehen linke Arm)
Dim A2a_x As Long
Dim A2a_y As Long
Dim A2a_z As Long
Dim Hilf1 As Single                                         'Hilfsvariable für Zwischenergebnisse
Dim Hilf2 As Long
Dim Hilf3 As Long
Hilf1 = 0.866025403784435                                   '=Cos(30°)
Hilf2 = Hilf1 * Abst_b_aa_xy
A2a_x = B_x - Hilf2
Hilf3 = Abst_b_aa_xy / 2                                    '=Sin(30°)*Abst_b_aa_xy = Abst_b_aa_xy / 2
A2a_y = B_y - Hilf3
A2a_z = B_z - Abst_b_aa_z

'Arm3: (Der von oben gesehen rechte Arm)
Dim A3a_x As Long
Dim A3a_y As Long
Dim A3a_z As Long
A3a_x = B_x + Hilf2
A3a_y = B_y - Hilf3
A3a_z = B_z - Abst_b_aa_z

'Maximale und minimale "Länge" des Arms (Abstand von Armanfang zu Armende)
'jeweils mit sicherheitsabsatnd von 10% bzw 30%, um instabile Positionen zu vermeiden.
Dim Abst_min As Long
Dim Abst_max As Long
Hilf2 = Oa - Ua
Hilf2 = Abs(hilf2)
Abst_min = Hilf2 * 1.3
Hilf2 = Oa + Ua
Abst_max = Hilf2 * 0.9


Print "Montagepunkt = (" ; B_x ; "," ; B_y ; "," ; B_z ; ")"
Print "Armanfangs-Versatz:"
Print Abst_b_aa_xy ; "nm vom Montagepunkt in xy-Ebene"
Print "und " ; Abst_b_aa_z ; "nm in z-Richtung."
Print "Armende-Versatz:"
Print Abst_t_ae_xy ; "nm vom Toolpunkt in xy-Ebene"
Print "und " ; Abst_t_ae_z ; "nm in z-Richtung."
Print "Arm1-Anfang = ( " ; A1a_x ; " , " ; A1a_y ; " , " ; A1a_z ; " )"
Print "Arm2-Anfang = ( " ; A2a_x ; " , " ; A2a_y ; " , " ; A2a_z ; " )"
Print "Arm3-Anfang = ( " ; A3a_x ; " , " ; A3a_y ; " , " ; A3a_z ; " )"

'Deklariere restliche Variablen:
'Tool-Punkt T (Dieser Punkt soll angefahren werden)
Dim T_x As Long
Dim T_y As Long
Dim T_z As Long
'Kontroll-Bits:
Dim Erreichbar As Bit                                       'Ist 1, wenn der derzeitig geladene Toolpunkt erreichbar ist.
Erreichbar = 0
'Arm-Endpunkte
'Arm1
Dim A1e_x As Long
Dim A1e_y As Long
Dim A1e_z As Long
'Arm2
Dim A2e_x As Long
Dim A2e_y As Long
Dim A2e_z As Long
'Arm3
Dim A3e_x As Long
Dim A3e_y As Long
Dim A3e_z As Long
'Abstände der Armenden:
Dim Abst(3) As Long
'Hilfsvariablen für Zwischenergebnisse
Dim Hilf4 As Single
Dim Hilf5 As Long
Dim Hilf6 As Long
Dim Hilf7 As Single
Dim I As Integer

Print "Tastenbelegung:"
Print "1 = Armendpunkte berechnen + Pruefen, ob Punkt ereichbar"
Print "2 = Tool-z-Koordinate testweise aendern"

'
'## ## ## ## ## ## ## ## ## Hauptprogramm-Schleife ## ## ## ## ## ## ## ## ##
'
Do
   Taste = Tastenabfrage()
   If Taste <> 0 Then

      Select Case Taste
         Case 1
            Call Update_ae
         Case 2
            Call Change_t
         Case 3
            Call Nix
         Case 4
            Call Nix
         Case 5
            Call Nix
      End Select
   End If
   Waitms 20
Loop

End


Sub Update_ae()
' Berechnen der Arm-Endpunkte bei gegebenem Werkzeugpunkt W = ( W_x , W_y , W_z )
' und die Abstände der Arm-Enden "Abst1" bis "Abst3"
' Muss für jede Position natürlich neu vorgenommen werden!
' Arm1: (Der von oben gesehen hintere Arm, genau in y-Richtung)
A1e_x = T_x
A1e_y = T_y + Abst_t_ae_xy
A1e_z = T_z + Abst_t_ae_z

' Arm2: (Der von oben gesehen linke Arm)
Hilf4 = 0.866025403784435                                   '=Cos(30°)
Hilf5 = Hilf4 * Abst_t_ae_xy
A2e_x = T_x - Hilf5
Hilf6 = Abst_t_ae_xy / 2
A2e_y = T_y - Hilf6
A2e_z = T_z + Abst_t_ae_z

' Arm3: (Der von oben gesehen rechte Arm)
A3e_x = T_x + Hilf5
A3e_y = T_y - Hilf6
A3e_z = T_z + Abst_t_ae_z

' Debug-Ausgabe
Print "Arm-Endpunkte:"
Print "A1e = ( " ; A1e_x ; " , " ; A1e_y ; " , " ; A1e_z ; " )"
Print "A2e = ( " ; A2e_x ; " , " ; A2e_y ; " , " ; A2e_z ; " )"
Print "A3e = ( " ; A2e_x ; " , " ; A2e_y ; " , " ; A2e_z ; " )"

' Prüfen, ob der Punkt erreicht werden kann.
Erreichbar = 0
' Arm1:
Hilf2 = A1a_x - A1e_x
Hilf1 = Hilf2 * Hilf2
Hilf2 = A1a_y - A1e_y
Hilf4 = Hilf2 * Hilf2
Hilf2 = A1a_z - A1e_z
Hilf7 = Hilf2 * Hilf2
Hilf7 = Hilf7 + Hilf4
Hilf7 = Hilf7 + Hilf1
Abst(1) = Sqr(hilf7)
' Arm2:
Hilf2 = A2a_x - A2e_x
Hilf1 = Hilf2 * Hilf2
Hilf2 = A2a_y - A2e_y
Hilf4 = Hilf2 * Hilf2
Hilf2 = A2a_z - A2e_z
Hilf7 = Hilf2 * Hilf2
Hilf7 = Hilf7 + Hilf4
Hilf7 = Hilf7 + Hilf1
Abst(2) = Sqr(hilf7)
' Arm3:
Hilf2 = A3a_x - A3e_x
Hilf1 = Hilf2 * Hilf2
Hilf2 = A3a_y - A3e_y
Hilf4 = Hilf2 * Hilf2
Hilf2 = A3a_z - A3e_z
Hilf7 = Hilf2 * Hilf2
Hilf7 = Hilf7 + Hilf4
Hilf7 = Hilf7 + Hilf1
Abst(3) = Sqr(hilf7)

Print "Abstaende der Armenden: Arm1: " ; Abst(1) ; " Arm2: " ; Abst(2) ; " Arm3: " ; Abst(3)

Erreichbar = 0

For I = 1 To 3                                              ' Teste Arme 1-3
   If Abst(i) < Abst_max And Abst(i) > Abst_min Then        ' Wenn der Abstand ok ist...
      Erreichbar = 1                                        ' Dann setze das erreichbar-bit
   Else                                                     ' Sonst
      Erreichbar = 0                                        ' Clear das Erreichbar-bit
      Exit For                                              ' Und verlasse sofort die Schleife
   End If
Next I

If Erreichbar = 1 Then
   Print "Punkt ist erreichbar!"
   Else
   Print "Punkt ist NICHT erreichbar! Fehler!"
End If

Waitms 500

End Sub


' Ändert den Punkt T (zu Testzwecken) in z-Richtung, also nach oben/unten
Sub Change_t()
   Select Case T_z
      Case 0
         T_z = -50000000
      Case -50000000
         T_z = -100000000
      Case -100000000
         T_z = -150000000
      Case -150000000
         T_z = -300000000
      Case -300000000
         T_z = 0
   End Select
Print "T_z geändert auf " ; T_z

Waitms 500

End Sub

Function Tastenabfrage() As Byte
Local Ws As Word

   Tastenabfrage = 0
   Start Adc
   Ws = Getadc(7)
   If Ws < 500 Then
      Select Case Ws
         Case 400 To 450
            Tastenabfrage = 1
         Case 330 To 380
            Tastenabfrage = 2
         Case 260 To 305
            Tastenabfrage = 3
         Case 180 To 220
            Tastenabfrage = 4
         Case 90 To 130
            Tastenabfrage = 5
      End Select
   End If
End Function

Sub Nix()
End Sub

// Parallele Kinematik                                            21.3.2008 mic
// RP6 steuert Deltaroboter mit Greifer

#include "RP6RobotBaseLib.h"

// Servoausgänge 1-4
#define servoinit {DDRB |= (1<<PB7); PORTB &= ~(1<<PB7); DDRC |= 0b01110000; PORTC &= ~0b01110000;}
#define servo1on  PORTC |=  (1<<PC4)
#define servo1off PORTC &= ~(1<<PC4)
#define servo2on  PORTC |=  (1<<PC5)
#define servo2off PORTC &= ~(1<<PC5)
#define servo3on  PORTC |=  (1<<PC6)
#define servo3off PORTC &= ~(1<<PC6)
#define servo4on  PORTB |=  (1<<PB7)
#define servo4off PORTB &= ~(1<<PB7)

typedef struct
{
   uint16_t pos1, pos2, pos3, pos4;
   uint8_t pause;
} schritt;

schritt schrittfolge[10];
uint8_t servo1, servo2, servo3, servo4, s[5]={0,200,200,200,200};  //oben offen
uint16_t adc1, adc2, adc3;

void getadcs(void)
{
   adc1=readADC(7);
   adc2=readADC(1);
   adc3=readADC(0);
}
int main(void)
{
   initRobotBase();
   getadcs();
   servoinit;

   //Timer2 Initialisierung
   TCCR2 = (0 << WGM21) | (0 << COM20) | (1 << CS22); // Normal Mode, prescaler /64
   TIMSK |= (1 << TOIE2); // Timer2 Overflow-Interrupt erlauben
   //TIMSK &= ~(1 << TOIE2); // Timer2 Overflow-Interrupt verbieten

   servo1=s[1];
   servo2=s[2];
   servo3=s[3];
   servo4=s[4];
   while(1)
   {
     servo1=200; // hoch
     servo2=200;
   servo3=200;
   mSleep(600);

    servo4=154; // aufpicken
   mSleep(200);
    servo1=150;
     servo2=150;
   servo3=150;
   mSleep(400);

     servo1=220; // hoch
     servo2=220;
   servo3=220;
   mSleep(400);
   servo2=70;  // rueber
   servo3=250;
   mSleep(150);
     servo1=130;
   mSleep(500);

   servo4=170; // oeffnen
   mSleep(200);
   servo1=120; // neu positionieren
   servo2=60;
   servo3=240;
   mSleep(300);
   servo4=154; // schliesen
   mSleep(200);

     servo1=200; // hoch
     servo2=120;
   servo3=240;
   mSleep(200);

     servo1=220; // mitte oben
     servo2=220;
   servo3=220;
   mSleep(600);
     servo1=170; // runter
     servo2=170;
   servo3=170;
   mSleep(500); // fallenlassen
   servo4=170;
   mSleep(300);
   }
   return(0);
}
ISR (TIMER2_OVF_vect)
{
   static uint8_t servo_nr=0, grundimpuls=0;
   static uint16_t impulspause;
   if(servo_nr)
   {
      if(grundimpuls++ & 1) TCNT2=200; else
      {
         if(servo_nr==1) {TCNT2=servo1; servo1on; impulspause-=servo1;}
         if(servo_nr==2) {TCNT2=servo2; servo1off; servo2on; impulspause-=servo2;}
         if(servo_nr==3) {TCNT2=servo3; servo2off; servo3on; impulspause-=servo3;}
         if(servo_nr==4) {TCNT2=servo4; servo3off; servo4on; impulspause-=servo4;}
         if(servo_nr==5) {servo4off; servo_nr=0;}
         if(servo_nr) servo_nr++;
      }
   }
   else
   {
      if(impulspause>256) impulspause-=256;
         else {TCNT2=-impulspause; servo_nr++; impulspause=1500;}
   }
}