//------------------------------------------------------ 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(); }