Code:
	
'******************************************************************************
'***                      Delta-Ansteuerung3.BAS                            ***
'******************************************************************************
'***                                                                        ***
'***  Stellt eine Rückwärtskinematik für einen 3-Armigen Roboter nach       ***
'***  Delta-Art zur Verfügung.                                              ***
'***                                                                        ***
'***  Programm wurde für AtMega32 auf RN_Control V1.4 konzipiert.           ***
'***                                                                        ***
'******************************************************************************
$regfile = "m32def.dat"
$framesize = 32
$swstack = 32
$hwstack = 32
$crystal = 16000000
$baud = 9600
'Für Tastenabfrage:
Config Adc = Single , Prescaler = Auto
Config Pina.7 = Input
Porta.7 = 1
'******************************************************************************
'***  Deklaration von Unterprogrammen                                       ***
'******************************************************************************
Declare Function Tastenabfrage() As Byte
Declare Sub Update_ae()
Declare Sub Update_abst()
Declare Function Check_abst() As Byte
Declare Sub Update_ebene1()
Declare Sub Init_calc()
Declare Sub Clr_hilf()
Declare Sub Change_t()
Declare Sub Nix()
'******************************************************************************
'***  Deklaration von Variablen                                             ***
'******************************************************************************
Dim Taste As Byte                                           'Für die Onboard-Tasten
Dim Abst_ok As Byte                                         'Für die Rückgabe von Check_abst()
Dim I As Integer
Dim J As Integer
Dim K As Integer
Dim Single_hilf(5) As Single                                'Hilfsvariablen für Zwischenergebnisse
Dim Long_hilf(5) As Long
Dim B_x As Long                                             'Drei Koordinaten des Punktes B = Befestigungspunkt des Bots im Raum
Dim B_y As Long
Dim B_z As Long
Dim Oa As Long                                              'Länge des Servosnahen Armteils
Dim Ua As Long                                              'Länge des servofernen Armteils
Dim Abst_b_aa_xy As Long                                    'Abstand der Arm-Anfänge vom Befestigungspunkt B, gemessen in Projektion auf die xy-Ebene
Dim Abst_b_aa_z As Long                                     'Abstand der Arm-Anfänge vom Befestigungspunkt B, gemessen nur in z-Richtung
Dim Abst_t_ae_xy As Long                                    'Abstand der Arm-Enden vom Tool-Punkt T, gemessen in Projektion auf die xy-Ebene
Dim Abst_t_ae_z As Long                                     'Abstand der Arm-Enden vom Tool-Punkt T, gemessen nur in z-Richtung
Dim Aa_x(3) As Long                                         'x-Koordinate des Armanfangs für jeden Arm
Dim Aa_y(3) As Long                                         'y-Koordinate des Armanfangs für jeden Arm
Dim Aa_z(3) As Long                                         'z-Koordinate des Armanfangs für jeden Arm
Dim Ae_x(3) As Long                                         'x-Koordinate des Armendes für jeden Arm
Dim Ae_y(3) As Long                                         'y-Koordinate des Armendes für jeden Arm
Dim Ae_z(3) As Long                                         'z-Koordinate des Armendes für jeden Arm
Dim Abst_min As Long                                        'Minimaler Abstand, den Armanfang zu Armende haben muss
Dim Abst_max As Long                                        'Maximaler Abstand, den Armanfang zu Armende haben darf
Dim T_x As Long                                             'Drei Koordinaten des Punktes T = Tool-Punkt.
Dim T_y As Long                                             'Das ist der Punkt, des der Bot erreichen soll.
Dim T_z As Long
Dim Abst(3) As Long                                         'Abstand von Armanfang zu Armende für jeden Arm.
Dim D(12) As Single                                         'Parameter der Schnittebene E2. Siehe Dokumentation ######################### TODO: ERKLÄRUNG IN DOKU EINFÜGEN ###########################
Dim E(3) As Single                                          'Parameter der Ebene E1 zu jedem Arm. Die Ebene geht durch B, AA und [ B - (0,0,1) ]
'******************************************************************************
'***  Definition der Konstruktionsmerkmale                                  ***
'******************************************************************************
'Nachfolgend werden die Konstruktionsmerkmale des Bots festgelegt. Die Bedeutung
'der Variablen ist bei der Deklaration kommentiert.
B_x = 0
B_y = 0
B_z = 0
Oa = 100000000                                              '=10cm
Ua = 200000000                                              '=20cm
Abst_b_aa_xy = 10000000                                     '=1cm
Abst_b_aa_z = 20000000                                      '=2cm
Abst_t_ae_xy = 20000000                                     '=2cm
Abst_t_ae_z = 10000000                                      '=1cm
'******************************************************************************
'******************************************************************************
'******************************************************************************
'***  Hauptprogramm                                                         ***
'******************************************************************************
'******************************************************************************
'******************************************************************************
Debug On                                                    'Wenn ON, werden viele Zwischenergebnisse ausgegeben.
                                                             'Wenn OFF, werden nur Fehler ausgegeben
