80 lines
3.9 KiB
Modula-2
Executable File
80 lines
3.9 KiB
Modula-2
Executable File
MODULE Module1
|
|
|
|
PERS num RGF_state := 0;
|
|
CONST robtarget Target_30_pullout:=[[-400,1100,800],[0.5,0.5,-0.5,-0.5],[-1,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_10_bottle_home:=[[-704,1100,730],[0.5,0.5,-0.5,-0.5],[-1,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_20_bottle_lift:=[[-688,1100,850],[0.5,0.5,-0.5,-0.5],[-1,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_40_midpoint:=[[0,900,800],[0.707106781,0.707106781,0,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_50_pour:=[[200,661.362966946,539.647238209],[0.608761429,0.79335334,0,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_60_present:=[[900,800,450],[0.612372436,0.612372436,0.35355339,0.353553391],[0,-1,1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_70_pullback:=[[712,888,450],[0.612372436,0.612372436,0.353553391,0.353553391],[0,-1,1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_80_rinse:=[[510,1800,40],[0.430459335,-0.560985527,0.560985527,-0.430459335],[1,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_80_rinse_approach:=[[430,1800,450],[0.430459335,-0.560985527,0.560985527,-0.430459335],[1,0,-2,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_home_1:=[[0,1049,557.1],[0,1,0,0],[0,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Target_home:=[[0,1306.281825287,105.663474045],[0,1,0,0],[0,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
CONST robtarget Traget_home:=[[0,1306.281825287,105.663474045],[0,1,0,0],[0,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
|
|
!***********************************************************
|
|
!
|
|
! Module: Module1
|
|
!
|
|
! Description:
|
|
! Controls the robot "Glassfetcher".
|
|
!
|
|
! Author: Andrej Pillar
|
|
!
|
|
! Version: 1.0
|
|
!
|
|
!***********************************************************
|
|
|
|
|
|
!***********************************************************
|
|
!
|
|
! Procedure main
|
|
!
|
|
! This is the entry point of your program
|
|
!
|
|
!***********************************************************
|
|
PROC main()
|
|
TEST RGF_state
|
|
CASE 0:
|
|
move_glass;
|
|
RGF_state := 1;
|
|
CASE 1:
|
|
serve_glass;
|
|
PulseDO do_glass_served;
|
|
RGF_state := 0;
|
|
ENDTEST
|
|
ENDPROC
|
|
PROC move_glass()
|
|
MoveJ Target_home_1,v2000,z100,My_Mechanism_1\WObj:=wobj0;
|
|
WaitTime 0.3;
|
|
MoveJ Target_30_pullout,v2000,z100,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDO do_GripRGF_wopen;
|
|
WaitTime 0.5;
|
|
MoveL Target_10_bottle_home,v1000,fine,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDO do_GripRGF_grab;
|
|
PulseDO do_att_glass;
|
|
WaitTime 1;
|
|
MoveL Target_20_bottle_lift,v200,z20,My_Mechanism_1\WObj:=wobj0;
|
|
MoveJ Target_30_pullout,v2000,z200,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDO do_GripRGF_raise15;
|
|
MoveL Target_40_midpoint,v2000,z50,My_Mechanism_1\WObj:=wobj0;
|
|
MoveJ Target_80_rinse_approach,v1000,z100,My_Mechanism_1\WObj:=wobj0;
|
|
MoveL Target_80_rinse,v1000,fine,My_Mechanism_1\WObj:=wobj0;
|
|
WaitTime 1;
|
|
MoveJ Target_80_rinse_approach,v1000,fine,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDO do_GripRGF_grab;
|
|
MoveJ Target_50_pour,v1000,fine,My_Mechanism_1\WObj:=wobj0;
|
|
ENDPROC
|
|
PROC serve_glass()
|
|
WaitDO do_bottle_empty, 1;
|
|
WaitTime 0.75;
|
|
MoveL Target_60_present,v800,fine,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDO do_GripRGF_wopen;
|
|
PulseDO do_det_glass;
|
|
WaitTime 0.3;
|
|
MoveL Target_70_pullback,v2000,z20,My_Mechanism_1\WObj:=wobj0;
|
|
MoveJ Target_home_1,v2000,fine,My_Mechanism_1\WObj:=wobj0;
|
|
PulseDO do_GripRGF_home;
|
|
ENDPROC
|
|
ENDMODULE |