public MvFanucRobotInfo ReadPosReg(int prno = 0)
{
var robotInfo = new MvFanucRobotInfo();
lock (this)
mobjPosReg.GetValue(prno, ref robotInfo.posArray, ref robotInfo.configArray, ref robotInfo.jointArray, ref robotInfo.userFrame, ref robotInfo.userTool, ref robotInfo.validC, ref robotInfo.validJ);
return(robotInfo);
}
public MvFanucRobotInfo(MvFanucRobotInfo newone)
{
newone.posArray.CopyTo(posArray, 0);
newone.configArray.CopyTo(configArray, 0);
newone.jointArray.CopyTo(jointArray, 0);
userFrame = newone.userFrame;
userTool = newone.userTool;
validC = newone.validC;
validJ = newone.validJ;
robotTime = newone.robotTime;
}
public MvFanucRobotInfo GetCurrRobotInfo()
{
lock (lockCurRobotInfo)
{
//short ValidC = 0, ValidJ = 0; // 移除未使用的變數。by YMWANGN, 2016/11/17。
//Alarm TEST
var msg = "";
var alarmInfo = new MvRobotAlarmInfo();
HasRobotFault(ref msg, ref alarmInfo);
var robotInfo = new MvFanucRobotInfo();
mobjCurPosUF.GetValue(ref robotInfo.posArray, ref robotInfo.configArray, ref robotInfo.jointArray,
ref robotInfo.userFrame, ref robotInfo.userTool, ref robotInfo.validC, ref robotInfo.validJ);
robotInfo.robotTime = DateTime.Now;
robotInfo.isReachTarget = this.MoveIsComplete();
return(robotInfo);
}
}
/// <summary>
/// must 用thread
/// </summary>
/// <param name="Target"></param>
/// <param name="_SelectCorJ"></param>
/// <param name="_SelectOfstOrPos"></param>
/// <param name="_IsMoveUT"></param>
/// <param name="Speed"></param>
/// <returns></returns>
public bool Pns0101MoveStraightSync(Array Target, int _SelectCorJ, int _SelectOfstOrPos, int _IsMoveUT, int Speed)
{
Pns0101MoveStraightAsync(Target, _SelectCorJ, _SelectOfstOrPos, _IsMoveUT, Speed);
while (this.MoveIsComplete())
{
m_currRobotInfo = GetCurrRobotInfo();
var msg = "";
var alarmInfo = new MvRobotAlarmInfo();
if (HasRobotFault(ref msg, ref alarmInfo))
{
break;
}
Thread.Sleep(100);
}
this.MoveCompeleteReply();
return(true);
}
public int Pns0101MoveStraightAsync(Array Target, int _SelectCorJ, int _SelectOfstOrPos, int _IsMoveUT, int Speed)
{
if (!this.IsConnected())
{
return(-1);
}
Array xyzwprArray = new float[9];
Array ConfigArray = new short[7];
Array JointArray = new float[9];
Array TargetPos = Target;
short UF = 0, UT = 0;
short ValidC = 0, ValidJ = 0;
int[] intValues = new int[5];
//object R2Value = 0;
//object R3Value = 0;
this.WriteRegValue(3, _SelectCorJ); //Write R[3]. 0:Mov position, 1:Rotate J1~6
this.WriteRegValue(7, _SelectOfstOrPos); //Write R[7]. 0:Mov with reated pos, 1:Mov with absolute Pos
this.WriteRegValue(8, _IsMoveUT); //Write R[8].0:Offset with UF, 1:Offset with UT
this.WriteRegValue(9, Speed); //Write R[9]. R[9] mm/sec
this.WriteRegValue(5, 0); //Clear to ZERO. R[5] uses to returen MOV END.Return 51 means done.
this.WriteRegValues(1, new int[] { 0, 0 }); //Clear to ZERO.ThisR[1] uses to trigger Robot. Set to 1 means go!
//從哪(當前 移到指到位置
m_currRobotInfo = GetCurrRobotInfo();
xyzwprArray = CurRobotInfo.posArray;
ConfigArray = CurRobotInfo.configArray;
JointArray = CurRobotInfo.jointArray;
UF = CurRobotInfo.userFrame;
UT = CurRobotInfo.userTool;
ValidC = CurRobotInfo.validC;
ValidJ = CurRobotInfo.validJ;
if (_SelectCorJ == 0)
{
if (ValidC != 0) //Valid Cartesian values
{
xyzwprArray.SetValue(TargetPos.GetValue(0), 0); //X_position
xyzwprArray.SetValue(TargetPos.GetValue(1), 1); //Y_position
xyzwprArray.SetValue(TargetPos.GetValue(2), 2); //Z_position
xyzwprArray.SetValue(TargetPos.GetValue(3), 3); //W_position
xyzwprArray.SetValue(TargetPos.GetValue(4), 4); //P_position
xyzwprArray.SetValue(TargetPos.GetValue(5), 5); //R_position
if (TargetPos.Length > 6 && TargetPos.GetValue(6) != null)
{
xyzwprArray.SetValue(TargetPos.GetValue(6), 6); //R_position
}
}
else
{
//return "InValid C Return";
}
}
else
{
if (ValidJ != 0) //Valid Cartesian values
{
JointArray.SetValue(TargetPos.GetValue(0), 0); //J1_position
JointArray.SetValue(TargetPos.GetValue(1), 1); //J2_position
JointArray.SetValue(TargetPos.GetValue(2), 2); //J3_position
JointArray.SetValue(TargetPos.GetValue(3), 3); //J4_position
JointArray.SetValue(TargetPos.GetValue(4), 4); //J5_position
JointArray.SetValue(TargetPos.GetValue(5), 5); //J6_position
if (TargetPos.Length > 6 && TargetPos.GetValue(6) != null)
{
JointArray.SetValue(TargetPos.GetValue(6), 6); //J6_position
}
}
else
{
//return "InValid J Return";
}
}
ConfigArray.SetValue((short)0, 4); //for Index 0
ConfigArray.SetValue((short)0, 5);
ConfigArray.SetValue((short)0, 6);
if (_SelectCorJ == 0)
{
if (!mobjPosReg.SetValueXyzwpr(1, ref xyzwprArray, ref ConfigArray, UF, UT))
{
System.Diagnostics.Debug.WriteLine("Write Fail");
}
}
else
{
mobjPosReg.SetValueJoint(2, ref JointArray, UF, UT);
}
this.mobjDataTable.Refresh();
this.WriteRegValues(1, new int[] { 1, 1 });
return(0);
}
public int Pns0103ContinuityMove(List <float[]> Targets, int _Continuity, int _SelectCorJ, int _SelectOfstOrPos, int _IsMoveUT, int Speed)
{
if (!this.IsConnected())
{
return(-1);
}
Array xyzwprArray = new float[9];
Array ConfigArray = new short[7];
Array JointArray = new float[9];
short UF = 0, UT = 0;
short ValidC = 0, ValidJ = 0;
int[] intValues = new int[5];
object R2Value = 0;
object R3Value = 0;
int targets_cnt = Targets.Count;
for (int i = 0; i < 1; i++)
{
intValues[i] = _SelectCorJ;
}
mobjNumReg.SetValues(3, intValues, 1); //Write R[3]. 0:Mov ,position, 1:Rotate J1~6
for (int i = 0; i < 1; i++)
{
intValues[i] = _SelectOfstOrPos;
}
mobjNumReg.SetValues(7, intValues, 1); //Write R[7]. 0:Mov with reated pos, 1:Mov with absolute Pos
for (int i = 0; i < 1; i++)
{
intValues[i] = _IsMoveUT;
}
mobjNumReg.SetValues(8, intValues, 1); //Write R[8].0:Offset with UF, 1:Offset with UT
for (int i = 0; i < 1; i++)
{
intValues[i] = Speed;
}
mobjNumReg.SetValues(9, intValues, 1); //Write R[9]. R[9] mm/sec
for (int i = 0; i < 1; i++) //Clear to ZERO. R[5]uses to returen MOV END.Return 51 means done.
{
intValues[i] = 0;
}
mobjNumReg.SetValues(5, intValues, 1);
for (int i = 0; i < 2; i++) //Clear to ZERO.ThisR[1] uses to trigger Robot. Set to 1 means go!
{
intValues[i] = 0;
}
mobjNumReg.SetValues(1, intValues, 2);
for (int i = 0; i < 1; i++)
{
intValues[i] = targets_cnt;
}
mobjNumReg.SetValues(42, intValues, 1); //Write R[42] so that give summation of Move Position Count
for (int i = 0; i < 1; i++)
{
intValues[i] = _Continuity;
}
mobjNumReg.SetValues(43, intValues, 1); //Write R[43] for moving continuity setting
//從哪(當前 移到指到位置
m_currRobotInfo = GetCurrRobotInfo();
xyzwprArray = CurRobotInfo.posArray;
ConfigArray = CurRobotInfo.configArray;
JointArray = CurRobotInfo.jointArray;
UF = CurRobotInfo.userFrame;
UT = CurRobotInfo.userTool;
ValidC = CurRobotInfo.validC;
ValidJ = CurRobotInfo.validJ;
ConfigArray.SetValue((short)0, 4); //for Index 0
ConfigArray.SetValue((short)0, 5);
ConfigArray.SetValue((short)0, 6);
for (int i = 0; i < targets_cnt; i++)
{
if (_SelectCorJ == 0)
{
if (ValidC != 0) //Valid Cartesian values
{
xyzwprArray.SetValue(Targets[i].GetValue(0), 0); //X_position
xyzwprArray.SetValue(Targets[i].GetValue(1), 1); //Y_position
xyzwprArray.SetValue(Targets[i].GetValue(2), 2); //Z_position
xyzwprArray.SetValue(Targets[i].GetValue(3), 3); //W_position
xyzwprArray.SetValue(Targets[i].GetValue(4), 4); //P_position
xyzwprArray.SetValue(Targets[i].GetValue(5), 5); //R_position
if (Targets[i].Length > 6 && Targets[i].GetValue(6) != null)
{
xyzwprArray.SetValue(Targets[i].GetValue(6), 6); //R_position
}
}
else
{
//return "InValid C Return";
}
}
else
{
if (ValidJ != 0) //Valid Cartesian values
{
JointArray.SetValue(Targets[i].GetValue(0), 0); //J1_position
JointArray.SetValue(Targets[i].GetValue(1), 1); //J2_position
JointArray.SetValue(Targets[i].GetValue(2), 2); //J3_position
JointArray.SetValue(Targets[i].GetValue(3), 3); //J4_position
JointArray.SetValue(Targets[i].GetValue(4), 4); //J5_position
JointArray.SetValue(Targets[i].GetValue(5), 5); //J6_position
if (Targets[i].GetValue(6) != null)
{
JointArray.SetValue(Targets[i].GetValue(6), 6); //R_position
}
}
else
{
//return "InValid J Return";
}
}
if (_SelectCorJ == 0)
{
mobjPosReg.SetValueXyzwpr(1, ref xyzwprArray, ref ConfigArray, UF, UT);
}
else
{
mobjPosReg.SetValueJoint(2, ref JointArray, UF, UT);
}
}
for (int i = 0; i < 2; i++)
{
intValues[i] = 1;
}
mobjNumReg.SetValues(1, intValues, 2);
return(0);
}
