Delta Parallel Robotic Arm by C# | Vision Synchronous Sorting

'************************************************************************************** 'if there are one pair of “through-beam photoelectric sensor” fixed at two sides of assemble line to real-time detect the product position on the assemble line. 'how to fill in MOVESYNC command parameters: 'syncposition: the belt axis position when the object arrived at induction position, it needs to record product position through encoder latching. 'pos1: the position from following axis 1 origin to photoelectric sensor induction position is fixed for each product. '************************************************************************************** GLOBAL CONST BeltAxis=4 'belt axis is axis 4 GLOBAL CONST FollowAxis1=0 'following axis 1 is axis 0 GLOBAL CONST InducPos1=30 'the position from following axis 1 origin to photoelectric sensor induction point GLOBAL CONST StandbyPos1=50'following axis 1’ standby position GLOBAL CONST EmptyPos1=400 'following axis 1’s unloading position 'stop all axes RAPIDSTOP(2) WAIT IDLE 'initialize axis parameters of corresponding axes BASE(FollowAxis1, BeltAxis) ATYPE = 1,1 UNITS = 1000,1000 SPEED = 50,100 DPOS = 0,0 'trigger SCOPE to capture data waveform TRIGGER DELAY(1000) 'following axis moves to standby at first BASE(FollowAxis1) MOVEABS(StandbyPos1) 'belt axis starts to move VMOVE(1) AXIS(BeltAxis) 'if there is one product detected when belt moved at 200 BASE(FollowAxis1) Wait UNTIL MPOS(BeltAxis)> 200 MOVESYNC(0, 2000, 200, BeltAxis, InducPos1) 'when the command (following axis accelerates to synchronize) executed, it will synchronize with the product MOVE_OP(0, ON) 'synchronized, ON vacuum suction MOVESYNC(0, 1000, 200, BeltAxis, InducPos1) 'keep synchronizing 1s MOVESYNC(-1, 0, 0, -1, EmptyPos1) 'move to unloading position MOVE_OP(0, OFF) 'arrive unloading position, off vacuum suction 

