91 lines
5.4 KiB
Modula-2
Executable File
91 lines
5.4 KiB
Modula-2
Executable File
MODULE Module1
|
|
|
|
PERS num RP_state := 0;
|
|
|
|
CONST robtarget Target_10_start:=[[0,-700,550],[0.707106781,0.707106781,0,0],[-1,0,1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_20_conv_home:=[[0.000015044,-899.999926384,269.999951061],[0.70710675,0.707106812,-0.000000043,0.000000022],[-1,0,1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_30_conv_grab:=[[0.00001142,-1021.999926384,269.999940363],[0.70710675,0.707106812,-0.000000043,0.000000022],[-1,0,1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_30_conv_pickup:=[[0.000002228,-1021.999935152,369.999940363],[0.70710675,0.707106812,-0.000000043,0.000000022],[-1,0,1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_40_path01:=[[479.999988804,-850.00000001,659.9999672],[0.706433743,0.706433803,0.030843521,0.030843588],[-1,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_40_path02:=[[649.999979484,-247.881903446,873.407485079],[0.500000008,0.500000007,0.499999948,0.500000037],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_50_path:=[[280.000025506,528.000034648,620.000102478],[0.405579805,0.405579788,0.579227916,0.579228003],[1,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_60_pour_start:=[[289.396951815,531.420235805,620.000103612],[0.579227994,0.183012719,0.683012656,0.405579817],[1,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_70_pour:=[[293.45671547,520.266112998,658.524172241],[0.683012739,-0.061628385,0.704415989,0.18301272],[1,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_70_pour_2:=[[278.726755256,546.117313059,699.325443391],[0.704416067,-0.298836196,0.640856358,-0.061628412],[1,0,-2,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_80_shake:=[[289.397,531.42,620],[0.24184477,0.541675207,0.45451943,0.664463065],[1,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_70_pour_3:=[[241.226011537,605.292679296,759.815826267],[0.664463065,-0.45451943,0.541675208,-0.241844769],[1,0,-2,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_90_bottlegone:=[[500,400,50],[0.5,0.5,0.5,0.5],[0,0,1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
!***********************************************************
|
|
!
|
|
! Module: Module1
|
|
!
|
|
! Description:
|
|
! Controls the robot "Pourer".
|
|
!
|
|
! Author: Karel Skubal
|
|
!
|
|
! Version: 1.0
|
|
!
|
|
!***********************************************************
|
|
|
|
|
|
!***********************************************************
|
|
!
|
|
! Procedure main
|
|
!
|
|
! This is the entry point of your program
|
|
!
|
|
!***********************************************************
|
|
PROC main()
|
|
TEST RP_state
|
|
CASE 0:
|
|
WaitDI di_sens_RP, 1;
|
|
Pour_beer;
|
|
RP_state := 1;
|
|
CASE 1:
|
|
RP_state := 0;
|
|
ENDTEST
|
|
ENDPROC
|
|
PROC Pour_beer()
|
|
PulseDo do_GripRP_open;
|
|
WaitTime 0.5;
|
|
MoveJ Target_10_start,v1000,fine,My_Mechanism_1\WObj:=wobj0;
|
|
MoveJ Target_20_conv_home,v600,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_30_conv_grab,v200,fine,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDo do_GripRP_close;
|
|
PulseDo do_att_BtoRP;
|
|
WaitTime 0.5;
|
|
MoveL Target_30_conv_pickup,v300,fine,My_Mechanism_1\WObj:=wobj0;
|
|
MoveJ Target_40_path01,v600,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveJ Target_40_path02,v600,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveJ Target_50_path,v600,z200,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_60_pour_start,v50,fine,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_70_pour,v20,fine,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_70_pour_2,v10,fine,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_70_pour,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_60_pour_start,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_50_path,v60,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_60_pour_start,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_80_shake,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_60_pour_start,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_80_shake,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_60_pour_start,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_80_shake,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_60_pour_start,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_70_pour,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_70_pour_2,v20,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_70_pour_3,v10,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_70_pour,v50,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_50_path,v100,z100,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDO do_bottle_empty;
|
|
MoveJ Target_90_bottlegone,v400,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveJ Target_50_path,v800,z100,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDo do_GripRP_open;
|
|
PulseDo do_det_bottle;
|
|
WaitTime 1;
|
|
MoveJ Target_40_path02,v800,z100,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDo do_GripRP_home;
|
|
MoveJ Target_40_path01,v1000,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveJ Target_10_start,v1000,fine,My_Mechanism_1\WObj:=wobj0;
|
|
ENDPROC
|
|
ENDMODULE |