/// <summary>
/// 开启主线程
/// </summary>
private void mainThread()
{
int Match_timers = 0; //匹配计数
int Start_Glue_Wait_time = 0; //开始喷胶等待的回合
int Stop_Glue_Wait_time = 0; //停止喷胶等待的回合
GLB.Camera_index = 0; //先从垛区拍照
bool isFinishPutDown = false; //是否完成覆合
Pose mypq = new Pose();
Point3 cls = new Point3();
double[] myQQ = cls.ABC2Q(0, Math.PI, 0);//欧拉角转四元数
//#######################################home#######################################
abbController.StartRoutine(AbbCtrl, "MainModule", "goHome");
WaitForRoutineFinish();//等待Rapid程序执行结束
while (ProgramStart)
{
//#######################################喷胶#######################################
if (GLB.run_mode == 1 && IsStart == true)
{
if (IO_IN_11 == false && isFinishPutDown == false)//滚筒上有海绵,未完成覆合
{
if (ForwardCylinder == "0")
{
ForwardCylinder = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO12"); //前挡
}
if (BackwardCylinder == "0")
{
BackwardCylinder = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO14"); //后挡阻挡
}
MotorStatus = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //停止正转
Thread.Sleep(200);
abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO13"); //侧推
Thread.Sleep(1000);
abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO13"); //侧推收回
GLB.run_mode = 2; //进入下一工作模式
GLB.Camera_index = 0;
Thread.Sleep(300);
}
else if (IO_IN_11 == false && isFinishPutDown == true) //滚筒上有海绵,完成覆合
{
ForwardCylinder = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO12"); //前挡收回
if (BackwardCylinder == "1")
{
BackwardCylinder = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO14"); //后挡收回
}
while (IO_IN_11 != true) //直到海绵离开
{
if (IsStart == true && MotorStatus != "1")
{
MotorStatus = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //正转
}
else if (IsStart == false && MotorStatus == "1")
{
MotorStatus = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //停止正转
}
else
{
Thread.Sleep(30);
}
}
isFinishPutDown = false; //清除标志
Thread.Sleep(50); //直到棉完全通过
}
else
{
if (ForwardCylinder != "1")
{
ForwardCylinder = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO12"); //前挡
}
if (MotorStatus != "1")
{
MotorStatus = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //正转:
}
if (Start_Glue_Wait_time == 0 && BackwardCylinder == "1") //后档首次放行
{
BackwardCylinder = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO14"); //后挡放行
}
if (IO_IN_10 == false) //检测到有棉
{
if (Start_Glue_Wait_time < 40) //延时开始喷胶
{
Start_Glue_Wait_time++;
Thread.Sleep(10);
}
else
{
Stop_Glue_Wait_time = 0;//清除停止喷胶延时次数
if (BackwardCylinder == "0")
{
BackwardCylinder = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO14"); //后挡阻挡
}
if (Gun1_6 == "0" && GLB.produceSampleList[GLB.produce_index].axisShort > 900)
{
Gun1_6 = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO9");//喷胶1.6
}
if (Gun2_3 == "0")
{
Gun2_3 = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO10"); //喷胶2.3
}
if (Gun4_5 == "0")
{
Gun4_5 = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO11"); //喷胶4.5
}
}
}
else
{
if (Stop_Glue_Wait_time < 40) //延时停止喷胶
{
Stop_Glue_Wait_time++;
Thread.Sleep(10);
}
else
{
Start_Glue_Wait_time = 0;//清除开始喷胶延时次数
if (Gun1_6 == "1")
{
Gun1_6 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO9"); //停止喷胶
}
if (Gun2_3 == "1")
{
Gun2_3 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO10");
}
if (Gun4_5 == "1")
{
Gun4_5 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO11");
}
}
}
}
}
else if (GLB.run_mode == 1 && IsStart == false)//中途暂停
{
Thread.Sleep(5);
if (MotorStatus == "1")
{
MotorStatus = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //停止正转
}
if (Gun1_6 == "1")
{
Gun1_6 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO9"); //停止喷胶
}
if (Gun2_3 == "1")
{
Gun2_3 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO10");
}
if (Gun4_5 == "1")
{
Gun4_5 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO11");
}
}
//#######################################图漾相机抓取乳胶棉#######################################
if (GLB.run_mode == 2 && GLB.Match_success == true && IsStart == true) //图漾匹配成功 并且没有暂停
{
GLB.avgAngle.Clear(); //旋转角清空队列
GLB.avgCameraPoint3.Clear(); //相机坐标队列清空
Match_timers = 0; //计数清零
GLB.Camera_index = -1; //停止拍照
GLB.Match_success = false; //清除标志位
//**************乳胶棉目标点**************
myQQ = cls.ABC2Q(-Math.PI / 2 - GLB.device_angl, Math.PI, 0);
mypq.FillFromString2("[[" + GLB.robot_device_point.X + "," + GLB.robot_device_point.Y + "," + GLB.robot_device_point.Z + "]," +
"[" + myQQ[0] + "," + myQQ[1] + "," + myQQ[2] + "," + myQQ[3] + "]]");
writePosQuat(mypq);//传输坐标
Thread.Sleep(50);
abbController.StartRoutine(AbbCtrl, "MainModule", "TakeUpFromZoneToDesk");
WaitForRoutineFinish(); //等待Rapid程序执行结束
GLB.Match_success = false; //清除标志位
GLB.Camera_index = 1; //切换相机
GLB.run_mode = 3;
Thread.Sleep(200);
}
else if (GLB.run_mode == 2 && GLB.Match_success == false && IsStart == true)
{
Match_timers += 1;
if (Match_timers > 20)//二十次匹配不到，暂停
{
this.BeginInvoke(new Action(() =>
{
StartBtn.PerformClick();//点击暂停
Emergency_Stop_Alarm = true;
Thread.Sleep(60);
MessageBox.Show("垛区无法识别到乳胶棉,或者数量过少");
Match_timers = 0;
}));
}
Thread.Sleep(300);
}
////#######################################映美金二次抓取乳胶棉#######################################
if (GLB.run_mode == 3 && GLB.Match_success == true && IsStart == true) //图漾匹配成功 并且没有暂停
{
Match_timers = 0; //计数清零
GLB.Camera_index = -1; //停止拍照
GLB.Match_success = false; //清除标志位
//**************平台乳胶棉目标点**************
myQQ = cls.ABC2Q(-GLB.device_angl, Math.PI, 0);
mypq.FillFromString2("[[" + GLB.robot_device_point.X + "," + GLB.robot_device_point.Y + "," + GLB.robot_device_point.Z + "]," +
"[" + myQQ[0] + "," + myQQ[1] + "," + myQQ[2] + "," + myQQ[3] + "]]");
writePosQuat(mypq);//传输坐标
//产品类型--放置点
abbController.producetypeLocal.FillFromString2(GLB.produce_index.ToString());
using (Mastership m = Mastership.Request(AbbCtrl.Rapid))
{
abbController.produceTypeRobot.Value = abbController.producetypeLocal;
m.Release();
}
Thread.Sleep(50);
abbController.StartRoutine(AbbCtrl, "MainModule", "TakeUpFromDeskToTansLine");
WaitForRoutineFinish(); //等待Rapid程序执行结束
isFinishPutDown = true; //完成覆合
GLB.Camera_index = 0; //切换相机
GLB.run_mode = 1;
Thread.Sleep(200);
}
else if (GLB.run_mode == 3 && GLB.Match_success == false && IsStart == true)
{
Thread.Sleep(500);
abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO3");//触发相机
Thread.Sleep(50);
abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO3");
Match_timers += 1;
if (Match_timers > 10)//十次匹配不到，暂停
{
this.BeginInvoke(new Action(() =>
{
StartBtn.PerformClick();//点击暂停
Thread.Sleep(60);
Emergency_Stop_Alarm = true;
MessageBox.Show("平台上无法识别到乳胶棉");
Match_timers = 0;
}));
}
Thread.Sleep(500);
}
else
{
Thread.Sleep(20);
}
}
}