/************************************************************************************ 'Task No.: / 'Function of this function: vision positioning 'Input: / 'Output: / 'returned value: sub thread – do vision positioning **************************************************************************************/ public void RunSubTaskVisua() { int TempArrid = 0; float TempVar = 0; WriteLog("Vision Function ON"); while (SysRunFlag > 0) { //when pause button is not pressed while (SysRunFlag == 1) { //capture the image VisuaOper.CameAcquisition(); //do template matching RTDisplay.Image = VisuaOper.ShapeFind(); if (MainWindows.BeltMpos != 0)//if gets the belt encoder position normally when capturing the image { //operate MoveSyncBuff data, please lock at first while (true) { if (MainWindows.SetMoveSyncFlag == 0) { MainWindows.SetMoveSyncFlag = 1; break; } } //find array starting subscript that can save data int ArrId = 0; for (int i = 0; i < 50; i++) { if (MainWindows.MoveSyncBuff[i, 0] == 0) { ArrId = i; break; } } //start to save the data, 10 results can be matched at most TempArrid = ArrId; for (int i = 0; i < 10; i++) { //if the score is OK if (MainWindows.VisionRst[i, 0] >= MainWindows.VisionScore) { int j; //if there is repeated target, delete repeated one for (j = 0; j < TempArrid; j++) { TempVar = MainWindows.VisionRst[i, 1] - MainWindows.BeltMpos - MainWindows.MoveSyncBuff[j, 1] + MainWindows.MoveSyncBuff[j, 4]; if (((TempVar) <= 10) && (TempVar >= -10)) { j = -10; break; } } if (j >= 0) { MainWindows.MoveSyncBuff[ArrId, 0] = 1; MainWindows.MoveSyncBuff[ArrId, 1] = MainWindows.VisionRst[i, 1]; //save matched X coordinate MainWindows.MoveSyncBuff[ArrId, 2] = MainWindows.VisionRst[i, 2]; //save matched Y coordinate MainWindows.MoveSyncBuff[ArrId, 3] = MainWindows.VisionRst[i, 3]; //save matched angle offset MainWindows.MoveSyncBuff[ArrId, 4] = MainWindows.BeltMpos; //save belt position when product is matched ArrId = ArrId + 1; IdentiNum.Text = (Convert.ToInt32(IdentiNum.Text) + 1).ToString(); WriteLog("Vision Target:" + "(" + MainWindows.VisionRst[i, 1].ToString("0,0") + "," + MainWindows.VisionRst[i, 2].ToString("0,0") + ")"); } } //clear scores MainWindows.VisionRst[i, 0] = 0; } //unlock MainWindows.SetMoveSyncFlag = 0; } } Thread.Sleep(100); } } 

/************************************************************************************ 'Task No.: / 'Function of this function: assemble line synchronous sorting 'Input: / 'Output: / 'returned value: / **************************************************************************************/ public void RunSubTaskMotion() { float[] MoveSyncTemp = new float[5]; float TempMpos = 0; while (SysRunFlag > 0) { while (SysRunFlag == 1) { if (MainWindows.MoveSyncBuff[0, 0] == 1) { MainWindows.ZauxErr = zmcaux.ZAux_Direct_GetMpos(MainWindows.g_Handle, MainWindows.ConveyorAxisId, ref TempMpos); //if gets the encoder position correctly if (0 == MainWindows.ZauxErr) { //how far the encoder moves forward TempMpos = TempMpos - MainWindows.MoveSyncBuff[0, 4]; //check whether it is at synchronization starting area if (((MainWindows.MoveSyncBuff[0, 1] + TempMpos) >= MainWindows.SyncReX[0]) && ((MainWindows.MoveSyncBuff[0, 1] + TempMpos) <= MainWindows.SyncReX[1])) { WriteLog("Start to Synchronize to Capture"); //get one group of data MoveSyncTemp[0] = MainWindows.MoveSyncBuff[0, 1] ; //X MoveSyncTemp[1] = MainWindows.MoveSyncBuff[0, 2] ; //Y MoveSyncTemp[2] = MainWindows.GetBinHigt; //capture height MoveSyncTemp[3] = MainWindows.MoveSyncBuff[0, 3]; //Aanle MoveSyncTemp[4] = MainWindows.MoveSyncBuff[0, 4]; //Mpos //send synchronous motion command //0, reset OUT zmcaux.ZAux_Direct_MoveOp(MainWindows.g_Handle, MainWindows.gVAxisList[0], MainWindows.VacSucIo, 0); //1, synchronize with transfer belt at first zmcaux.ZAux_Direct_MoveSync(MainWindows.g_Handle, 0, 0, MainWindows.MoveSyncBuff[0, 4], MainWindows.ConveyorAxisId, 4, MainWindows.gVAxisList, MoveSyncTemp); //2, synchronize a certain time, (there is lag of joint axis) 50ms zmcaux.ZAux_Direct_MoveSync(MainWindows.g_Handle, 0, 50, MainWindows.MoveSyncBuff[0, 4], MainWindows.ConveyorAxisId, 4, MainWindows.gVAxisList, MoveSyncTemp); //3, open vacuum nozzle zmcaux.ZAux_Direct_MoveOp(MainWindows.g_Handle, MainWindows.gVAxisList[0], MainWindows.VacSucIo, 1); //4, synchronize a certain time, 1500ms zmcaux.ZAux_Direct_MoveSync(MainWindows.g_Handle, 0, 700, MainWindows.MoveSyncBuff[0, 4], MainWindows.ConveyorAxisId, 4, MainWindows.gVAxisList, MoveSyncTemp); //5, in synchronization, move axis Z up to safe height, select axis unloading angle MoveSyncTemp[2] = MainWindows.StandPos[2]; //capture height zmcaux.ZAux_Direct_MoveSync(MainWindows.g_Handle, 0, 100, MainWindows.MoveSyncBuff[0, 4], MainWindows.ConveyorAxisId, 4, MainWindows.gVAxisList, MoveSyncTemp); //4, release “synchronize to unloading point” MoveSyncTemp[0] = MainWindows.EmptPos[0]; //X MoveSyncTemp[1] = MainWindows.EmptPos[1]; //Y MoveSyncTemp[2] = MainWindows.EmptPos[2]; //unloading height MoveSyncTemp[3] = MainWindows.EmptPos[3]; //Aanle zmcaux.ZAux_Direct_MoveSync(MainWindows.g_Handle, -1, 0, 0, -1, 4, MainWindows.gVAxisList, MoveSyncTemp); //5, close vacuum nozzle to place material, delay 100ms zmcaux.ZAux_Direct_MoveOp(MainWindows.g_Handle, MainWindows.gVAxisList[0], MainWindows.VacSucIo, 0); zmcaux.ZAux_Direct_MoveDelay(MainWindows.g_Handle, MainWindows.gVAxisList[0], 100); //6, move to safe height MoveSyncTemp[0] = MainWindows.EmptPos[0]; //X MoveSyncTemp[1] = MainWindows.EmptPos[1]; //Y MoveSyncTemp[2] = MainWindows.StandPos[2]; //unloading height MoveSyncTemp[3] = MainWindows.EmptPos[3]; //Aanle zmcaux.ZAux_Direct_MoveAbs(MainWindows.g_Handle, 4, MainWindows.gVAxisList, MoveSyncTemp); //operate MoveSyncBuff data, please lock at first while (true) { if (MainWindows.SetMoveSyncFlag == 0) { MainWindows.SetMoveSyncFlag = 1; break; } } //in vision matching buffer, data are covered for (int k = 0; k < 49; k++) { MainWindows.MoveSyncBuff[k, 0] = MainWindows.MoveSyncBuff[k + 1, 0]; MainWindows.MoveSyncBuff[k, 1] = MainWindows.MoveSyncBuff[k + 1, 1]; MainWindows.MoveSyncBuff[k, 2] = MainWindows.MoveSyncBuff[k + 1, 2]; MainWindows.MoveSyncBuff[k, 3] = MainWindows.MoveSyncBuff[k + 1, 3]; MainWindows.MoveSyncBuff[k, 4] = MainWindows.MoveSyncBuff[k + 1, 4]; } //unlock MainWindows.SetMoveSyncFlag = 0; //until OUT opens int TimeOut = 10000; TimeOut = 100000; //until axis stops int AxisIdle = 0; //axis stopped state while (TimeOut > 0) { zmcaux.ZAux_Direct_GetIfIdle(MainWindows.g_Handle, MainWindows.gVAxisList[0], ref AxisIdle); if (AxisIdle == (-1)) { break; } Thread.Sleep(10); TimeOut = TimeOut - 10; } if (TimeOut <= 0) { //if timeout, report an error, program stops WriteLog("Timeout Waiting for Axis to Stop "); //Thread.Sleep(100); SysRunFlag = 0; break; } SortNum.Text = (Convert.ToInt32(SortNum.Text) + 1).ToString(); WriteLog("Unloading Succeeded "); //exit while, cycle once continue; } else if ((MainWindows.MoveSyncBuff[0, 1] + TempMpos) > MainWindows.SyncReX[1]) { //operate MoveSyncBuff data, please lock at first while (true) { if (MainWindows.SetMoveSyncFlag == 0) { MainWindows.SetMoveSyncFlag = 1; break; } } //in vision matching buffer, data are covered for (int k = 0; k < 49; k++) { MainWindows.MoveSyncBuff[k, 0] = MainWindows.MoveSyncBuff[k + 1, 0]; MainWindows.MoveSyncBuff[k, 1] = MainWindows.MoveSyncBuff[k + 1, 1]; MainWindows.MoveSyncBuff[k, 2] = MainWindows.MoveSyncBuff[k + 1, 2]; MainWindows.MoveSyncBuff[k, 3] = MainWindows.MoveSyncBuff[k + 1, 3]; MainWindows.MoveSyncBuff[k, 4] = MainWindows.MoveSyncBuff[k + 1, 4]; } //unlock MainWindows.SetMoveSyncFlag = 0; //if there is no data in the vision matching buffer area if (MainWindows.MoveSyncBuff[0, 0] == 0) { //Delta moves to standby position zmcaux.ZAux_Direct_MoveAbs(MainWindows.g_Handle, 4, MainWindows.gVAxisList, MainWindows.StandPos); WriteLog("To Standby"); } } } } else { //Delta moves to standby position zmcaux.ZAux_Direct_MoveAbs(MainWindows.g_Handle, 4, MainWindows.gVAxisList, MainWindows.StandPos); } Thread.Sleep(50); } //stop transfer belt zmcaux.ZAux_Direct_Single_Cancel(MainWindows.g_Handle, MainWindows.ConveyorAxisId, 2); } }