//------------------------------------------------------
bool calc_q1q2q3(Vector3 desired_effector_position)
{
double L1;
double d2;
double d3;
double d4;
double r4;
double Lpince;
double Px;
double Py;
double Pz;
double delta;
double cdelta;
double sdelta;
double q1;
double q1prime;
double c1;
double s1;
double X;
double Z;
double Y;
double c3delta;
double s3delta;
double q3;
double A;
double B;
double s2;
double c2;
double q2;
double last_q3;
double last_q2;
double last_q1;
string aux;
L1 = 0.350;
d2 = 0.150;
d3 = 0.250;
d4 = 0.075;
r4 = 0.290;
Lpince = 0.27 + 0.04;
Px = desired_effector_position.x;
Py = desired_effector_position.z;
Pz = desired_effector_position.y;
if (DEBUG == true)
{
aux = "Opince désiré = [" + Px.ToString("0.0000") + "," + Pz.ToString("0.0000") + "," + (-Py).ToString("0.0000") + "] dans le repère lié à O0";
Debug.Log(aux);
}
delta = System.Math.Atan(r4 / d4);
cdelta = System.Math.Cos(delta);
sdelta = System.Math.Sin(delta);
q1 = System.Math.Atan(Py / Px);
q1prime = q1 + System.Math.PI;
last_q1 = last_q1q2q3.x;
if (q1 - last_q1 > System.Math.PI / 2)
{
q1 = q1 - System.Math.PI;
}
if (q1 - last_q1 < -System.Math.PI / 2)
{
q1 = q1 + System.Math.PI;
}
c1 = System.Math.Cos(q1);
s1 = System.Math.Sin(q1);
X = Px / c1 - d2;
Z = Pz - L1 + Lpince;
Y = d4 / cdelta;
c3delta = (Z * Z + X * X - Y * Y - d3 * d3) / (2 * d3 * Y);
if (c3delta > 1.0F)
{
if (DEBUG == true)
{
Debug.Log("c3delta=" + c3delta.ToString() + " > 1");
}
num_axe_butee = -1;
return(false);
}
s3delta = System.Math.Sqrt(1.0F - c3delta * c3delta);
q3 = -System.Math.Atan(s3delta / c3delta) + delta;
if (DEBUG == true)
{
Debug.Log("q3 pré: " + q3.ToString());
}
last_q3 = last_q1q2q3.z;
if (DEBUG == true)
{
Debug.Log("last_q3: " + last_q3.ToString());
}
if (q3 - last_q3 > System.Math.PI / 2)
{
q3 = q3 - System.Math.PI;
}
if (q3 - last_q3 < -System.Math.PI / 2)
{
q3 = q3 + System.Math.PI;
}
if (DEBUG == true)
{
Debug.Log("q3 post: " + q3.ToString());
}
A = Y * c3delta + d3;
B = -Y * s3delta;
s2 = (A * X + B * Z) / (A * A + B * B);
c2 = (A * Z - B * X) / (A * A + B * B);
q2 = System.Math.Atan((A * X + B * Z) / (A * Z - B * X));
last_q2 = last_q1q2q3.y;
if (q2 - last_q2 > System.Math.PI / 2)
{
q2 = q2 - System.Math.PI;
}
if (q2 - last_q2 < -System.Math.PI / 2)
{
q2 = q2 + System.Math.PI;
}
desired_q1q2q3 = new Vector3((float)q1, (float)q2, (float)q3);
if (DEBUG == true)
{
aux = "q1,q2,q3 = [" + q1.ToString("0.0000") + "," + q2.ToString("0.0000") + "," + q3.ToString("0.0000") + "]";
Debug.Log(aux);
}
return(true);
}
private void cmdWZ_Click(object sender, EventArgs e)
{
Pz pz = new Pz(Pz.TYPE.WZ);
pz.ShowDialog();
if (pz.CANCEL) return;
ProcessTowar(pz);
}
private void ProcessTowar(Pz pz)
{
int newRowID = CustomerDatabase.NewPzWz(USER.User.ID, pz.CID, pz.MID, pz.TYP_DOKUMENTU, pz.DATA, pz.SUMA);
if (newRowID < 0) { Message.ErrorMessage("Błąd dodawania danych do bazy"); return; }
CustomerDatabase.NewPzWzItems(newRowID, pz.PRODUKTY.ToArray());
if (pz.TYP_DOKUMENTU == 0) CustomerDatabase.NewTowarToMagazyn(pz.PRODUKTY);
else if (pz.TYP_DOKUMENTU == 1) CustomerDatabase.RemoveTowarFromMagazyn(pz.PRODUKTY);
ProcessResult(ErrorCode.CODE.SUCCES, pz.TYP_DOKUMENTU);
//else if (pz.TYPE == 2)
//{
// int newRowID = CustomerDatabase.NewPzWz(USER.User.ID, pz.MIDII, pz.MID, pz.TYPE, pz.DATA, pz.SUMA);
// if (newRowID < 0) { Message.ErrorMessage("Błąd dodawania danych do bazy"); return; }
// CustomerDatabase.NewPzWzItems(newRowID, pz.PRODUKTY.ToArray());
// CustomerDatabase.RemoveTowarFromMagazyn(pz.PRODUKTY);
// for (int i = 0; i < pz.PRODUKTY.Count; i++)
// { pz.PRODUKTY[i].MID = pz.MIDII; }
// CustomerDatabase.NewTowarToMagazyn(pz.PRODUKTY);
// ProcessResult(ErrorCode.CODE.SUCCES, pz.TYPE);
//}
}
private void cmdPodgladPzWz_Click(object sender, EventArgs e)
{
if (Tryb != TRYB.PZWZ) { Message.InfoMessage("Proszę wyszukać dokument PZ lub WZ"); return; }
Produkt selectedPzWz;
try { selectedPzWz = (Produkt)dgwMagazyn.CurrentRow.DataBoundItem; }
catch { Message.Position(); return; }
Pz viewPzWz = new Pz(Pz.TYPE.VIEW);
viewPzWz.SELECTED = selectedPzWz;
viewPzWz.ShowDialog();
}
