Kezdjük a legtetején.
Van egy osztályunk, aminek az a neve, hogy PokTest. Ebben vannak tárolva a középre kirajzolt póknak a koordinátái, valamint ebbe van összefogva a hat pókláb. Lehet ugyanide lesz betéve még a póknak a feje is, ami szintén egy lábból lesz példányosítva. Legfontosabb metódusa a test kirajzolása, valamint a lábak végpontjainak újraszámolása.
Eggyel lennebbi szint a Láb. Ez csak pár sor, viszont szükséges volt azért, hogy a két különböző nézetet (fentről és oldalról) össze tudjam fogni egy helyre.
public Lab(Nezet oldal, Nezet fent)
{
oldalrol = oldal;
felulrol = fent;
}
Ez a Lábnak a konstruktora. Két nézetet kell megadni. Egy oldalnézetet és egy felülnézetet.
Mégeggyel lennebb van a nézet, mint osztály. A nézetnek már több tulajdonsága illetve több metódusa van.
public int lfx, lfy;//lab-felso-x, lab-felso-y
public int lkx, lky;//lab-kozepso
public int lax, lay;//lab-also
public static int L1 = 50;
public static int L2 = 75;
public double teta1, teta2, teta3;//szervok szogei
public double teta1szog, teta2szog, teta3szog;
public double eltolas1, eltolas2, eltolas3; // szervok pozicionalasabol adodik
public double alfa;//az a szog, amit a vegpont es kezdopont osszekotese a kezdopont sikjaval bezar.
public int rajzx, rajzy;
public TextBox t1, t2, t3, t4, t5;
public bool baloldali;
public Nezet parja;
public Graphics testRajz;
private Pen myPen, whitePen, blackPen;
Ezek azok az adattagok, amiket itt inicializálni kell (magyar kifejezéssel élve kezdőértéket kell adni nekik). A változónevek számomra jelentenek valamit, más számára nem hiszem. Például lkx nálam a láb középső forgójának X koordinátáját jelenti. Ugyanígy a többi. Teta1 az a szög, amit a felkar a vállal bezár (vagyis amit a robotkar felső része a fix pont által meghatározott síkkal bezár). Teta2 az alkar és felkar által bezárt szögek. Teta3 pedig a kinyujtott karunk arcunkkal bezárt szöge(sajnos ennél jobban nem tudom elmagyarázni itt...) Minderre azért van szükségem, mert ezek az értékek szervókkal lesznek beállítva és a szervóknak ki kell küldeni valamilyen vezérlőjelet. Erre majd később visszatérünk.
Teta1 és teta2 az oldalnézeten számítódik ki, teta3 pedig a felülnézeten.
public void Kiszamol_OldalNezet()
{
if (baloldali)
{
Tukroz(rajzx);
Transzponal(0, rajzy); //eltoljuk a kart az origoba.
alfa = Math.Atan(lay / (double)lax);
teta1 = alfa - Math.Acos((double)(lax * lax + lay * lay + L1 * L1 - L2 * L2) / (double)(2 * L1 * Math.Sqrt(lax * lax + lay * lay)));
teta2 = (double)Math.PI - Math.Acos((double)(L1 * L1 + L2 * L2 - lax * lax - lay * lay) / (double)(2 * L1 * L2));
teta1szog = teta1 * 180 / (double)Math.PI;
teta2szog = (teta2 + eltolas2) * 180 / (double)Math.PI;
try
{
t1.Text = teta1szog.ToString();
t2.Text = teta2szog.ToString();
t4.Text = lax.ToString();
t5.Text = lay.ToString();
}
catch
{
//
}
lkx = lfx + (int)(Math.Cos(teta1+eltolas1) * L1);
lky = lfy + (int)(Math.Sin(teta1+eltolas1) * L1);
parja.lkx = parja.lfx + (int)(Math.Cos(parja.teta3+parja.eltolas3) * lkx);
parja.lky = parja.lfy + (int)(Math.Sin(parja.teta3+parja.eltolas3) * lkx);
parja.lax = parja.lfx + (int)(Math.Cos(parja.teta3+parja.eltolas3) * lax);
parja.lay = parja.lfy + (int)(Math.Sin(parja.teta3+parja.eltolas3) * lax);
Transzponal(0, -rajzy);
VisszaTukroz(rajzx);
}
else
{
Transzponal(rajzx, rajzy); //eltoljuk a kart az origoba.
alfa = Math.Atan(lay / (double)lax);
teta1 = alfa - Math.Acos((double)(lax * lax + lay * lay + L1 * L1 - L2 * L2) / (double)(2 * L1 * Math.Sqrt(lax * lax + lay * lay)));
teta2 = (double)Math.PI - Math.Acos((double)(L1 * L1 + L2 * L2 - lax * lax - lay * lay) / (double)(2 * L1 * L2));
teta1szog = teta1 * 180 / (double)Math.PI;
teta2szog = (teta2 + eltolas2) * 180 / (double)Math.PI;
try
{
t1.Text = teta1szog.ToString();
t2.Text = teta2szog.ToString();
t4.Text = lax.ToString();
t5.Text = lay.ToString();
}
catch
{
//
}
lkx = lfx + (int)(Math.Cos(teta1) * L1);
lky = lfy + (int)(Math.Sin(teta1) * L1);
parja.lkx = parja.lfx + (int)(Math.Cos(parja.teta3) * lkx);
parja.lky = parja.lfy + (int)(Math.Sin(parja.teta3) * lkx);
parja.lax = parja.lfx + (int)(Math.Cos(parja.teta3) * lax);
parja.lay = parja.lfy + (int)(Math.Sin(parja.teta3) * lax);
Transzponal(-rajzx, -rajzy);
}
}
Legelőször megvizsgáljuk, hogy bal vagy jobb lábról van-e szó, mivel a kettőt különbözőképpen kell számolni. A baloldalit tükrözni meg transzponálni, hogy a pozitív irányba nézzen, illetve, hogy a kezdőpont a (0,0) legyen. Aztán ki kell számolni pár szöget. Ezekről volt már szó korábban. A szögeket radiánból fokba alakítani, majd kiszámolni a szögek függvényében az új koordinátákat, nem csak a rajzon belül, hanem a neki megfelelő másik nézetben is az új érték után igazítani a paramétereket. Erről a részről szólnak a parja.lax, illetve parja.lay kezdetű sorok. Ezután következik a visszaalakítás rajznak megfelelő koordinátarendszerbe, illetve a kirajzolás. Persze az valahol máshol van megírva.
Ennél a Nézet osztálynál lennebb már csak a Pont osztály van, amit franc tudja, hogy minek írtam meg, mert nem használom semmire. Lagalább nem töltöttem vele sok időt, amig elkészült.
Ezen kívül ott van a main() valamint a grafikus felület megtervezése. A mainhez sokmindent nem fűznék hozzá, ott inkább a már megírt függvényeket hívogattam meg paramétereztem. A grafikus felület meg csak gomb-behuzogatás.
S ha mindent jól csináltunk, a következő eredményhez jutunk :
Az X és Y értékek frissítése még nem az igazi, viszont a programot én közel befejezettnek tekintem. Talán dobok még rá egy soros portot, s összekötöm direktben a FPGA-val. Persze csak majd robot-tesztelési fázisban. De addig még van egy kicsi.
0 megjegyzés:
Megjegyzés küldése