/// <summary>
/// 热点信息转rpy
/// 传入Transform ， position
/// </summary>
/// <param name="src">温度信息</param>
/// <param name="dest">指针</param>
/// <returns>0是正常，1是错误</returns>
public static int Trans_to_rpy(Transform src, Posistion dest)
{
int re = 0;
if (src == null)
{
re = 1;
}
else
{
// dest->x = src->p.x;
dest.x = src.v4.x;
// dest->y = src->p.y;
dest.y = src.v4.y;
// dest->z = src->p.z;
dest.z = src.v4.z;
if (Math.Abs(src.v1.x) > EPS || Math.Abs(src.v1.y) > EPS)
{
dest.Rx = float.Parse((Math.Atan2(src.v2.z, src.v3.z) * 180 / Math.PI).ToString());
dest.Ry = float.Parse((Math.Atan2(-src.v1.z, Math.Sqrt(Math.Pow(src.v1.y, 2))) * 180 / Math.PI).ToString());
dest.Rz = float.Parse((Math.Atan2(src.v1.y, src.v1.x) * 180 / Math.PI).ToString());
}
else if (Math.Abs(src.v1.z) - 1 < EPS)
{
if (src.v1.z > 0)
{
dest.Rx = float.Parse((-Math.Atan2(src.v2.x, src.v2.y) / 180 / Math.PI).ToString());
dest.Ry = -90;
dest.Rz = 0;
}
else
{
dest.Rx = float.Parse((Math.Atan2(src.v2.x, src.v2.y) * 180 / Math.PI).ToString());
dest.Ry = 90;
dest.Rz = 0;
}
}
else
{
re = 1;
}
}
return(re);
}
/// <summary>
/// 机器人姿态
/// </summary>
/// <param name="src">RPY数据</param>
/// <param name="dst">温度信息</param>
/// <returns>0是正常，1是错误</returns>
public static int Rpy_to_trans(Posistion src, ref Transform dst)
{
if (src == null)
{
return(1);
}
//dst->n.x = 1;
dst.v1.x = 1;
//dst->n.y = 0;
dst.v1.y = 0;
// dst->n.z = 0;
dst.v1.z = 0;
// dst->o.x = 0;
dst.v2.x = 0;
// dst->o.y = 1;
dst.v2.y = 1;
// dst->o.z = 0;
dst.v2.z = 0;
// dst->a.x = 0;
dst.v3.x = 0;
// dst->a.y = 0;
dst.v3.y = 0;
// dst->a.z = 1;
dst.v3.z = 1;
//dst->p.x = src->x;
dst.v4.x = src.x; //机器人坐标X
//dst->p.y = src->y;
dst.v4.y = src.y; //机器人坐标Y
// dst->p.z = src->z;
dst.v4.z = src.z; //机器人坐标Z
//rot_x(*dst, src->Rx, dst, LEFT);
Rot_x(dst, src.Rx, dst, LEFT);
// rot_y(*dst, src->Ry, dst, LEFT);
Rot_y(dst, src.Ry, dst, LEFT);
//rot_z(*dst, src->Rz, dst, LEFT);
Rot_z(dst, src.Rz, dst, LEFT);
return(0);
}
/// <summary>
/// 获取相机位置，判断哪个相机可用
/// </summary>
void GetCameraPositon()
{
Posistion posistion = new Posistion
{
x = RobitGroup.ReadD(0).CastTo <float>(-1),
y = RobitGroup.ReadD(1).CastTo <float>(-1),
z = RobitGroup.ReadD(2).CastTo <float>(-1),
Ry = RobitGroup.ReadD(3).CastTo <float>(-1),
Rx = RobitGroup.ReadD(4).CastTo <float>(-1),
Rz = RobitGroup.ReadD(5).CastTo <float>(-1)
}; //机器人矩阵
Transform transform = new Transform(); //相机矩阵
if (CalculatorClass.Rpy_to_trans(posistion, ref transform) == 0) //机器人姿态转为相机所在为位置
{
}
else
{
FormMain.GetOPCTaskInfo("机器人姿态转为相机位置失败");
}
}
/// <summary>
/// 当值发生改变时
/// </summary>
/// <param name="group"></param>
/// <param name="clientId"></param>
/// <param name="values"></param>
public async void OnDataChange(int group, int[] clientId, object[] values)
{
if (group == 2) //任务下发位置
{
for (int i = 0; i < clientId.Length; i++) // 获取跳变信号
{
int tempvalue = int.Parse((values[i].ToString())); //标志位
if (tempvalue == 0) //如果等于0 就是已经处理 可以下发任务
{
if (!isWork)
{
isWork = true;
aa : Posistion posistion = new Posistion
{
x = RobitGroup.ReadD(0).CastTo <float>(-1),
y = RobitGroup.ReadD(1).CastTo <float>(-1),
z = RobitGroup.ReadD(2).CastTo <float>(-1),
Ry = RobitGroup.ReadD(3).CastTo <float>(-1),
Rx = RobitGroup.ReadD(4).CastTo <float>(-1),
Rz = RobitGroup.ReadD(5).CastTo <float>(-1)
}; //机器人矩阵
Transform transform = new Transform(); //相机矩阵
if (CalculatorClass.Rpy_to_trans(posistion, ref transform) == 0) //机器人姿态转为相机所在为位置
{
if (transform.v1.x > 0) //如果是一号热像仪可用
{
CamIndex = 0;
}
else if (transform.v1.x > 1)//如果是二号热像仪可用
{
CamIndex = 1;
}
else//都不可用的情况下 再取一次机器人的位置
{
Thread.Sleep(50);
goto aa;
}
FormDisplay frmDisplay = _DataControl.GetBindedDisplayForm(_LstEnumInfo[CamIndex].intCamIp); //选择已经绑定的IP的显示窗口
FormMain.GetOPCTaskInfo("正在使用窗体：" + frmDisplay.Name + "的热像仪！");
object[] obj = new object[36];
RpyGroup.SyncWrite(obj);//写入坐标和温度
isWork = false;
}
else
{
FormMain.GetOPCTaskInfo("机器人姿态转为相机位置失败");
}
}
else
{
FormMain.GetOPCTaskInfo("在获取热点信息时，产生了一次跳变信号，第" + JumpSingle + "次");
JumpSingle++;
}
}
else
{
FormMain.GetOPCTaskInfo("跳变未找到Group组");
}
}
}
#region 暂时无用
//else if (group == 3)//机器人姿态位置
//{
// for (int i = 0; i < clientId.Length; i++)// 获取跳变信号
// {
// aa: Posistion posistion = new Posistion
// {
// x = float.Parse(RobitGroup.ReadD(0).ToString()),
// y = float.Parse(RobitGroup.ReadD(1).ToString()),
// z = float.Parse(RobitGroup.ReadD(2).ToString()),
// Rx = float.Parse(RobitGroup.ReadD(3).ToString()),
// Ry = float.Parse(RobitGroup.ReadD(4).ToString()),
// Rz = float.Parse(RobitGroup.ReadD(5).ToString())
// };
// Transform transform = new Transform();
// if (CalculatorClass.Rpy_to_trans(posistion, ref transform) > 0)//机器人姿态转换相机位置
// {
// if(transform.v1.x > 0)//如果是一号热像仪可用
// {
// CamIndex = 0;
// }
// else if(transform.v1.x > 1)//如果是二号热像仪可用
// {
// CamIndex = 1;
// }
// else//都不可用的情况下 再取一次机器人的位置
// {
// goto aa;
// }
// FormDisplay frmDisplay = _DataControl.GetBindedDisplayForm(_LstEnumInfo[CamIndex].intCamIp); //选择已经绑定的IP的显示窗口
// FormMain.GetOPCTaskInfo("正在使用窗体："+frmDisplay.Name +"的热像仪！");
// object[] obj = new object[36];
// await Task.Run(()=> frmDisplay.GetInfo(1, out obj) );
// RpyGroup.SyncWrite(obj);//写入坐标和温度
// }
// else
// {
// FormMain.GetOPCTaskInfo("Rpy_to_trans：机器人姿态值转变失败，组:" + group);
// }
// }
//}
#endregion
else
{
FormMain.GetOPCTaskInfo("跳变信号组错误,组:" + group);
}
}