Call Init_calc
Print
Print "Gestartet"
Print "Tastenbelegung:"
Print "Taste1: Testweise den Punkt T ändern"
Print "Taste2: Armenden berechnen & Armabstaende berechnen"
Print "Taste3: Armabstaende pruefen"
Print "Taste4: Schnittebene berechnen"
Print "Taste5: Hilfsvariablen nullen"
Do
   Taste = Tastenabfrage()
   If Taste <> 0 Then
      Select Case Taste
         Case 1
            Call Change_t
         Case 2
            Call Update_ae
            Call Update_abst
         Case 3
            Abst_ok = Check_abst()
         Case 4
            Call Update_ebene1
         Case 5
            Call Clr_hilf
      End Select
   End If
   Waitms 100
Loop
End
'******************************************************************************
'***  Unterprogramme                                                        ***
'******************************************************************************
'Fragt die Tastatur am analogen Port A.7 ab. Rückgabewert=Tastennummer(1...5)
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
'Generiert aus dem Toolpunkt T die Armendpunkte Ae
Sub Update_ae()
Debug "Update_ae() gestartet"
'Arm1
Ae_x(1) = T_x
Ae_y(1) = T_y + Abst_t_ae_xy
Ae_z(1) = T_z + Abst_t_ae_z
'Arm2
Single_hilf(1) = 0.866025403784435                          '=Cos(30°)
Long_hilf(1) = Single_hilf(1) * Abst_t_ae_xy
Ae_x(2) = T_x - Long_hilf(1)
Long_hilf(2) = Abst_t_ae_xy / 2
Ae_y(2) = T_y - Long_hilf(2)
Ae_z(2) = T_z + Abst_t_ae_z
'Arm3
Ae_x(3) = T_x + Long_hilf(1)
Ae_y(3) = T_y - Long_hilf(2)
Ae_z(3) = T_z + Abst_t_ae_z
Debug "Armenden berechnet:"
Debug "Ae1: ( " ; Ae_x(1) ; " , " ; Ae_y(1) ; " , " ; Ae_z(1) ; ")"
Debug "Ae2: ( " ; Ae_x(2) ; " , " ; Ae_y(2) ; " , " ; Ae_z(2) ; ")"
Debug "Ae3: ( " ; Ae_x(3) ; " , " ; Ae_y(3) ; " , " ; Ae_z(3) ; ")"
End Sub
'###############################################################################
'Berechnet den Abstand von Armanfang zu Armende für jeden Arm.
'Die Formel ist der "verallgemeinerte Pythagoras".
Sub Update_abst()
Debug "Update_abst() gestartet"
For I = 1 To 3
   Long_hilf(1) = Aa_x(i) - Ae_x(i)
   Single_hilf(1) = Long_hilf(1) * Long_hilf(1)
   Long_hilf(1) = Aa_y(i) - Ae_y(i)
   Single_hilf(2) = Long_hilf(1) * Long_hilf(1)
   Long_hilf(1) = Aa_z(i) - Ae_z(i)
   Single_hilf(3) = Long_hilf(1) * Long_hilf(1)
   Single_hilf(3) = Single_hilf(3) + Single_hilf(2)
   Single_hilf(3) = Single_hilf(3) + Single_hilf(1)
   Abst(i) = Sqr(single_hilf(3))
