private void lbRightPredefPos_MouseDoubleClick(object sender, MouseEventArgs e)
{
PredefPosition temp;
Vector tempPos;
this.armsMan.RightArm.TorqueOnOff(true);
for (int n = 0; n < this.armsMan.RightPredefPos.Count; n++)
{
if (!this.armsMan.RightPredefPos.ContainsKey("newPredefPos" + n))
{
temp = new PredefPosition("newPredefPos" + n, this.armsMan.RightArm);
tempPos = armsMan.RightArm.GetPositionArticular();
if (tempPos != null)
{
temp.Q1 = tempPos[0];
temp.Q2 = tempPos[1];
temp.Q3 = tempPos[2];
temp.Q4 = tempPos[3];
temp.Q5 = tempPos[4];
temp.Q6 = tempPos[5];
temp.Q7 = tempPos[6];
}
this.armsMan.RightPredefPos.Add(temp.Name, temp);
break;
}
}
btnSaveAll.PerformClick();
}
public bool SaveMovsAndPositions()
{
bool succes = true;
succes &= PredefPosition.SerializeToXml(this.leftPredefPos.Values.ToArray(), this.fileLeftPredefPos);
succes &= PredefPosition.SerializeToXml(this.rightPredefPos.Values.ToArray(), this.fileRightPredefPos);
succes &= PredefMovement.SerializeToXml(this.leftPredefMovs.Values.ToArray(), this.fileLeftMovements);
succes &= PredefMovement.SerializeToXml(this.rightPredefMovs.Values.ToArray(), this.fileRightMovements);
return(succes);
}
public bool LaGoToPredefPos(string position)
{
if (!this.leftPredefPos.ContainsKey(position))
{
TextBoxStreamWriter.DefaultLog.WriteLine("TaskPlanner: Can't find predefined position for left arm : \"" +
position + "\"");
}
PredefPosition gp = this.leftPredefPos[position];
return(this.LaGoToArticularPosition(gp.Q1, gp.Q2, gp.Q3, gp.Q4, gp.Q5, gp.Q6, gp.Q7));
}
public bool AddPredefinedPosition(string name)
{
PredefPosition pp;
if (this.rightPredefPos.ContainsKey(name))
{
return(false);
}
else
{
pp = new PredefPosition(name, this.rightArm);
this.rightPredefPos.Add(name, pp);
return(true);
}
}
private void newRightPredefPosition_Click(object sender, EventArgs e)
{
PredefPosition temp;
for (int n = 0; n < this.armsMan.RightPredefPos.Count; n++)
{
if (!this.armsMan.RightPredefPos.ContainsKey("newPredefPos" + n))
{
temp = new PredefPosition("newPredefPos" + n, this.armsMan.RightArm);
this.armsMan.RightPredefPos.Add(temp.Name, temp);
break;
}
}
btnSaveAll.PerformClick();
}
public bool ArmsGoToPredefPos(string position)
{
bool succes = false;
double maxError = 5 * Math.PI / 180; //5°
if (!this.leftPredefPos.ContainsKey(position))
{
TextBoxStreamWriter.DefaultLog.WriteLine("TaskPlanner: Can't find predefined position for left arm : \"" + position + "\"");
return(false);
}
if (!this.rightPredefPos.ContainsKey(position))
{
TextBoxStreamWriter.DefaultLog.WriteLine("TaskPlanner: Can't find predefined position for right arm: \"" + position + "\"");
return(false);
}
if (position == "home")
{
this.LaOpenGripper(10);
this.RaOpenGripper(10);
}
PredefPosition rp = this.rightPredefPos[position];
PredefPosition lp = this.leftPredefPos[position];
Vector rv;
Vector lv;
bool lSuccess = false;
bool rSuccess = false;
if (!armMan.status.IsLeftArmReady)
{
TextBoxStreamWriter.DefaultLog.WriteLine("TaskPlanner: LeftArm is NOT READY");
return(false);
}
if (!armMan.status.IsRightArmReady)
{
TextBoxStreamWriter.DefaultLog.WriteLine("TaskPlanner: RightARM is NOT READY");
return(false);
}
movingLeftArm = true;
movingRightArm = true;
this.leftArm.GoToArticularPositionNoAnswer(lp.Q1, lp.Q2, lp.Q3, lp.Q4, lp.Q5, lp.Q6, lp.Q7);
this.rightArm.GoToArticularPositionNoAnswer(rp.Q1, rp.Q2, rp.Q3, rp.Q4, rp.Q5, rp.Q6, rp.Q7);
Thread.Sleep(3000); //Tiempo de espera para alcanzar cada posición
lv = this.leftArm.GetPositionArticular();
lSuccess =
(Math.Abs(lv[0] - lp.Q1) < maxError) &&
(Math.Abs(lv[1] - lp.Q2) < maxError) &&
(Math.Abs(lv[2] - lp.Q3) < maxError) &&
(Math.Abs(lv[3] - lp.Q4) < maxError) &&
(Math.Abs(lv[4] - lp.Q5) < maxError) &&
(Math.Abs(lv[5] - lp.Q6) < maxError);
rv = this.rightArm.GetPositionArticular();
rSuccess =
(Math.Abs(rv[0] - rp.Q1) < maxError) &&
(Math.Abs(rv[1] - rp.Q2) < maxError) &&
(Math.Abs(rv[2] - rp.Q3) < maxError) &&
(Math.Abs(rv[3] - rp.Q4) < maxError) &&
(Math.Abs(rv[4] - rp.Q5) < maxError) &&
(Math.Abs(rv[5] - rp.Q6) < maxError);
succes = lSuccess && rSuccess;
if (succes)
{
TextBoxStreamWriter.DefaultLog.WriteLine("Both arms reached predefined potition " + succes.ToString());
}
else
{
if (!lSuccess)
{
TextBoxStreamWriter.DefaultLog.WriteLine("Left arm not reached pp");
}
if (!rSuccess)
{
TextBoxStreamWriter.DefaultLog.WriteLine("Right arm not reached pp");
}
}
movingRightArm = false;
movingLeftArm = false;
return(succes);
}
public bool LoadMovsAndPositions()
{
PredefPosition[] tempPos = PredefPosition.DeserializeFromXml(this.fileLeftPredefPos);
if (tempPos != null)
{
foreach (PredefPosition pp in tempPos)
{
if (this.leftPredefPos.ContainsKey(pp.Name))
{
TextBoxStreamWriter.DefaultLog.WriteLine("Predefined Position repeated name: " + pp.Name);
}
else
{
this.leftPredefPos.Add(pp.Name, pp);
}
}
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("Cannot load left predefined positions from file: " + this.fileLeftPredefPos);
}
tempPos = PredefPosition.DeserializeFromXml(this.fileRightPredefPos);
if (tempPos != null)
{
foreach (PredefPosition pp in tempPos)
{
if (this.rightPredefPos.ContainsKey(pp.Name))
{
TextBoxStreamWriter.DefaultLog.WriteLine("Predefined Position repeated name: " + pp.Name);
}
else
{
this.rightPredefPos.Add(pp.Name, pp);
}
}
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("Cannot load right predefined positions from file: " + this.fileLeftPredefPos);
}
PredefMovement[] tempMovs = PredefMovement.DeserializeFromXml(this.fileLeftMovements);
if (tempMovs != null)
{
foreach (PredefMovement pm in tempMovs)
{
if (this.leftPredefMovs.ContainsKey(pm.Name))
{
TextBoxStreamWriter.DefaultLog.WriteLine("Predefined Movement repeated name: " + pm.Name);
}
else
{
List <int> toErase = new List <int>();
for (int i = 0; i < pm.Positions.Count; i++)
{
if (this.leftPredefPos.ContainsKey(pm.Positions[i].Name))
{
pm.Positions[i] = this.leftPredefPos[pm.Positions[i].Name];
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("leftArm: PredefPosition " + pm.Positions[i].Name + " on PredefMov " + pm.Name + " couldn´t be found on current PredefPos list");
toErase.Add(i);
//pm.Positions.RemoveAt(i);
}
}
int count = 0;
foreach (int num in toErase)
{
pm.Positions.RemoveAt(num - count);
count++;
}
this.leftPredefMovs.Add(pm.Name, pm);
}
}
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("Cannot load left predefined movements from file: " + this.fileLeftMovements);
}
/*
* if(this.leftPredefMovs!=null)
* foreach(PredefMovement pm in (PredefMovement[])this.leftPredefMovs)
* foreach (PredefPosition pp in pm.Positions)
* {
* if (this.leftPredefPos.ContainsKey(pp.Name))
* pp = this.leftPredefPos[pp.Name];
* else
* {
* TextBoxStreamWriter.DefaultLog.WriteLine("leftArm: PredefPosition " + pp.Name + " on PredefMov " + pm.Name + " couldn´t be found on current PredefPos list");
* pm.Positions.Remove(pp.Name);
* }
* }
*/
tempMovs = PredefMovement.DeserializeFromXml(this.fileRightMovements);
if (tempMovs != null)
{
foreach (PredefMovement pm in tempMovs)
{
if (this.rightPredefMovs.ContainsKey(pm.Name))
{
TextBoxStreamWriter.DefaultLog.WriteLine("Predefined Movement repeated name: " + pm.Name);
}
else
{
List <int> toErase = new List <int>();
for (int i = 0; i < pm.Positions.Count; i++)
{
if (this.rightPredefPos.ContainsKey(pm.Positions[i].Name))
{
pm.Positions[i] = this.rightPredefPos[pm.Positions[i].Name];
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("rightArm: PredefPosition " + pm.Positions[i].Name + " on PredefMov " + pm.Name + " couldn´t be found on current PredefPos list");
toErase.Add(i);
//pm.Positions.RemoveAt(i);
}
}
int count = 0;
foreach (int num in toErase)
{
pm.Positions.RemoveAt(num - count);
count++;
}
this.rightPredefMovs.Add(pm.Name, pm);
}
}
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("Cannot load right predefined movements from file: " + this.fileRightMovements);
}
/*
* if (this.righttPredefMovs != null)
* foreach (PredefMovement pm in this.rightPredefMovs)
* foreach (PredefPosition pp in pm.Positions)
* {
* if (this.rightPredefPos.ContainsKey(pp.Name))
* pp = this.rightPredefPos[pp.Name];
* else
* {
* TextBoxStreamWriter.DefaultLog.WriteLine("rightArm: PredefPosition " + pp.Name + " on PredefMov " + pm.Name + " couldn´t be found on current PredefPos list");
* pm.Positions.Remove(pp.Name);
* }
* }
*/
this.OnFilesLoaded(this.status);
return(true);
}
