hallo,
in Bascom heist der befehl dan einfach acos(var)
aber das führt zum selben ergebnis!
Code:
$regfile = "m8def.dat"
$crystal = 3686400
$baud = 19200
$framesize = 64
$swstack = 64
$hwstack = 64
Config Timer1 = Timer , Prescale = 1 'timer für Servos
Enable Timer1
Timer1 = 56320
Config Portc = Output
Portc.0 = 0 'hier hängt servo1
Portc.1 = 0 'hier hängt servo2
Portc.2 = 0 'hier hängt servo3
Portc.3 = 0 'hier hängt servo4
On Timer1 Servoirq 'servo
Enable Interrupts
Declare Sub Berechnung
Dim Kanal As Byte
Dim Servo(4) As Word 'links: 63200, mitte 60000, rechts 56800
Dim Hoehe As Byte
Dim Abstand As Byte
Dim Hoehe_q As Word
Dim Abstand_q As Word
Dim Dis_q As Word
Dim Dis As Byte
Dim Knie_wi As Byte
Dim Kniea As Word
Dim Knieb As Word
Dim Knie_zw_a As Word
Dim Knie_zw_b As Word
Dim Knie_zw_c As Word
Dim Knie_zw_d As Integer
Dim Knie_zw_e As Single
Dim Knie_zw_wi As Single
Dim Huft_wi As Byte
Dim Hufta As Word
Dim Huftb As Word
Const Oberschenkel = 67
Const Unterschenkel = 87
Const Oberschenkel_q = 4489
Const Unterschenkel_q = 7569
Hoehe = 50
Abstand = 100
Call Berechnung
Servo(3) = 60000
Servo(4) = 60000
Wait 3
Do
Hoehe = 50
Abstand = 100
Call Berechnung
Wait 2
Loop
Servoirq:
If Kanal = 0 Then
If Portc.0 = 0 Then 'wenn port low
Timer1 = Servo(1) 'dann timer auf entsprechende verzögerung
Portc.0 = 1 'und port anschalten
Else 'das hier passiert erst bei dem darauf folgenden interrupt
Portc.0 = 0 'dann port wieder ausschalten
Incr Kanal 'und den nächsten kanal bearbeiten
End If
End If
If Kanal = 1 Then
If Portc.1 = 0 Then
Timer1 = Servo(2)
Portc.1 = 1
Else
Portc.1 = 0
Incr Kanal
End If
End If
If Kanal = 2 Then
If Portc.2 = 0 Then
Timer1 = Servo(3)
Portc.2 = 1
Else
Portc.2 = 0
Incr Kanal
End If
End If
If Kanal = 3 Then
If Portc.3 = 0 Then
Timer1 = Servo(4)
Portc.3 = 1
Else
Portc.3 = 0
Incr Kanal
End If
End If
If Kanal = 4 Then
Timer1 = 21300 '54016 eine pause von ca. 12ms bis zum nächsten interrupt. Bei guten Servos oder Brushlessreglern kann man hier bis auf 65530 gehen ==> ansteuerfrequenz von ~ 200Hz
Kanal = 0
End If
Return
'------------------------------------------
Sub Berechnung
If Hoehe > 110 Then Hoehe = 110
If Abstand < 37 Then Abstand = 37
Hoehe_q = Hoehe * Hoehe
Abstand_q = Abstand * Abstand
Dis_q = Hoehe_q + Abstand_q
Dis = Sqr(dis_q)
'-------------- Knie ----------------------
Knie_zw_a = Dis_q - Unterschenkel_q
Knie_zw_b = Knie_zw_a - Oberschenkel_q
Knie_zw_c = Unterschenkel * Oberschenkel
Knie_zw_d = -2 * Knie_zw_c
Knie_zw_e = Knie_zw_b / Knie_zw_d
Knie_zw_wi = Cos(knie_zw_e)
Knie_wi = Knie_zw_wi * 100
If Knie_wi < 45 Then Knie_wi = 45
If Knie_wi > 180 Then Knie_wi = 180
Kniea = 6400 / 180
Knieb = Kniea * Knie_wi
Servo(2) = 63200 - Knieb
'-------------- Hüfte ---------------------
If Huft_wi < 55 Then Huft_wi = 55
If Huft_wi > 180 Then Huft_wi = 180
Hufta = 6400 / 180
Huftb = Hufta * Huft_wi
Servo(1) = 56800 + Huftb
Print "----------------------------------"
Print
Print "Knie_zw_a " ; Knie_zw_a
Print "Knie_zw_b " ; Knie_zw_b
Print
Print "Knie_zw_c " ; Knie_zw_c
Print "Knie_zw_d " ; Knie_zw_d
Print
Print "Knie_zw_e " ; Knie_zw_e
Print "Knie_zw_wi " ; Knie_zw_wi
Print
Print "Knie_wi " ; Knie_wi
Print
End Sub
End
das ist der vollständige code!
wenn nohc verbessungsvorschläge da sind imemr her damit. fange erst an mit der programmierung. vorherige Projekte waren kleiner. nen glcd mit touchscreen angesteuert. und kleinere Sensoren ausgewertet.
dies ist bislang mein erstes richtiges projekt.
den mein traum ist es iwann mal ein hexabot zu bauen ;-D ich weis das wollen viele aber iwann wil ich das verwirklichen und da mich die IK sehr interessiert und reizt wollt ihc mal erste versuche machen.
vohopri weis was ich meine. den er/du machst es ja auch so. viele kleinere projekt die zum schluss zusammengesetzt werden
(dein auton. fahrzeug fand bzw. find ich genial)
mfg bammel
Lesezeichen