Next I
Debug "Abstände berechnet:"
Debug "Abst(1) = " ; Abst(1)
Debug "Abst(2) = " ; Abst(2)
Debug "Abst(2) = " ; Abst(3)
End Sub
'###############################################################################
'Prueft, ob der Abstand von Armanfang zu Armende zulaessig ist,
'also, ob der Bot diesen Ort erreichen kann. Wenn der Ort erreichbar ist,
'wird 3 zurückgegeben, sonst eine Zahl < 3.
Function Check_abst() As Byte
Debug "Check_abst() gestartet"
Debug "Abst_max = " ; Abst_max
Debug "Abst_min = " ; Abst_min
Check_abst = 0
I = 1
For I = 1 To 3
   If Abst(i) < Abst_max And Abst(i) > Abst_min Then
      Check_abst = Check_abst + 1
   End If
Next I
Debug "Rueckgabewert: " ; Check_abst ;
Debug " (3=OK, weniger=Fehler)"
Debug "Check_abst beendet"
End Function
'###############################################################################
'Berechnet eine Ebenengleichung. In dieser Ebene muss das Armgelenk liegen.
'Die Ebenengleichung hat die Form D(j)*x + D(j+1)*y + D(j+2)*z - D(j+3) = 0
'wobei für Arm1 J=1, für Arm2 J=5 und für Arm3 J=9 gilt.
'Alle drei Ebenen liegen nachher im Array D(12) vor.
Sub Update_ebene1()
Debug "Update_ebene1() gestartet"
If Abst_ok = 3 Then
   Debug "Ebenendaten werden berechnet."
   I = 1
   J = 1
   Gosub Berechne_ebene1
   I = 2
   J = 5
   Gosub Berechne_ebene1
   I = 3
   J = 9
   Gosub Berechne_ebene1
   Debug "Ebenendaten berechnet"
   Debug "Ebene1(1): " ; D(1) ; " , " ; D(2) ; " , " ; D(3) ; " , " ; D(4)
   Debug "Ebene1(2): " ; D(5) ; " , " ; D(6) ; " , " ; D(7) ; " , " ; D(8)
   Debug "Ebene1(3): " ; D(9) ; " , " ; D(10) ; " , " ; D(11) ; " , " ; D(12)
Else
   Print "Fehler: Punkt nicht erreichbar. Ebene nicht berechnet."
   End If
Debug "Update_ebene1() beendet"
End Sub
'###############################################################################
'Führt einmalige Berechnungen durch, zB für den Arm-Anfangs-Punkt jedes Armes
Sub Init_calc()
Debug "Init_calc() gestartet"
'Arm1 Anfangspunkt
Aa_x(1) = B_x
Aa_y(1) = B_y + Abst_b_aa_xy
Aa_z(1) = B_z - Abst_b_aa_z
'Arm2 Anfangspunkt
Single_hilf(1) = 0.866025403784435                          '=Cos(30°)
Long_hilf(1) = Single_hilf(1) * Abst_b_aa_xy
Aa_x(2) = B_x - Long_hilf(1)
Long_hilf(2) = Abst_b_aa_xy / 2                             '=Sin(30°)*Abst_b_aa_xy = Abst_b_aa_xy / 2
Aa_y(2) = B_y - Long_hilf(2)
Aa_z(2) = B_z - Abst_b_aa_z
'Arm3 Anfangspunkt
Aa_x(3) = B_x + Long_hilf(1)
Aa_y(3) = B_y - Long_hilf(3)
Aa_z(3) = B_z - Abst_b_aa_z
'definiert minimalen/maximalen Abstand, den Armanfang von Armende haben muss.
'Die Korrektur verhindert, dass die Arme ganz durchgestreckt
'bzw komplett in sich gefaltet werden.
Long_hilf(1) = Oa - Ua
Long_hilf(1) = Abs(long_hilf(1))
'Korrektur:
Abst_min = Long_hilf(1)
Long_hilf(1) = Long_hilf(1) / 7
Abst_min = Abst_min + Long_hilf(1)
Long_hilf(1) = Oa + Ua
Abst_max = Long_hilf(1)
'Korrektur:
Long_hilf(1) = Long_hilf(1) / 8
Abst_max = Abst_max - Long_hilf(1)
Debug "Abst_max=" ; Abst_max
Debug "Abst_min=" ; Abst_min
Debug "Init_calc beendet"
End Sub
'###############################################################################
'Überschreibt alle Hilfsvariablen mit Nullen
Sub Clr_hilf()
Debug "Clr_Hilf() gestartet"
For I = 1 To 5
   Single_hilf(i) = 0
   Long_hilf(i) = 0
