Code:
CON
_CLKMODE = XTAL1 + PLL16X 'Set to ext crystal, 16x PLL, 80MHz Clock
_XINFREQ = 5_000_000 'Frequency on XIN pin is 5 MHz
l_fuss = 138.0 'Länge vom Fuss (b)
b_2 = 19044.0 ' (l_fuss)²
l_schenk = 110.0 'Länge vom Oberschenkel
a_2 = 12100.0 ' (l_schenk)²
aa_p_bb = 31144.0 'a²+ b² !!! mit xxx.0 angeben
aa_m_bb = -6944.0 'a²- b² !!! mit xxx.0 angeben
ax2 = 220.0 '2a !!! mit xxx.0 angeben
abx2 = 30360.0 '2ab !!! mit xxx.0 angeben
'Globale Werte für HEXA - können noch dynamisch gemacht werden
x_off = 101.0
'Ri_max = 100.0
'Steps = 11 -7>steps_all>7
ON = 1
OFF = 0
OBJ
math : "Float32Full"
Var
'-----------Servo-Puls-Erzeugung----------------------
LONG new_peri_r,PinStart_r,pin_r,high_pulse_r[9] 'Reihenfolge nicht ändern da Pointer in Move-Servo darauf zu greifen
LONG new_peri_l,PinStart_l,pin_l,high_pulse_l[9] 'DO NOT TOUCH
LONG periode
LONG stack_setpulse[25]
LONG speed
LONG Apos[3], Aini[3], Alast[3], Aziel[3], Asign[3]
LONG Bpos[3], Bini[3], Blast[3], Bziel[3], Bsign[3]
LONG Cpos[3], Cini[3], Clast[3], Cziel[3], Csign[3]
LONG Dpos[3], Dini[3], Dlast[3], Dziel[3], Dsign[3]
LONG Epos[3], Eini[3], Elast[3], Eziel[3], Esign[3]
LONG Fpos[3], Fini[3], Flast[3], Fziel[3], Fsign[3]
LONG xA_trans, xB_trans, xC_trans, xD_trans, xE_trans, xF_trans
LONG yA_trans, yB_trans, yC_trans, yD_trans, yE_trans, yF_trans
LONG xA_rota, xB_rota, xC_rota, xD_rota, xE_rota, xF_rota
LONG yA_rota, yB_rota, yC_rota, yD_rota, yE_rota, yF_rota
LONG a_pos, b_pos, c_pos, d_pos, e_pos, f_pos
LONG p_a, p_b, p_c, p_d, p_e, p_f
' Geometrische Variablen
LONG hA, hB, hC, hD, hE, hF
LONG xA, xB, xC, xD, xE, xF
LONG yA, yB, yC, yD, yE, yF
LONG lA, lB, lC, lD, lE, lF, l_max
LONG ph_A, ph_B, ph_C, ph_D, ph_E, ph_F
LONG RiA, RiB, RiC, RiD, RiE, RiF, Ri_min
LONG i_A, i_B, i_C, i_D, i_E, i_F
LONG Ri_max
LONG wQ, wL, hBasis, wx, wy
BYTE A_back, B_back, C_back, D_back, E_back, F_back
BYTE c_a, c_b, c_c, c_d, c_e, c_f
pub main
init
speed := 3000
'-------------------------------------------------------------------------------------------------------------------------
'### INITIALPOSITION ANFAHREN
coginit(6,@moveServo,@new_peri_r)
coginit(7,@moveServo,@new_peri_l) ' Anfahren der Initalposition
coginit(5,setpulse,@stack_setpulse)
repeat
busy_led ' Bestätigen, dass bereit
if ina[6] == 1
waitcnt(2_000_000 + cnt)
outa[26]~
outa[27]~~
quit
'-------------------------------------------------------------------------------------------------------------------------
hBasis := 193.0
wQ := 0.0
wL := 0.0
wx := 0.0
wy := 0.0
xA := x_off
yA := 0.0
hA := 0.0
xB := x_off
yB := 0.0
hB := 0.0
xC := x_off
yC := 0.0
hC := 0.0
xD := x_off
yD := 0.0
hD := 0.0
xE := x_off
yE := 0.0
hE := 0.0
xF := x_off
yF := 0.0
hF := 0.0
IK
'-------------------------------------------------------------------------------------------------------------------------
repeat
busy_led ' Bestätigen, dass bereit
if ina[6] == 1
waitcnt(2_000_000 + cnt)
outa[26] := OFF
quit
hBasis := 193.0
wQ := 0
wL := 0
wx := 0
wy := 0
Richtung(0.0, 0.0, -350.0, 0.0, 45.0, 1.0) 'Richtung(y, x,w_y, v_x, teta, speed)
'Vorgabe
repeat
outa[27] := OFF
waitcnt(10_000_000 + cnt)
outa[27] := ON
waitcnt(10_000_000 + cnt)
repeat
outa[27] := ON
' ######################################################################################################
' ##################### EBENE 2 #######################################################################
' ######################################################################################################
PUB Richtung(y,x,w,v,teta,speed_2)|alpha_trans,distance,steps_trans,steps_rota,steps_all, step_counter,Xi,sign ' v,w,
alpha_trans := math.Atan2(y,x) 'y,x in Rad
if (y== 0.0 AND x < 0.0)
alpha_trans :=math.Radians(180.0)
if(x==0.0)AND(y==0.0)
distance := 0.0
else
distance := math.FSqr(math.FAdd(math.pow(x, 2.0), math.Pow(y,2.0)))
if ((alpha_trans => -1.0471 AND alpha_trans =< -2.0943) OR (alpha_trans => 1.0471 AND alpha_trans =< 2.093)) AND(teta==0.0)
Ri_Max := 150.0
elseif (teta == 0.0)
Ri_Max := 90.0
else
Ri_Max := 90.0
lA := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FSub(math.Fneg(x_off),110.0), v),2.0), math.Pow(math.FSub(-115.0,w),2.0)))
lB := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FSub(math.Fneg(x_off),110.0), v),2.0), math.Pow(w, 2.0)))
lC := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FSub(math.Fneg(x_off),110.0), v),2.0), math.Pow(math.FSub(115.0,w), 2.0)))
lD := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FAdd(x_off, 110.0), v),2.0), math.Pow(math.FSub(115.0,w), 2.0)))
lE := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FAdd(x_off, 110.0), v),2.0), math.Pow(w, 2.0)))
lF := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FAdd(x_off, 110.0), v),2.0), math.Pow(math.FSub(-115.0,w),2.0)))
l_max := math.FMax(lA, lB)
l_max := math.FMax(l_max, lC)
l_max := math.FMax(l_max, lD)
l_max := math.FMax(l_max, lE)
l_max := math.FMax(l_max, lF)
Xi := math.Degrees(math.ATan(math.FDiv(Ri_max,math.FMul(2.0,l_max))))
if (teta == 0.0)
steps_rota := math.FRound(0.0)
Ri_min := Ri_max
i_A := math.FDiv(Ri_Max,14.0)
i_B := i_A
i_C := i_A
i_D := i_A
i_E := i_A
i_F := i_A
else
steps_rota := math.FRound(math.FAbs(math.FDiv(teta, Xi)))
steps_rota++
RiA := math.FMul(lA,math.FMul(math.tan(math.Radians(Xi)),2.0))
RiB := math.FMul(lB,math.FMul(math.tan(math.Radians(Xi)),2.0))
RiC := math.FMul(lC,math.FMul(math.tan(math.Radians(Xi)),2.0))
RiD := math.FMul(lD,math.FMul(math.tan(math.Radians(Xi)),2.0))
RiE := math.FMul(lE,math.FMul(math.tan(math.Radians(Xi)),2.0))
RiF := math.FMul(lF,math.FMul(math.tan(math.Radians(Xi)),2.0))
Ri_min :=math.FMin(RiA,RiB)
Ri_min :=math.FMin(Ri_min,RiC)
Ri_min :=math.FMin(Ri_min,RiD)
Ri_min :=math.FMin(Ri_min,RiE)
Ri_min :=math.FMin(Ri_min,RiF)
i_A := math.FDiv(RiA,14.0)
i_B := math.FDiv(RiB,14.0)
i_C := math.FDiv(RiC,14.0)
i_D := math.FDiv(RiD,14.0)
i_E := math.FDiv(RiE,14.0)
i_F := math.FDiv(RiF,14.0)
if(teta > 0.0)
sign := 1.0
else
sign := -1.0
if(distance == 0.0)
steps_trans := math.FRound(0.0)
else
steps_trans := math.FRound(math.FDiv(distance, Ri_min))
steps_trans++
ph_A := math.FNeg(math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(math.FAdd(w,115.0),lA)))))
ph_B := math.FNeg(math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(w,lB)))))
ph_C := math.FNeg(math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(math.FSub(w,115.0),lC)))))
ph_D := math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(math.FSub(w,115.0),lD))))
ph_E := math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(w,lE))))
ph_F := math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(math.FAdd(w,115.0),lF))))
case speed_2
1.0:
a_pos := -2.0
b_pos := -4.0
c_pos := 8.0
d_pos := -1.0
e_pos := 5.0
f_pos := -7.0
1.75:
a_pos := 8.25
b_pos := 1.75
c_pos := -5.25
d_pos := 1.75
e_pos := 8.75
f_pos := -5.25
2.5:
a_pos := 10.5
b_pos := -3.5
c_pos := 10.5
d_pos := -3.5
e_pos := 10.5
f_pos := -3.5
if (steps_trans => steps_rota)
steps_all := steps_trans
else
steps_all := steps_rota
c_a := 0
c_b := 0
c_c := 0
c_d := 0
c_e := 0
c_f := 0
p_a := 0
p_b := 0
p_c := 0
p_d := 0
p_e := 0
p_f := 0
Step_counter := 0
repeat until(step_counter == steps_all)
if (step_counter == steps_rota)
teta := 0.0
if (step_counter == steps_trans)
distance := 0.0
if (a_pos > 7.0)
A_back := 1
case c_a
0:
p_a := 7.0
hA := -50.0
c_a++
1:
p_a := 0.0
hA := -50.0
c_a++
2:
p_a := -7.0
hA := -50.0
c_a++
3:
p_a := -7.0
hA := 0.0
a_pos := -7.0
c_a := 0
else
A_back := 0
p_a := a_pos
hA := 0.0
if (b_pos > 7.0)
B_back := 1
case c_b
0:
p_b := 7.0
hb := -50.0
c_b++
1:
p_b := 0.0
hb := -50.0
c_b++
2:
p_B := -7.0
hB := -50.0
c_b++
3:
p_b := -7.0
hB := 0.0
b_pos := -7.0
c_b := 0
else
B_back := 0
p_b := b_pos
hB := 0.0
if (c_pos > 7.0)
C_back := 1
case c_c
0:
p_c := 7.0
hC := -50.0
c_c++
1:
p_c := 0.0
hC := -50.0
c_c++
2:
p_C := -7.0
hC := -50.0
c_c++
3:
p_c := -7.0
hC := 0.0
C_pos := -7.0
c_C := 0
else
C_back := 0
p_c := c_pos
hC := 0.0
if (d_pos > 7.0)
D_back := 1
case c_d
0:
p_d := 7.0
hD := -50.0
c_d++
1:
p_d := 0.0
hD := -50.0 '150
c_d++
2:
p_D := -7.0
hD := -50.0
c_D++
3:
p_d := -7.0
hD := 0.0
D_pos := -7.0
c_D := 0
else
D_back := 0
p_d := d_pos
hD := 0.0
if (e_pos > 7.0)
E_back := 1
case c_e
0:
p_e := 7.0
hE := -50.0
c_e++
1:
p_e := 0.0
hE := -50.0
c_e++
2:
p_E := -7.0
hE := -50.0
c_e++
3:
p_e := -7.0
hE := 0.0
e_pos := -7.0
c_e := 0
else
E_back := 0
p_e := e_pos
hE := 0.0
if (f_pos > 7.0)
F_back := 1
case c_f
0:
p_f := 7.0
hF := -50.0
c_f++
1:
p_f := 0.0
hF := -50.0
c_f++
2:
p_F := -7.0
hF := -50.0
c_F++
3:
p_F := -7.0
hF := 0.0
F_pos := -7.0
c_f := 0
else
F_back := 0
p_f := f_pos
hF := 0.0
if (distance == 0.0)
xA := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_A),math.cos(math.Radians(ph_A))),math.FMul(i_A,sign)))
xB := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_B),math.cos(math.Radians(ph_B))),math.FMul(i_B,sign)))
xC := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_C),math.cos(math.Radians(ph_C))),math.FMul(i_C,sign)))
xD := math.FSub(x_off,math.FMul(math.FMul(p_D ,math.cos(math.Radians(ph_D))),math.FMul(i_D,sign)))
xE := math.FSub(x_off,math.FMul(math.FMul(p_E ,math.cos(math.Radians(ph_E))),math.FMul(i_E,sign)))
xF := math.FSub(x_off,math.FMul(math.FMul(p_F ,math.cos(math.Radians(ph_F))),math.FMul(i_F,sign)))
yA := math.FMul(math.FMul(math.FNeg(p_A),math.sin(math.Radians(ph_A)) ),math.FMul(i_A,sign))
yB := math.FMul(math.FMul(math.FNeg(p_B),math.sin(math.Radians(ph_B)) ),math.FMul(i_B,sign))
yC := math.FMul(math.FMul(math.FNeg(p_C),math.sin(math.Radians(ph_C)) ),math.FMul(i_C,sign))
yD := math.FMul(math.FMul(math.FNeg(p_D),math.sin(math.Radians(ph_D)) ),math.FMul(i_D,sign))
yE := math.FMul(math.FMul(math.FNeg(p_E),math.sin(math.Radians(ph_E)) ),math.FMul(i_E,sign))
yF := math.FMul(math.FMul(math.FNeg(p_F),math.sin(math.Radians(ph_F)) ),math.FMul(i_F,sign))
else
if (teta == 0.0)
xA := math.FSub(x_off,math.FMul(math.FNeg(p_A),math.FMul(i_A,math.cos(alpha_trans))))
xB := math.FSub(x_off,math.FMul(math.FNeg(p_B),math.FMul(i_B,math.cos(alpha_trans))))
xC := math.FSub(x_off,math.FMul(math.FNeg(p_C),math.FMul(i_C,math.cos(alpha_trans))))
xD := math.FSub(x_off,math.FMul(p_D,math.FMul(i_D,math.cos(alpha_trans))))
xE := math.FSub(x_off,math.FMul(p_E,math.FMul(i_E,math.cos(alpha_trans))))
xF := math.FSub(x_off,math.FMul(p_F,math.FMul(i_F,math.cos(alpha_trans))))
yA := math.FMul(math.FNeg(p_A),math.FMul(i_A,math.Sin(alpha_trans)))
yB := math.FMul(math.FNeg(p_B),math.FMul(i_B,math.Sin(alpha_trans)))
yC := math.FMul(math.FNeg(p_C),math.FMul(i_C,math.Sin(alpha_trans)))
yD := math.FMul(math.FNeg(p_D),math.FMul(i_D,math.Sin(alpha_trans)))
yE := math.FMul(math.FNeg(p_E),math.FMul(i_E,math.Sin(alpha_trans)))
yF := math.FMul(math.FNeg(p_F),math.FMul(i_F,math.Sin(alpha_trans)))
else
xA_rota := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_A),math.cos(math.Radians(ph_A))),math.FMul(i_A,sign)))
xB_rota := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_B),math.cos(math.Radians(ph_B))),math.FMul(i_B,sign)))
xC_rota := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_C),math.cos(math.Radians(ph_C))),math.FMul(i_C,sign)))
xD_rota := math.FSub(x_off,math.FMul(math.FMul(p_D ,math.cos(math.Radians(ph_D))),math.FMul(i_D,sign)))
xE_rota := math.FSub(x_off,math.FMul(math.FMul(p_E ,math.cos(math.Radians(ph_E))),math.FMul(i_E,sign)))
xF_rota := math.FSub(x_off,math.FMul(math.FMul(p_F ,math.cos(math.Radians(ph_F))),math.FMul(i_F,sign)))
yA_rota := math.FMul(math.FMul(math.FNeg(p_A),math.sin(math.Radians(ph_A)) ),math.FMul(i_A,sign))
yB_rota := math.FMul(math.FMul(math.FNeg(p_B),math.sin(math.Radians(ph_B)) ),math.FMul(i_B,sign))
yC_rota := math.FMul(math.FMul(math.FNeg(p_C),math.sin(math.Radians(ph_C)) ),math.FMul(i_C,sign))
yD_rota := math.FMul(math.FMul(math.FNeg(p_D),math.sin(math.Radians(ph_D)) ),math.FMul(i_D,sign))
yE_rota := math.FMul(math.FMul(math.FNeg(p_E),math.sin(math.Radians(ph_E)) ),math.FMul(i_E,sign))
yF_rota := math.FMul(math.FMul(math.FNeg(p_F),math.sin(math.Radians(ph_F)) ),math.FMul(i_F,sign))
xA_trans := math.FSub(x_off,math.FMul(math.FNeg(p_A),math.FMul(i_A,math.cos(alpha_trans))))
xB_trans := math.FSub(x_off,math.FMul(math.FNeg(p_B),math.FMul(i_B,math.cos(alpha_trans))))
xC_trans := math.FSub(x_off,math.FMul(math.FNeg(p_C),math.FMul(i_C,math.cos(alpha_trans))))
xD_trans := math.FSub(x_off,math.FMul(p_D,math.FMul(i_D,math.cos(alpha_trans))))
xE_trans := math.FSub(x_off,math.FMul(p_E,math.FMul(i_E,math.cos(alpha_trans))))
xF_trans := math.FSub(x_off,math.FMul(p_F,math.FMul(i_F,math.cos(alpha_trans))))
yA_trans := math.FMul(math.FNeg(p_A),math.FMul(i_A,math.Sin(alpha_trans)))
yB_trans := math.FMul(math.FNeg(p_B),math.FMul(i_B,math.Sin(alpha_trans)))
yC_trans := math.FMul(math.FNeg(p_C),math.FMul(i_C,math.Sin(alpha_trans)))
yD_trans := math.FMul(math.FNeg(p_D),math.FMul(i_D,math.Sin(alpha_trans)))
yE_trans := math.FMul(math.FNeg(p_E),math.FMul(i_E,math.Sin(alpha_trans)))
yF_trans := math.FMul(math.FNeg(p_F),math.FMul(i_F,math.Sin(alpha_trans)))
xC := math.FAdd(math.FDiv(math.FSub(xC_trans, xC_rota),2.0),xC_rota)
yC := math.FAdd(math.FDiv(math.FSub(yC_trans, yC_rota),2.0),yC_rota)
xD := math.FAdd(math.FDiv(math.FSub(xD_trans, xD_rota),2.0),xD_rota)
yD := math.FAdd(math.FDiv(math.FSub(yD_trans, yD_rota),2.0),yD_rota) }}
IK
a_pos := math.FAdd(a_pos, speed_2)
b_pos := math.FAdd(b_pos, speed_2)
c_pos := math.FAdd(c_pos, speed_2)
d_pos := math.FAdd(d_pos, speed_2)
e_pos := math.FAdd(e_pos, speed_2)
f_pos := math.FAdd(f_pos, speed_2)
' ######################################################################################################
' ##################### EBENE 1c #######################################################################
' ######################################################################################################
pub move|i
i:=0
repeat 3
if Aziel[i] > Alast[i] ' Servo-bewegung vorwärts
Asign[i] := +1
if Aziel[i] < Alast[i] ' Servo-bewegung rückwärts
Asign[i] := -1
if Bziel[i] > Blast[i] ' Servo-bewegung vorwärts
Bsign[i] := +1
if Bziel[i] < Blast[i] ' Servo-bewegung rückwärts
Bsign[i] := -1
if Cziel[i] > Clast[i] ' Servo-bewegung vorwärts
Csign[i] := +1
if Cziel[i] < Clast[i] ' Servo-bewegung rückwärts
Csign[i] := -1
if Dziel[i] > Dlast[i] ' Servo-bewegung vorwärts
Dsign[i] := +1
if Dziel[i] < Dlast[i] ' Servo-bewegung rückwärts
Dsign[i] := -1
if Eziel[i] > Elast[i] ' Servo-bewegung vorwärts
Esign[i] := +1
if Eziel[i] < Elast[i] ' Servo-bewegung rückwärts
Esign[i] := -1
if Fziel[i] > Flast[i] ' Servo-bewegung vorwärts
Fsign[i] := +1
if Fziel[i] < Flast[i] ' Servo-bewegung rückwärts
Fsign[i] := -1
if A_back == 1
ASign[i] := ASign[i]*5
if B_back == 1
BSign[i] := BSign[i]*5
if C_back == 1
CSign[i] := CSign[i]*5
if D_back == 1
DSign[i] := DSign[i]*5
if E_back == 1
ESign[i] := ESign[i]*5
if F_back == 1
FSign[i] := FSign[i]*5
i++
repeat ' Bewegung
i:= 0
repeat 3
Apos[i] += Asign[i] ' Positionsänderung
if ((Asign[i]>0.0) AND (Apos[i] > Aziel[i])) OR ((Asign[i]<0.0) AND (Apos[i] < Aziel[i])) ' Ziel erreicht
Asign[i]:=0
Bpos[i] += Bsign[i] ' Positionsänderung
if ((Bsign[i]>0.0) AND (Bpos[i] > Bziel[i])) OR ((Bsign[i]<0.0) AND (Bpos[i] < Bziel[i])) ' Ziel erreicht
Bsign[i]:=0
Cpos[i] += Csign[i] ' Positionsänderung
if ((Csign[i]>0.0) AND (Cpos[i] > Cziel[i])) OR ((Csign[i]<0.0) AND (Cpos[i] < Cziel[i])) ' Ziel erreicht
Csign[i]:=0
Dpos[i] += Dsign[i] '
if ((Dsign[i]>0.0) AND (Dpos[i] > Dziel[i])) OR ((Dsign[i]<0.0) AND (Dpos[i] < Dziel[i])) ' Ziel erreicht
Dsign[i]:=0
Epos[i] += Esign[i] ' Positionsänderung
if ((Esign[i]>0.0) AND (Epos[i] > Eziel[i])) OR ((Esign[i]<0.0) AND (Epos[i] < Eziel[i])) ' Ziel erreicht
Esign[i]:=0
Fpos[i] += Fsign[i] ' Positionsänderung
if ((Fsign[i]>0.0) AND (Fpos[i] > Fziel[i])) OR ((Fsign[i]<0.0) AND (Fpos[i] < Fziel[i])) ' Ziel erreicht
Fsign[i]:=0
i++
setpulse ' Servo einen Schritt bewegen
repeat speed ' Warten
until ((Asign[0] == 0) AND (Asign[1] == 0) AND (Asign[2] == 0)AND(Bsign[0] == 0) AND (Bsign[1] == 0) AND (Bsign[2] == 0)AND (Csign[0] == 0) AND (Csign[1] == 0) AND (Csign[2] == 0)AND(Dsign[0] == 0) AND (Dsign[1] == 0) AND (Dsign[2] == 0)AND(Esign[0] == 0) AND (Esign[1] == 0) AND (Esign[2] == 0)AND(Fsign[0] == 0) AND (Fsign[1] == 0) AND (Fsign[2] == 0)) ' Bis zum Ziel und Schluss
' ######################################################################################################
' ##################### EBENE 1a/1b ###################################################################
' ######################################################################################################
PUB IK|i, ca, ca_2, alpha, alpha_PP, alpha_P, gamma, phi, dhAF, dhBE, dhCD, dhABC, dhDEF, dQ, dL, rA, rB, rC, rD, rE, rF, hhA, hhB, hhC, hhD, hhE, hhF
'~~~~~ BERECHNUNG KIPPEN UND NEIGEN ~~~~~
dQ := math.Sin(math.Radians(wQ))
dL := math.Sin(math.Radians(wL))
'Prüfen ob -wy bzw. -wx korrekt ist)
dhCD := math.FMul(dQ, math.FSub(-230.0, -wy))
dhBE := math.FMul(dQ, math.FSub(0.0, -wy))
dhAF := math.FMul(dQ, math.FSub(+230.0, -wy))
dhABC := math.FMul(dL, math.FSub(-110.0, -wx))
dhDEF := math.FMul(dL, math.FSub(+110.0, -wx))
hhA := math.FAdd(hBasis, dhAF)
hhA := math.FAdd(hhA, dhABC)
hhA := math.FAdd(hhA, hA)
hhB := math.FAdd(hBasis, dhBE)
hhB := math.FAdd(hhB, dhABC)
hhB := math.FAdd(hhB, hB)
hhC := math.FAdd(hBasis, dhCD)
hhC := math.FAdd(hhC, dhABC)
hhC := math.FAdd(hhC, hC)
hhD := math.FAdd(hBasis, dhCD)
hhD := math.FAdd(hhD, dhDEF)
hhD := math.FAdd(hhD, hD)
hhE := math.FAdd(hBasis, dhBE)
hhE := math.FAdd(hhE, dhDEF)
hhE := math.FAdd(hhE, hE)
hhF := math.FAdd(hBasis, dhAF)
hhF := math.FAdd(hhF, dhDEF)
hhF := math.FAdd(hhF, hF)
rA := math.FSqr(math.FAdd(math.FMul(xA, xA), math.FMul(yA,yA)))
rB := math.FSqr(math.FAdd(math.FMul(xB, xB), math.FMul(yB,yB)))
rC := math.FSqr(math.FAdd(math.FMul(xC, xC), math.FMul(yC,yC)))
rD := math.FSqr(math.FAdd(math.FMul(xD, xD), math.FMul(yD,yD)))
rE := math.FSqr(math.FAdd(math.FMul(xE, xE), math.FMul(yE,yE)))
rF := math.FSqr(math.FAdd(math.FMul(xF, xF), math.FMul(yF,yF)))
'~~~~~ BERECHNUNG SERVOWINKEL ALPHA GAMMA PHI
'----- Länge von Bein A ----
ca_2 := math.FAdd(math.FMul(hhA, hhA), math.FMul(rA,rA))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte A0 ----
phi := math.Degrees(math.Atan2(xA, yA))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Aziel[0] := math.FRound(phi)
Aziel[0] := Aziel[0] +0
'----- Winkel Schulter A1 ----
alpha_P := math.ATan(math.FDiv(rA, hhA))
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Aziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Aziel[1] := Aziel[1] - 60
'----- Winkel Fuss A2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Aziel[2] := math.FSub(2910, math.FRound(gamma))
Aziel[2] := Aziel[2] +80
'---_- Länge von Bein B ----
ca_2 := math.FAdd(math.FMul(hhB, hhB), math.FMul(rB,rB))
ca := math.FSqr(ca_2)
'---_- Winkel von Hüfte B0 ----
phi := math.Degrees(math.Atan2(xB, yB))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Bziel[0] := math.FRound(phi)
Bziel[0] := Bziel[0] +40
'----- Winkel Schulter B1 ----
alpha_P := math.ATan(math.FDiv(rB, hhB) )
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Bziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Bziel[1] := Bziel[1] -40
'----- Winkel Fuss B2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Bziel[2] := math.FSub(2910, math.FRound(gamma))
Bziel[2] := Bziel[2] +100
'----- Länge von Bein C ----
ca_2 := math.FAdd(math.FMul(hhC, hhC), math.FMul(rC,rC))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte C0 ----
phi := math.Degrees(math.Atan2(xC, yC))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Cziel[0] := math.FRound(phi)
Cziel[0] := Cziel[0] +60
'----- Winkel Schulter C1 ----
alpha_P := math.ATan(math.FDiv(rC, hhC) )
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Cziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Cziel[1] := Cziel[1] +80
'----- Winkel Fuss C2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Cziel[2] := math.FSub(2910, math.FRound(gamma))
Cziel[2] := Cziel[2] +100
'----- Länge von Bein D ----
ca_2 := math.FAdd(math.FMul(hhD, hhD), math.FMul(rD,rD))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte D0 ----
phi := math.Degrees(math.Atan2(xD, yD))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Dziel[0] := math.FSub(2910, math.FRound(phi))
Dziel[0] := Dziel[0] +80
'----- Winkel Schulter D1 ----
alpha_P := math.ATan(math.FDiv(rD, hhD))
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Dziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Dziel[1] := math.FSub(2910, Dziel[1])
Dziel[1] := Dziel[1] +5
'----- Winkel Fuss D2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Dziel[2] := math.FRound(gamma)
Dziel[2] := Dziel[2] -80
'----- Länge von Bein E ----
ca_2 := math.FAdd(math.FMul(hhE, hhE), math.FMul(rE,rE))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte E0 ----
phi := math.Degrees(math.Atan2(xE, yE))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Eziel[0] := math.FSub(2910, math.FRound(phi))
Eziel[0] := Eziel[0] +70
'----- Winkel Schulter E1 ----
alpha_P := math.ATan(math.FDiv(rE, hhE))
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Eziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Eziel[1] := math.FSub(2910, Eziel[1])
Eziel[1] := Eziel[1] -20
'----- Winkel Fuss E2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Eziel[2] := math.FRound(gamma)
Eziel[2] := Eziel[2] +0
'----- Länge von Bein F ----
ca_2 := math.FAdd(math.FMul(hhF, hhF), math.FMul(rF,rF))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte F0 ----
phi := math.Degrees(math.Atan2(xF, yF))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Fziel[0] := math.FSub(2910, math.FRound(phi))
Fziel[0] := Fziel[0] +50
'----- Winkel Schulter F1 ----
alpha_P := math.ATan(math.FDiv(rF, hhF))
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Fziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Fziel[1] := math.FSub(2910, Fziel[1])
Fziel[1] := Fziel[1] -100
'----- Winkel Fuss F2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Fziel[2] := math.FRound(gamma)
Fziel[2] := Fziel[2] -0
move
i:=0
repeat 3
Alast[i] := Aziel[i]
Blast[i] := Bziel[i]
Clast[i] := Cziel[i]
Dlast[i] := Dziel[i]
Elast[i] := Eziel[i]
Flast[i] := Fziel[i]
i++
pub busy_led
!outa[26] ' Ausgang invertieren
waitcnt(3_000_000 + cnt) ' 300ms warten
pub ready_led
outa[26]~~ ' LED ON
waitcnt(100_000_000 + cnt)
outa[26]~
pub init|i
math.start
'------------------Feste-Parameter----------------------------------------------------------------------------------------
periode := ((clkfreq/1000000)*20000) 'Länge der gesamten Periode
PinStart_r:=|<16
PinStart_l:=|<7
dira[7..15]~~
pin_l:= dira
dira[16..24]~~
pin_r:= dira
dira[27]~~ 'Ausgang für LED - Gelb
dira[26]~~ 'Ausgang für LED - Grün
dira[25]~ 'Eingang Button
dira[6]~
'Initialposition
Aini[0] := 1490
Aini[1] := 1490
Aini[2] := 1490
Bini[0] := 1490
Bini[1] := 1490
Bini[2] := 1490
Cini[0] := 1490
Cini[1] := 1490
Cini[2] := 1490
Dini[0] := 1490
Dini[1] := 1490
Dini[2] := 1490
Eini[0] := 1490
Eini[1] := 1490
Eini[2] := 1490
Fini[0] := 1490
Fini[1] := 1490
Fini[2] := 1490
'Min, Max - Werte für Servobewegung und Positionen anfahren.
i:=0
repeat 3
Apos[i] #>= 809 ' Untere Servogrenze
Apos[i] <#= 2169
Alast[i] := Aini[i]
Apos[i] := Aini[i]
Bpos[i] #>= 809 ' Untere Servogrenze
Bpos[i] <#= 2169
Blast[i] := Bini[i]
Bpos[i] := Bini[i]
Cpos[i] #>= 809 ' Untere Servogrenze
Cpos[i] <#= 2169
Clast[i] := Cini[i]
Cpos[i] := Cini[i]
Dpos[i] #>= 809 ' Untere Servogrenze
Dpos[i] <#= 2169
Dlast[i] := Dini[i]
Dpos[i] := Dini[i]
Epos[i] #>= 809 ' Untere Servogrenze
Epos[i] <#= 2169
Elast[i] := Eini[i]
Epos[i] := Eini[i]
Fpos[i] #>= 809 ' Untere Servogrenze
Fpos[i] <#= 2169
Flast[i] := Fini[i]
Fpos[i] := Fini[i]
i++
setpulse
' ######################################################################################################
' ##################### EBENE 0 #######################################################################
' ######################################################################################################
pub setpulse|i,j 'Bewegen der Servos, berechnen Impulslänge
new_peri_r:= periode
new_peri_l:= periode
'-----------------LINKE SEITE-----------------------------------------------------
i:=0
j:=0
repeat 3
high_pulse_l[i] := ((clkfreq/ 1000000) * Fpos[j]) 'aktuelle pulselänge
new_peri_l-=high_pulse_l[i]
i+=1
j+=1
j:=0
repeat 3
high_pulse_l[i] := ((clkfreq/ 1000000) * Epos[j]) 'aktuelle pulselänge
new_peri_l-=high_pulse_l[i]
i+=1
j+=1
j:=0
repeat 3
high_pulse_l[i] := ((clkfreq/ 1000000) * Dpos[j]) 'aktuelle pulselänge
new_peri_l-=high_pulse_l[i]
i+=1
j+=1
'-----------------RECHTE SEITE-----------------------------------------------------
i:=0
j:=0
repeat 3
high_pulse_r[i] := ((clkfreq/ 1000000) * Cpos[j]) 'aktuelle pulselänge
new_peri_r-=high_pulse_r[i]
i+=1
j+=1
j:=0
repeat 3
high_pulse_r[i] := ((clkfreq/ 1000000) * Bpos[j]) 'aktuelle pulselänge
new_peri_r-=high_pulse_r[i]
i+=1
j+=1
j:=0
repeat 3
high_pulse_r[i] := ((clkfreq/ 1000000) * Apos[j]) 'aktuelle pulselänge
new_peri_r-=high_pulse_r[i]
i+=1
j+=1
DAT 'Ausgeben der Pulse
ORG 0
moveServo mov Index, par 'die adresse des cograms wird nach index geschreiben
Zitieren
Lesezeichen