habs nochmal genau durchgeschaut bei folgenden daten geht das bein hoch:
Hoehe = 50
Abstand = 97
bei abstand = 98 bleibt es noch unten und verhält sich so wie es soll
code ist folgender:
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.2 = 0 'hier hängt servo1
Portc.3 = 0 'hier hängt servo2
Portc.4 = 0 'hier hängt servo3
Portc.5 = 0 'hier hängt servo4
Config Pinc.0 = Input ' Port C.0 Ausgang
Config Pinc.1 = Input
On Timer1 Servoirq 'servo
Config Adc = Single , Prescaler = 64 , Reference = Avcc
Start Adc
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 Wert1_a As Single
Dim Wert1_b As Word
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 Hufta_zw_a As Single
Dim Hufta_zw_b As Single
Dim Hufta_wi As Byte
Dim Huftb_zw_a As Word
Dim Huftb_zw_b As Word
Dim Huftb_zw_c As Word
Dim Huftb_zw_d As Integer
Dim Huftb_zw_e As Single
Dim Huftb_zw_wi As Single
Dim Huftb_wi As Byte
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
Wert1_b = Getadc(0)
Waitus 50
Wert1_a = 150 / 1024
Abstand = Wert1_a * Wert1_b
Call Berechnung
Loop
Servoirq:
If Kanal = 0 Then
If Portc.2 = 0 Then 'wenn port low
Timer1 = Servo(1) 'dann timer auf entsprechende verzögerung
Portc.2 = 1 'und port anschalten
Else 'das hier passiert erst bei dem darauf folgenden interrupt
Portc.2 = 0 'dann port wieder ausschalten
Incr Kanal 'und den nächsten kanal bearbeiten
End If
End If
If Kanal = 1 Then
If Portc.3 = 0 Then
Timer1 = Servo(2)
Portc.3 = 1
Else
Portc.3 = 0
Incr Kanal
End If
End If
If Kanal = 2 Then
If Portc.4 = 0 Then
Timer1 = Servo(3)
Portc.4 = 1
Else
Portc.4 = 0
Incr Kanal
End If
End If
If Kanal = 3 Then
If Portc.5 = 0 Then
Timer1 = Servo(4)
Portc.5 = 1
Else
Portc.5 = 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 = Acos(knie_zw_e)
Knie_wi = Rad2deg(knie_zw_wi)
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 a ---------------------
Hufta_zw_a = Hoehe / Dis
Hufta_zw_b = Acos(hufta_zw_a)
Hufta_wi = Rad2deg(hufta_zw_b)
'-------------- Hüfte b --------------------- alpha = acos((ober² + dis² - unter²)/(2·ober·dis))
'alpha = acos((ober² - unter² - dis²) / (-2 * ober * dis))
Huftb_zw_a = Oberschenkel_q + Dis_q
Huftb_zw_b = Huftb_zw_a - Unterschenkel_q
Huftb_zw_c = Oberschenkel * Dis
Huftb_zw_d = 2 * Huftb_zw_c
Huftb_zw_e = Huftb_zw_b / Huftb_zw_d
Huftb_zw_wi = Acos(huftb_zw_e)
Huftb_wi = Rad2deg(huftb_zw_wi)
Huft_wi = Hufta_wi + Huftb_wi
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_wi " ; Knie_wi
Print
Print "Abstand " ; Abstand
Print "adc " ; Wert1_b
Print "huft_wi " ; Huft_wi
Print
End Sub
End
EDIT: hab gerade noch ein poti für die höhe eingebaut. und ich muss feststellen sobald der kniewinkel kleiner als 90° werden würde geht das bein in fehlstellung. aber ich glauber nur der unterschenkel. dieser stellt sich dann immer in den 180° winkel
mfg bammel
Zitieren
Lesezeichen