Next I
Debug "Clr_Hilf beendet:" ; Single_hilf(1) ; Single_hilf(2) ; Single_hilf(3) ; Single_hilf(4) ; Single_hilf(5) ; Long_hilf(1) ; Long_hilf(2) ; Long_hilf(3) ; Long_hilf(4) ; Long_hilf(5)
End Sub
'###############################################################################
Sub Change_t()
Debug "Change_t() gestartet"
Select Case T_z
   Case 0
      T_x = 20000000
      T_y = 30000000
      T_z = -103000000
   Case -103000000
      T_x = 0
      T_y = -30000000
      T_z = -150000000
   Case -150000000
      T_x = -20000000
      T_y = 60000000
      T_z = -180000000
   Case -180000000
      T_x = -40000000
      T_y = -20000000
      T_z = -250000000
   Case -250000000
      T_x = 60000000
      T_y = 150000000
      T_z = -320000000
   Case -320000000
      T_x = 0
      T_y = 0
      T_z = 0
End Select
Debug "T geändert auf ( " ; T_x ; " , " ; T_y ; " , " ; T_z ; ")"
Debug "Change_t beendet"
End Sub
'###############################################################################
'Dieses Label wird vom Sub Update-ebene1() benutzt.
'I=Arm-Nummer, J=Erstes Element im Array D(12) für den jeweiligen Arm.
Berechne_ebene1:
D(j) = Aa_x(i) - Ae_x(i)
D(j + 1) = Aa_y(i) - Ae_y(i)
D(j + 2) = Aa_z(i) - Ae_z(i)
Single_hilf(1) = Aa_x(i) * Aa_x(i)
D(j + 3) = Single_hilf(1)
Single_hilf(1) = Aa_y(i) * Aa_y(i)
D(j + 3) = D(j + 3) + Single_hilf(1)
Single_hilf(1) = Aa_z(i) * Aa_z(i)
D(j + 3) = D(j + 3) + Single_hilf(1)
Single_hilf(1) = Oa * Oa
D(j + 3) = D(j + 3) - Single_hilf(1)
Single_hilf(1) = Ae_x(i) * Ae_x(i)
D(j + 3) = D(j + 3) - Single_hilf(1)
Single_hilf(1) = Ae_y(i) * Ae_y(i)
D(j + 3) = D(j + 3) - Single_hilf(1)
Single_hilf(1) = Ae_z(i) * Ae_z(i)
D(j + 3) = D(j + 3) - Single_hilf(1)
Single_hilf(1) = Ua * Ua
D(j + 3) = D(j + 3) + Single_hilf(1)
Return
'Macht nichts und belegt unnoetig Speicherplatz. Die Funktion ist noetig,
'damit sich Nutzer von Windows heimisch fühlen.
Sub Nix()
End Sub
 Deine Algebraische Lösung interessiert mich natürlich sehr (und auch alles andere, was du zu diesem Thema so loswerden willst 
Lesezeichen