SIGMA robopac 150 - IRB 6620

Robot Movement Analysis : design path

Relative Distance to p10, p20, p30 Relative Distance to Center of Coordinate

Data of (ModPos) (Positions in Coordinate)
Step-
Position
Xp Yp Zp X Y Z
1 pPick 0 0 0 1744.76 159.09 1111.56
2 pPick 0 0 -69 1744.76 159.09 1042.56
3 pPick 0 0 90 1744.76 159.09 1201.56
4 pHome 0 0 0 1371.19 159.09 1300
5 pPlace 0 0 150 1016.42 159.09 1150
6 pPlace 0 0 0 1016.42 159.09 1000
7 pPlace 0 0 -150 1016.42 159.09 850
8 pPlace 0 0 0 1016.42 159.09 1000
9 pHome 0 0 -40 1371.19 159.09 1260
10 pPick 0 0 90 1744.76 159.09 1201.56

Note: robtarget x y z Note

pPick 1744.76 159.09 1111.56 "Before Pick" Position
pPlace 1016.42 159.09 1000 "Before Place" Position
pHome 1371.19 159.09 1300 Home Position

robtarget
Input
ModPos
Output Positions in Coordinate

Brake
AccSet: (1) (2) (3) (4) Brake (5) Brake (6) (7)

Brake
(step) Brake
(13) (12) (11) Brake (10) (9) (8)

PATH MAPPING IN Y-AXIS DIRRECTION (SIDE OF VIEW)

1400

1350

1300
step_4
1250

1200
Movement in z-axis dirrection

step_9 step_3 = step_10

1150

1100 step_5 step_1 sumbu-x

1050
Column H
1000 step_2
950 step_6 = step_8

900

850 step_7

800

750

700
1000 1100 1200 1300 1400 1500 1600 1700 1800
Movement in x-axis dirrection
 PATH MAPPING IN Z-AXIS DIRRECTION (ON TOP OF VIEW)
165

164

163
Movement in y-axis dirrection

162

161

160

159
step_5 = step_6 = step_1 = step_2 =
step_4 = step_9
step_7 = step_8 step_3 = step_10
158

157

156

155
1000 1100 1200 1300 1400 1500 1600 1700 1800
Movement in x-axis dirrection

PATH MAPPING IN X-AXIS DIRRECTION (FRONT OF VIEW)

1600
1550
1500
1450
1400
Movement in z-axis dirrection

1350
1300
step_4
1250
1200 step_9

1150 step_3 = step-10

1100 step_5
step_1
1050
1000 step_2
step_6
950
900
850 step_7

800
150 151 152 153 154 155 156 157 158 159 160
Movement in y-axis dirrection
WORKING RANGE USING IRB 660 (4-axes) - for
alternative

300

1350
380 1000

650

275 275

930
 CALCULATION OF MUCH TIME THAT ROBOT NEED TO FINISH ONE CYCLE MOVEMENT

"Linear Velocity" Approach

"Move" Instruction on program
Step S(mm) Vreal (mm/S) t (second) Vmax (mm/S)
MoveL pPick, v600, z0 1 to 2 69 120 0.58 150
MoveL Offs(pPick,0,0,-69), v150, fine 2 to 3 159 400 0.4 500
MoveLDO Offs(pPick,0,0,150), v500, z100 3 to 4 386.32 480 0.8 600
MoveL pHome, v600, z200 4 to 5 385.18 240 1.6 300
MoveL Offs(pPlace,0,0,200), v300, z100 5 to 6 150 480 0.31 600
MoveL pPlace, v600, z0 6 to 7 150 240 0.63 300
MoveL Offs(pPlace,0,0,-229), v300, fine 7 to 8 150 800 0.19 1000
MoveL pPlace, v1000, z200 8 to 9 439.84 800 0.55 1000
MoveL Offs(pHome,0,0,-20), v1000, z200 9 to 10 378.11 800 0.47 1000
MoveL Offs(pPick,0,0,100), v1000, z100 10 to 1 90 480 0.19 600
Total of time when Robot moves ("Move" instruction) : 5.53 second
Total of time when Robot doesn't move ("Wait Time" instruction) : 1.2 second
Speed Setting : 80 %

