Programme d’exemple 6 : MM_S6_Viz_ErrorHandle
Présentation du programme
Description |
Le robot démarre le projet Mech-Viz, obtient la trajectoire planifiée, puis détermine si la trajectoire planifiée a été obtenue avec succès en fonction du code d’état. Si la trajectoire planifiée est obtenue avec succès, le robot exécute les opérations de prise et de dépôt ; sinon, le robot s’arrête. |
||
Chemin du fichier |
Vous pouvez accéder au répertoire d’installation de Mech-Vision et Mech-Viz et trouver le fichier en utilisant le chemin
|
||
Projet |
Projets Mech-Vision et Mech-Viz |
||
Prérequis |
|
| Ce programme d’exemple est fourni à titre de référence uniquement. Avant d’utiliser le programme, veuillez le modifier en fonction du scénario réel. |
Description du programme
Cette partie décrit le programme d’exemple MM_S6_Viz_ErrorHandle.
| La seule différence entre le programme d’exemple MM_S6_Viz_ErrorHandle et le programme d’exemple MM_S2_Viz_Basic est que MM_S6_Viz_ErrorHandle peut gérer les erreurs en fonction des différents codes d’erreur (le code correspondant à cette fonctionnalité est en gras). Par conséquent, seule la fonctionnalité de gestion des erreurs basées sur des codes d’erreur spécifiques est décrite dans la section suivante. Pour les informations concernant les parties de MM_S6_Viz_ErrorHandle qui sont identiques à celles de MM_S2_Viz_Basic, voir Programme d’exemple 2 : MM_S2_Viz_Basic. |
MODULE MM_S6_Viz_ErrorHandle
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project and get planned path,
! handle errors according to status codes (if no point cloud
! in ROI, retry several times before exit loop)
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num toolid{5}:=[0,0,0,0,0];
LOCAL VAR num vis_pose_num:=0;
LOCAL VAR num count:=0;
LOCAL VAR num retry_cnt:=0;
LOCAL VAR num label{5}:=[0,0,0,0,0];
!define local joint&pose variables
LOCAL CONST jointtarget home:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL CONST jointtarget snap_jps:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget camera_capture:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget drop_waypoint:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget drop:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS jointtarget jps{5}:=
[
[[11.1329,49.0771,-36.9666,0.5343,79.2476,-169.477],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[11.2355,52.1281,-23.3996,0.5938,62.6295,-169.548],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[11.1329,49.0771,-36.9666,0.5343,79.2476,-169.477],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]]
];
!define local tooldata variables
LOCAL PERS tooldata gripper1:=[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]];
PROC Sample_6()
!set the acceleration parameters
AccSet 50, 50;
!set the velocity parameters
VelSet 50, 1000;
!move to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,gripper1;
!initialize communication parameters (initialization is required only once)
MM_Init_Socket \"127.0.0.1\",50000,300;
!move to image-capturing position
MoveL camera_capture,v1000,fine,gripper1;
!open socket connection
MM_Open_Socket;
RECAP:
!trigger Mech-Viz project
MM_Start_Viz 2,snap_jps;
!get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData 1, pose_num, vis_pose_num, status;
!check whether planned path has been got from Mech-Viz successfully
IF status <> 2100 THEN
IF status = 2038 THEN
!no point cloud in ROI, add handling logic here
!self-adding then check retry counter
retry_cnt:=retry_cnt+1;
IF retry_cnt<3 THEN
!jump back to vision retry label if the number of retry times is less than 3
GOTO RECAP;
ELSE
!reset counter and exit loop if the number of retry times has reached 3
retry_cnt:=0;
GOTO END_LOOP;
ENDIF
ELSE
!add other error handling logic here
Stop;
GOTO END_LOOP;
ENDIF
ENDIF
!close socket connection
MM_Close_Socket;
!save waypoints of the planned path to local variables one by one
MM_Get_Jps 1,jps{1},label{1},toolid{1};
MM_Get_JPS 2,jps{2},label{2},toolid{2};
MM_Get_JPS 3,jps{3},label{3},toolid{3};
!follow the planned path to pick
!move to approach waypoint of picking
MoveAbsJ jps{1},v1000,fine,gripper1;
!move to picking waypoint
MoveAbsJ jps{2},v1000,fine,gripper1;
!add object grasping logic here, such as "setdo DO_1, 1;"
Stop;
!move to departure waypoint of picking
MoveAbsJ jps{3},v1000,fine,gripper1;
!move to intermediate waypoint of placing
MoveJ drop_waypoint,v1000,z50,gripper1;
!move to approach waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
!move to placing waypoint
MoveL drop,v300,fine,gripper1;
!add object releasing logic here, such as "setdo DO_1, 0;"
Stop;
!move to departure waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
!move back to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,gripper1;
RETURN;
END_LOOP:
Stop;
ENDPROC
ENDMODULELe flux de travail correspondant au code de programme d’exemple ci-dessus est illustré dans la figure ci-dessous.
Le tableau ci-dessous décrit la fonctionnalité de traitement de différents codes d’état.
| Fonctionnalité | Code et description | ||
|---|---|---|---|
Traiter différents codes d’état |
Après l’exécution de MM_Get_VizData, le robot enregistre le code d’état reçu dans la variable status. Vous pouvez effectuer l’opération correspondante en fonction du code d’erreur reçu spécifique.
|