2EEEB/VRM_20-21
1
0
Fork 0
Code Issues Pull Requests Releases 2 Activity
master
VRM_20-21/Project_1/rapid_programs/ROB1/Module1.mod

153 lines
7.7 KiB
Modula-2
Executable File
Raw Permalink Blame History

MODULE Module1
CONST robtarget T_Vacuum_0:=[[569.499621149,18.499681552,825.6],[0,0,1,0],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_Servo_0:=[[569.499583396,18.499694248,825.600008618],[-0.000000023,-0.000000025,1,0.00000004],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_A1_approach:=[[318.326,-160.729,129.35],[0,0,1,0],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_A2_approach:=[[405.826,-159.341,279.35],[0,0,1,0],[-1,-1,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_A3_approach:=[[493.326,-159.341,129.35],[0,0,1,0],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_B1_approach:=[[318.326,140.659,329.35],[0,0,1,0],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_B2_approach:=[[405.826,140.659,279.35],[0,0,1,0],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_B3_approach:=[[493.326,140.659,179.35],[0,0,1,0],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_A1:=[[318.326,-160.729,29.35],[0,0,1,0],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_A2:=[[405.826,-159.341,29.35],[0,0,1,0],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_A3:=[[493.326,-159.341,29.35],[0,0,1,0],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_B1:=[[318.326,140.659,29.35],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_B2:=[[405.826,140.659,29.35],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_B3:=[[493.326,140.659,29.35],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_A_drop_approach:=[[-49.164,-389.932,250],[0,0,1,0],[-2,-2,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_B_drop_approach:=[[126.126,-389.841,279.35],[0,0,1,0],[-1,-1,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_Plate_A_drop:=[[-49.164,-389.932,29.35],[0,0,1,0],[-2,-2,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_Plate_B_drop:=[[126.126,-389.841,29.35],[0,0,1,0],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_Plate_approach:=[[38.5400092,-389.885016,237.5],[-0.000000023,-0.000000025,1,0.00000004],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_Plate:=[[38.540000368,-389.88500064,45.5],[-0.000000023,-0.000000025,1,0.00000004],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_Plate_drop_approach:=[[188.849,-769.784,375.84],[-0.000000023,-0.000000025,1,0.00000004],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget T_Plate_drop:=[[188.849,-769.784,175.84],[-0.000000023,-0.000000025,1,0.00000004],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR num robot0_state:=0;
VAR num product := 0;
PROC main()
TEST robot0_state
CASE 0:
! Zero positions
PulseDO do_pos0;
MoveAbsJ [[0,0,0,0,0,0], [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, fine, tool0;
! Wait for input from station
WaitDI di_productChosen, 1;
robot0_state := 1;
CASE 1:
TEST product
CASE 0:
Path_10_act;
CASE 1:
Path_20_act;
CASE 2:
Path_30_act;
ENDTEST
Path_40_act;
robot0_state := 2;
CASE 2:
MoveAbsJ [[0,0,0,0,0,0], [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, fine, tool0;
ENDTEST
ENDPROC
PROC Path_10_act()
MoveJ T_A1_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_A1,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_attA1;
WaitTime 0.1;
MoveL T_A1_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_A_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_Plate_A_drop,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_detA1;
WaitTime 0.1;
MoveL T_A_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveJ T_B1_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_B1,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_attB1;
WaitTime 0.1;
MoveL T_B1_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveJ T_B_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_Plate_B_drop,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_detB1;
WaitTime 0.1;
MoveJ T_B_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
ENDPROC
PROC Path_20_act()
MoveJ T_A2_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_A2,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_attA2;
WaitTime 0.1;
MoveL T_A2_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_A_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_Plate_A_drop,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_detA2;
WaitTime 0.1;
MoveL T_A_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveJ T_B2_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_B2,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_attB2;
WaitTime 0.1;
MoveL T_B2_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_B_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_Plate_B_drop,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_detB2;
WaitTime 0.1;
MoveL T_B_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
ENDPROC
PROC Path_30_act()
MoveJ T_A3_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_A3,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_attA3;
WaitTime 0.1;
MoveL T_A3_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveJ T_A_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_Plate_A_drop,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_detA3;
WaitTime 0.1;
MoveL T_A_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveJ T_B3_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_B3,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_attB3;
WaitTime 0.1;
MoveL T_B3_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveJ T_B_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
MoveL T_Plate_B_drop,v1000,fine,VaccumOne\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_detB3;
WaitTime 0.1;
MoveL T_B_drop_approach,v1000,z100,VaccumOne\WObj:=wobj0;
ENDPROC
PROC Path_40_act()
PulseDO do_readyGrip0;
MoveJ T_Plate_approach,v1000,z100,Servo\WObj:=wobj0;
MoveL T_Plate,v1000,fine,Servo\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_gripGrip0;
PulseDO do_attPlate;
WaitTime 0.1;
MoveL T_Plate_approach,v1000,z100,Servo\WObj:=wobj0;
MoveL T_Plate_drop_approach,v1000,z100,Servo\WObj:=wobj0;
MoveL T_Plate_drop,v1000,fine,Servo\WObj:=wobj0;
WaitTime 0.1;
PulseDO do_readyGrip0;
PulseDO do_detPlate;
WaitTime 0.1;
MoveL T_Plate_drop_approach,v1000,z20,Servo\WObj:=wobj0;
WaitTime 0.3;
PulseDO do_plateRdy;
PulseDO do_homeGrip0;
MoveL T_Servo_0,v1000,z100,Servo\WObj:=wobj0;
ENDPROC
ENDMODULE
Reference in New Issue View Git Blame Copy Permalink