TOTAL OF TIME TO FINISH 1 CYCLE : 6.73 second / cycle

WORKING RANGE : Using Pedestrial (height = 655mm)

sumbu-z

2564

pHome

pPick

300
pPlace

sumbu-x

380 1000 1350

655 650
976

275 275

930
858 854
210
210

1473 2204
 Notes :

S : The distance that robot must reach from position at step(n) to

position at step(n+1)

example :
position at step_4
Y-axis ( X2,Y2 )

S (mm)

( X1,Y1 )
position at step_3

X-axis
Phytagoras :

S * S = [ ( X2 - X1 ) * ( X2 - X1 ) + ( Y2 - Y1 ) * ( Y2 - Y1 ) ]

Vmax : Maximum speed of the robot

Vreal : Speed after "Speed Setting" has been changed

t : The time that robot need to move from position at

step(n) to position at step(n+1)

S = Vreal * t

S
t =
Vreal

Limitation : 1. We neglect AccSet (acceleration/deccelaration instruction)

The Instruction to make acceleration/decceleration more "smooth"
is not included in my calculation.

2. We neglect the zonedata (z)

In actual : in corner path, robot moves in parabolic
In calculation : in corner path, robot makes angle point,
so the velocity is linear (not angular).

3. We neglect the other instruction (SetDO gripper, IF, etc)

When robot do SetDO instruction (pick and place the bottles)
or choose selection (IF…condition), it need a much time too.
But this is not included in my calculation.
1350
 SIGMA robopac 150 - IRB 6620
main TOP
Jackson Structured Programming
SEQUENCE Date : 20090114
ORDER OF Programmer : Dhyan Wicaksono
EXECUTION Feature :
pressure_low
A B C Dry Run Mode
1 Air Pressure Switch Sensor
"Conveyor is Full of Bottles" Sensor
DOWN "Case is ready" Sensor
pressure_low = 1
0 RIGHT LEFT
ORDER OF
EXECUTION

SetDO bottle_conv, 0 Stop; flag_stop

true false
flag_stop = true flag_emerg

true false

SetDO bottle_conv, 0 Stop; flag_stop = false flag_emerg = true flag_emerg = false

WaitDI
safety_door, 1

SetDO flag_emerg = move_to_above_ move to dry_run_

Stop; initial_condition
bottle_conv, 0 false pActual pHome (step-4) ………. step-10 mode
home position normal
1
step-4 until step-10 : copy of
SetDO gripper_on, 0 dry_run_
robot_movement (normal step)
SetDO gripper_off, 0 move to point mode=1 loop1
SetDO Lamp_RED, 0 1300 mm
SetDO Lamp_GREEN, 1 on z-axis dirrection
SetDO case_conv, 1 TPWrite
Interrupt
SetDO case_stopper_1, 0 "DRY RUN MODE" step-1 ……….
SetDO case_stopper_2, 1 dry run
without gripper o
air pressure ch
stop_button, 1 Red_emergency, 1 safety_door, 0 pause, 1

stop_is_pressed emerg_is_pressed pause_is_pressed

flag_stop = tru
TPWrite
"RESET PAUSE"

flag_stop = true flag_emerg = true StopMove\ Lamp_RED_is_ StopMove\ SetDO TPWrite WaitDI SetDO
ExitCycle StartMove
TPWrite "STOP" TPWrite "EMER Quick turn_on Quick Lamp_GREEN, 0 "PAUSE" reset_pause, 1 Lamp_GREEN, 1 SetDO
StopMove
GENCY STOP" lamp_GREE

Without "Multitasking" : although bottle_sensor has

become OFFover the limit time we have defined, but
bottle_conveyor become still ON, because the checking
 bottle_conveyor become still ON, because the checking
