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