by Controller still wait until robot reach the position at S31 STATE / KEADAAN (OUTPUT)
step-1.

Program execution
CONTROL (INPUT)
in parallel
Sequential Function Chart (SFC) S0
SIGMA robopac 150 MX
start

S11 (bottle_conv) ON S21 (case_conv) ON S31 (step_1)

(case_stopper_1) OFF
bottle_sensor OFF
(case_stopper_2) ON
stop_button ON bottle_sensor OFF case_sensor_1 ON Without "Multitasking" : When Robot has finished
case_sensor_2 ON S32 (step_2) step-8 (place the bottle), robot must wait until all of
case_sensor become OFF, then robot can do its task at
case_sensor_3 ON ………..
step-9 and so on. This waste the time. Robot should still
move to pick the bottle and doesn't care whatever
S12 (bottle_conv) OFF (step_5)
S22 (case_conv) OFF condition of the case_sensor at this time.
case_sensor_1 ON
(centering) ON
case_sensor_2 ON
stop_ (case_stopper_2) OFF case_sensor_3 ON
button step_3 bottle_sensor ON
ON step_8
S33 (step_6)

………..
S23 (case_conv) ON
(step_8)
(centering) OFF

Bottle Conveyor System case_sensor_1 OFF

(case_stopper_1) ON
case_sensor_2 OFF
case_sensor_1 OFF case_sensor_3 OFF
case_sensor_2 OFF
case_sensor_3 OFF
S34 (step_9)

(step_10)

Case Conveyor System

stop_
button stop_button ON
OFF

Note: Untuk sistem alarm juga harus menggunakan Multitasking.

S35 (Exit Cycle)

Robot Movement
 Without "Multitasking" : although bottle_sensor has
become OFFover the limit time we have defined, but
bottle_conveyor become still ON, because the checking
by Controller still wait until robot reach the position at
step-1.
0

dry_run_
mode=0
loop

step_1
flag_stop
……. step-10
dry run step_2 WaitTime
bottle_sensor 1
per on/off & step-1 step-2 0.5
e checking normal normal
1 0
pressure_low = 1
true false bottle_sensor = 1 bottle_sensor = 0

move_to_above_ move to TPWrite SetDO

= true GOTO StopMove
WaitUntil WaitUntil pActual pHome (step-4) "PRESSURE Lamp_G
loop1
bottle_sensor = 0 bottle_sensor = 1 home position LOW"

move to point
DO 1300 mm
ExitCycle =< 2sec > 2sec =< 2sec > 2sec
REEN, 0 on z-axis dirrection

SetDO bottle_conv, 0 GOTO SetDO bottle_conv, 1 GOTO SetDO bottle_conv, 1 GOTO SetDO bottle_conv, 0 GOTO
step_2 step_1 step_1 step_2

TPWrite "CONVEYOR IS NOT FULL ENOUGH OF BOTTLES

 pressure_low

0
Without "Multitasking" : When Robot has finished step-
pressure_low = 0 8 (place the bottle), robot must wait until all of
case_sensor become OFF, then robot can do its task at
step-9 and so on. This waste the time. Robot should still
move to pick the bottle and doesn't care whatever
condition of the case_sensor at this time.
DO step_5
ExitCycle
mp_GREEN, 0

SetDO gripper_on, 1
SetDO gripper_off, 0
PulseDO, bottle_stopper
case_sensor_1 = 1 AND
WaitTime step-3 ……… step-5 case_sensor_2 = 1 AND
0.5 normal normal case_sensor_3 = 1
0 1
SetDO gripper_on, 0
case_sensor_1 = 1 AND
GOTO
case_sensor_2 = 1 AND
step_5
case_sensor_3 = 1
MoveLDO bottle_conv, step_8
1

SetDO centering, 1 WaitTime SetDO WaitTime SetDO case_stopper_1, 1 case_sensor_1 = 0 AND

SetDO case_conv, 0 step-6 0.5 gripper_off, 1 step-8 0.5 SetDO centering, 0 case_sensor_2 = 0 AND
SetDO case_stopper_2, 0 normal normal SetDO case_conv, 1 case_sensor_3 = 0

0 1
 case_sensor_1 = 0 AND
GOTO
step-7 case_sensor_2 = 0 AND
step_8
normal case_sensor_3 = 0

SetDO case_stopper_1, 0
flag_stop
SetDO case_stopper_2, 1 step-9 step-10
normal normal

true false

flag_stop = true GOTO

loop

SetDO
StopMove ExitCycle
lamp_GREEN, 0

off
case-1 case-2 case-3 case-4 case-5
Sequence-1

on
M on

bottle_sensor on
off off
case-1 case-2 case-3 case-4 case-5 case-6 Sequence-2

M off

S1 S2 S3
off
case-1 case-2 case-3 case-4 case-5 case-6 Sequence-3
bottle_conv on

reflector on
M on

bottle_sensor off
off
case-4 case-5 case-6 case-7 Sequence-4

on
M on

case_stopper_1 case_stopper_2 S1 : case-sensor-1

S2 : case-sensor-2
bottle_conv off S3 : case-sensor-3
M : motor case_conv
Bottle Conveyor System Case Conveyor System
 SYSTEM I/0
INPUT
IRC5 Connector PIN Symbol
X3 1 CH0
X3 2 CH1
X3 3 CH2
X3 4 CH3
X3 5 CH4
X3 6 CH5
X3 7 CH6
X3 8 CH7
X3 0V 0V
X3 NC NC
X4 9 CH8
X4 10 CH9
X4 11 CH10
X4 12 CH11
X4 13 CH12
X4 14 CH13
X4 15 CH14
X4 16 CH15
X4 0V 0V
X4 NC NC

OUTPUT
IRC5 Connector PIN Symbol
X1 1 CH0
X1 2 CH1
X1 3 CH2
X1 4 CH3
X1 5 CH4
X1 6 CH5
X1 7 CH6
X1 8 CH7
X1 0V 0V
X1 24V 24V
X2 9 CH8
X2 10 CH9
X2 11 CH10
X2 12 CH11
X2 13 CH12
X2 14 CH13
X2 15 CH14
X2 16 CH15
X2 0V 0V
X2 24V 24V
EM I/0

Label Description
START start / start_to_main
STOP stop_button
EMERG Red_emergency
DOOR safety_door
RES EMERG reset_Red_emerg
PAUSE pause
RES PAUSE reset_pause

0V
-
DRYRUN dry_run_mode
PRESS pressure_low
BOTTLE SEN bottle_sensor
CASE SEN1 case_sensor_1
CASE SEN2 case_sensor_2
CASE SEN3 case_sensor_3

0V
-

Label Description
GRP ON gripper_on
GRP OFF gripper_off
TL RED Lamp_RED
TL GREEN Lamp_GREEN
BOT STOP bottle_stopper
BOT CONV bottle_conv
CASE CONV case_conv
CASE STOP1 case_stopper_1
0V
24V
CASE STOP2 case_stopper_2
CENTER centering
 BACKUP20090114
MODULE MainModule
!--------------------------DATA DECLARATION--------------------------------!
CONST robtarget pPick:=[[1744.76,159.09,1111.56],[0.00173032,-0.0037009,-0.99999,-0.00187268],[0,0,0,0],[9E+0
PERS robtarget pActual:=[[1567.81,159.09,1337.58],[0.00309014,-0.00227079,-0.999991,-0.001609],[0,0,0,0],[9E+0
VAR bool flag_stop:=FALSE;
VAR intnum stop_action:=0;
VAR intnum emerg_action:=0;
VAR bool flag_emerg:=FALSE;
VAR intnum safety_action:=0;
VAR num nilai_z_pActual:=0;
VAR intnum pause_action:=0;
VAR bool flag_pause:=FALSE;
VAR bool timer_empty_on:= FALSE;
VAR bool timer_full_on:= FALSE;
PERS tooldata gripper:=[TRUE,[[0,0,0],[1,0,0,0]],[74,[-2.505,-170,-3.978],[1,0,0,0],0,0,0]];
CONST robtarget pHome:=[[1371.19,159.09,1497.59],[0.00460104,-0.000681911,-0.999988,-0.0013159],[0,0,0,0],[9E+09,9E
CONST robtarget pPlace:=[[1016.42,159.09,1176.83],[0.00584116,-0.000938533,-0.999982,-0.00131916],[0,0,0,0],[9E+09,9E
TASK PERS loaddata load1:=[0,[0,0,0],[1,0,0,0],0,0,0];
!VAR intnum release_pause:=0; => Pause button use toggle system.
!------------------------------------------------------------------------!

!----------------------------MAIN CONTROL-------------------------------!
PROC main()
!WaitDI start, 1;
! => If start signal has been used to trigger the system input
! (start, motor ON, and PP to main), we don't need to use
! <WaitDI start, 1;> as above.
! Using WaitDI make the program is "confuse".

IF pressure_low=1 THEN
SetDO bottle_conv,0;
Stop; !This instruction doesn't work in the first cycle. If we set
!pressure_low on first start, robot still move until execution pass
!"ExitCycle" in the IF instruction after step-2.

!pressure_low := 0; => This "reset" causes Error 40223 ("Program

!execution is immediatelly stoped by a run time error") if we set
!pressure_low=1 on first start.
ENDIF

IF flag_stop=true THEN
SetDO bottle_conv,0;
Stop;
flag_stop := FALSE; !=> as AUTO RESET function
!Stop; => There's no effect <Stop;> is put before
! or after <flag_stop := FALSE;>
ENDIF

IF flag_emerg=true THEN
SetDO bottle_conv,0;
flag_emerg := FALSE; !=> as AUTO RESET function
Stop; !=> is used to prevent Error 40191
ENDIF

!IF flag_pause=true THEN => There's no effect to the program execution.

!flag_pause := FALSE; => There's no effect to the program execution.
!WaitDI pause, 0; => There's no effect to the program execution.
!StartMove; => There's no effect to the program execution.
!initial_condition; => There's no effect to the program execution.
!Stop; => There's no effect to the program execution.
!ENDIF

WaitDI safety_door, 1; !=> waiting for Safety door is closed

initial_condition;
move_to_above_pActual; !=> as Safety Movement

!-------------------------FIRST CYCLE------------------------!
MoveJ pHome, v600, z200, gripper; !Home Position / step-4
AccSet 100, 30;
MoveL Offs(pPlace,0,0,200), v300, z100, gripper; !step-5
step_5:
IF case_sensor_1 = 1 AND case_sensor_2 = 1 AND case_sensor_3 = 1 THEN
SetDO case_conv, 0;
SetDO centering, 1;
SetDO case_stopper_2, 0;
GOTO step_6;
ENDIF
GOTO step_5;

step_6:
AccSet 100, 30;
MoveL pPlace, v600, z0, gripper; !step-6
AccSet 100, 30;
MoveL Offs(pPlace,0,0,-229), v300, fine, gripper; !step-7
WaitTime 0.5;
SetDO gripper_off, high;
MoveL pPlace, v1000, z200, gripper; !step-8
WaitTime 0.5;
SetDO case_stopper_1, 1;
setDO centering, 0;
SetDO case_conv, 1;
step_8:
IF case_sensor_1 = 0 AND case_sensor_2 = 0 AND case_sensor_3 = 0 THEN
SetDO case_stopper_1, 0;
SetDO case_stopper_2, 1;
GOTO step_9;
ENDIF
GOTO step_8;

step_9:
MoveL Offs(pHome,0,0,-20), v1000, z200, gripper; !step-9
MoveL Offs(pPick,0,0,100), v1000, z100, gripper; !step-10
!------------------------------------------------------------!

IF dry_run_mode=1 THEN
 TPWrite "DRY RUN MODE";
loop1:
dry_run_movement;
GOTO loop1;
ENDIF

loop:
robot_movement;
GOTO loop;

ENDPROC
!------------------------------------------------------------------------!

!---------------INITIAL CONDITION & INTERRUPT TRIGGER--------------------!

PROC initial_condition()
SetDO gripper_on, 0;
SetDO gripper_off, 0;
SetDO Lamp_RED, 0;
SetDO Lamp_GREEN, 1;
SetDO case_conv, 1;
SetDO case_stopper_2, 1;
SetDO case_stopper_1, 0;

CONNECT stop_action WITH stop_is_pressed;

ISignalDI stop_button, 1, stop_action;
CONNECT emerg_action WITH emerg_is_pressed;
ISignalDI Red_emergency, 1, emerg_action;
CONNECT safety_action WITH emerg_is_pressed;
ISignalDI safety_door, 0, safety_action;
CONNECT pause_action WITH pause_is_pressed;
ISignalDI pause, 1, pause_action;
!IDelete pause_action; => cause the interrupt will not happened
!CONNECT release_pause WITH pause_is_released;
!ISignalDI\Single, reset_pause, 1, release_pause;
ENDPROC
!------------------------------------------------------------------------!

!-------------------------INTERRUPT STOP---------------------------------!
TRAP stop_is_pressed
flag_stop := TRUE;
TPWrite "STOP";
!SetDO Lamp_GREEN, 0;
! => If we put SetDO Lamp_GREEN here,then when we push on
! the stop button, so GREEN lamp will light off.
! Eventhough the Robot still move to Offs(pPick, 0, 0, 200) position.

!flag_stop := FALSE;
! => If we reset TRAP with using <flag_stop := FALSE;> as above,
! then writing "STOP" will be displayed, but the Robot still move
! eventhough stop_button is pressed.
! In other words, this TRAP can't trigger both IF (IF in "main" and
! IF in "robot_movement").
ENDTRAP
!------------------------------------------------------------------------!

!-------------------INTERRUPT EMERGENCY STOP-----------------------------!

!to activate e-stop that is controlled from touch screen (motors is still ON)
!It's not real e-stop button on flexpendant
TRAP emerg_is_pressed
flag_emerg := TRUE;
TPWrite "EMERGENCY STOP";
StopMove\Quick; !=> Robot stop to move as soon as possible.
!SetDO Lamp_RED, 1;
! => As an alternative using PROC Lamp_RED_is_turn-on
Lamp_RED_is_turn_on;
!SetDO gripper_on, high;
!SetDO gripper_off, low;
! => Both instructions above have no effect to the program execution.
ExitCycle; !=> As a RELEASE function of <StopMove;> instruction above.
! So the Robot will not be trapped on the last Robot
! position when interrupt was happened.
ENDTRAP
!------------------------------------------------------------------------!

!---------------------RED LAMP IS TURN ON--------------------------------!

PROC Lamp_RED_is_turn_on()
!loop_lamp_red:
! => Except at "main proc",using "loop" is not allowed.
! Using loop at this procedure will make PP(Program Pointer)
! can't go out from this procedure.
SetDO Lamp_RED, 1;
SetDO Lamp_GREEN, 0;
!WaitTime 0.5;
!SetDO Lamp_RED, low;
!WaitTime 0.5;
!GOTO loop_lamp_red;
ENDPROC
!------------------------------------------------------------------------!

!------------------------SAFETY MOVEMENT---------------------------------!
PROC move_to_above_pActual()
pActual := CRobT(); !=> to save the Robot position.
nilai_z_pActual := 1300-pActual.trans.z;
StartMove;
MoveL offs(pActual, 0, 0, nilai_z_pActual), v400, fine, gripper;
ENDPROC
!------------------------------------------------------------------------!

!---------------------------INTERRUPT PAUSE------------------------------!
TRAP pause_is_pressed
!flag_pause := TRUE; => There's no effect to the program execution.
StopMove\Quick;
SetDO Lamp_GREEN, 0;
TPWrite "PAUSE";
!WaitDI pause, 0;
WaitDI reset_pause,1;
 StartMove;
SetDO Lamp_GREEN, 1;
TPWrite "RESET PAUSE";
!IDelete pause_action; => There's no effect to the program execution.
!ExitCycle; => Instruction error 40191: Variable and trap routine
! already connected.It's not legal to connect
! a specific variable with a trap routine more than once.
!RETURN; => There's no effect to the program execution.
ENDTRAP
!------------------------------------------------------------------------!
!TRAP pause_is_released
!WaitDI reset_pause,0;
!StartMove;
!SetDO Lamp_GREEN, 1;
!ENDTRAP

!-----------------------CONTROL FLOW GERAKAN ROBOT-----------------------!

PROC robot_movement()
AccSet 100, 30;
MoveL pPick, v600, z0, gripper; !step-1
step_1:
IF bottle_sensor = 1 THEN
WaitUntil bottle_sensor = 0 \ MaxTime:=2 \TimeFlag:=timer_empty_on;
IF timer_empty_on THEN
SetDO bottle_conv, 1;
TPWrite "CONVEYOR IS NOT FULL ENOUGH OF BOTTLES";
GOTO step_1;
ENDIF
SetDO bottle_conv, 0;
GOTO step_2;
ENDIF
WaitUntil bottle_sensor = 1 \ MaxTime:=2 \TimeFlag:=timer_full_on;
IF timer_full_on THEN
SetDO bottle_conv, 0;
GOTO step_2;
ENDIF
SetDO bottle_conv, 1;
TPWrite "CONVEYOR IS NOT FULL ENOUGH OF BOTTLES";
GOTO step_1;

!If we put "IF pressure_low=1" here, then it's too late. Robot still
!pick the bottles if air pressure drop while robot move to pPick(0,0,-69)
!position
step_2:
AccSet 100, 30;
MoveL Offs(pPick,0,0,-69), v150, fine, gripper; !step-2
WaitTime 0.5;

IF pressure_low=1 THEN
move_to_above_pActual;
MoveL pHome, v600, z200, gripper; !Home Position
StopMove;
TPWrite "PRESSURE LOW";
 SetDO Lamp_GREEN, 0;
ExitCycle;
ENDIF

!PulseDO\PLength:=0.2, gripper_on;
SetDO gripper_on, high;
SetDO gripper_off, low;
PulseDO\PLength:=0.7, bottle_stopper;
WaitTime 0.2;
SetDO gripper_on, low;
AccSet 100, 50;
MoveLDO Offs(pPick,0,0,150), v500, z100, gripper,bottle_conv,1;!step-3
MoveL pHome, v600, z200, gripper; !Home Position / step-4
AccSet 100, 30;
MoveL Offs(pPlace,0,0,200), v300, z100, gripper; !step-5
step_5:
IF case_sensor_1 = 1 AND case_sensor_2 = 1 AND case_sensor_3 = 1 THEN
SetDO case_conv, 0;
SetDO centering, 1;
SetDO case_stopper_2, 0;
GOTO step_6;
ENDIF
GOTO step_5;

step_6:
AccSet 100, 30;
MoveL pPlace, v600, z0, gripper; !step-6
AccSet 100, 30;
MoveL Offs(pPlace,0,0,-229), v300, fine, gripper; !step-7
WaitTime 0.5;
SetDO gripper_off, high;
MoveL pPlace, v1000, z200, gripper; !step-8
WaitTime 0.5; !As a delay timer, centering will not OFF / case_conv
!will not ON / case_stopper_1 will not ON until Robot reach pPlace.
!Cause their ON/OFF action is faster than robot movement.
SetDO case_stopper_1, 1;
setDO centering, 0;
SetDO case_conv, 1;
step_8:
IF case_sensor_1 = 0 AND case_sensor_2 = 0 AND case_sensor_3 = 0 THEN
SetDO case_stopper_1, 0;
SetDO case_stopper_2, 1;
GOTO step_9;
ENDIF
GOTO step_8;

step_9:
MoveL Offs(pHome,0,0,-20), v1000, z200, gripper; !step-9
MoveL Offs(pPick,0,0,100), v1000, z100, gripper; !step-10

IF flag_stop=true THEN
StopMove; !=> Robot stops to move with a long distance
! of braking.
 SetDO Lamp_GREEN, 0;

!flag_stop := FALSE;
! => Instruction error 40191: Variable and trap routine already
! connected.It's not legal to connect a specific variable
! with a trap routine more than once.
! If we reset this IF, then we can't trigger above IF,
! so <Stop;> will not ON (program execution can't stop)

ExitCycle; !=> has a function as RELEASE of Robot Movement,

! so Robot will not be trapped at the last position
! Offs(pPick, 0, 0, 100) when we start program again.
ENDIF
ENDPROC
!------------------------------------------------------------------------!

!----------------------------DRY RUN MOVEMENT----------------------------!

PROC dry_run_movement()
AccSet 100, 30;
MoveL pPick, v600, z0, gripper; !step-1
step_1:
IF bottle_sensor = 1 THEN
WaitUntil bottle_sensor = 0 \ MaxTime:=2 \TimeFlag:=timer_empty_on;
IF timer_empty_on THEN
SetDO bottle_conv, 1;
TPWrite "CONVEYOR IS NOT FULL ENOUGH OF BOTTLES";
GOTO step_1;
ENDIF
SetDO bottle_conv, 0;
GOTO step_2;
ENDIF
WaitUntil bottle_sensor = 1 \ MaxTime:=2 \TimeFlag:=timer_full_on;
IF timer_full_on THEN
SetDO bottle_conv, 0;
GOTO step_2;
ENDIF
SetDO bottle_conv, 1;
TPWrite "CONVEYOR IS NOT FULL ENOUGH OF BOTTLES";
GOTO step_1;

step_2:
AccSet 100, 30;
MoveL Offs(pPick,0,0,-69), v150, fine, gripper; !step-2
AccSet 100, 50;
MoveL Offs(pPick,0,0,150), v500, z100, gripper;!step-3
MoveL pHome, v600, z200, gripper; !Home Position / step-4
AccSet 100, 30;
MoveL Offs(pPlace,0,0,200), v300, z100, gripper; !step-5
step_5:
IF case_sensor_1 = 1 AND case_sensor_2 = 1 AND case_sensor_3 = 1 THEN
SetDO case_conv, 0;
SetDO centering, 1;
SetDO case_stopper_2, 0;
 GOTO step_6;
ENDIF
GOTO step_5;

step_6:
AccSet 100, 30;
MoveL pPlace, v600, z0, gripper; !step-6
AccSet 100, 30;
MoveL Offs(pPlace,0,0,-229), v300, fine, gripper; !step-7
MoveL pPlace, v1000, z200, gripper; !step-8
WaitTime 0.5;
SetDO case_stopper_1, 1;
setDO centering, 0;
SetDO case_conv, 1;
step_8:
IF case_sensor_1 = 0 AND case_sensor_2 = 0 AND case_sensor_3 = 0 THEN
SetDO case_stopper_1, 0;
SetDO case_stopper_2, 1;
GOTO step_9;
ENDIF
GOTO step_8;

step_9:
MoveL Offs(pHome,0,0,-20), v1000, z200, gripper; !step-9
MoveL Offs(pPick,0,0,100), v1000, z100, gripper; !step-10

IF flag_stop=true THEN
StopMove;
SetDO Lamp_GREEN, 0;
ExitCycle;
ENDIF
ENDPROC

ENDMODULE
0.00187268],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
1,-0.001609],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

013159],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
0131916],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];