Multitasking

SIGMA robopac 150 - IRB 6620 Robot Movement Analysis : design path

Step-

Relative Distance to p10, p20, p30 (ModPos)

Data of Position

Relative Distance to Center of Coordinate (Positions in Coordinate)

Xp

Yp

Zp

X

Y

Z

1744.76 1744.76 1744.76 1371.19 1016.42 1016.42 1016.42 1016.42 1371.19 1744.76

159.09 159.09 159.09 159.09 159.09 159.09 159.09 159.09 159.09 159.09

1111.56 1042.56 1201.56 1300 1150 1000 850 1000 1260 1201.56

1 2 3 4 5 6 7 8 9 10

pPick pPick pPick pHome pPlace pPlace pPlace pPlace pHome pPick

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 -69 90 0 150 0 -150 0 -40 90

Note:

robtarget pPick pPlace pHome

x 1744.76 1016.42 1371.19

y 159.09 159.09 159.09

z 1111.56 1000 1300

Input

robtarget ModPos

Output

Positions in Coordinate

AccSet:

(1)

(step)

(2)

Brake

Note "Before Pick" Position "Before Place" Position Home Position

(4) Brake

(3)

(5) Brake

(6)

(7) Brake

Brake (13)

(12)

(11)

Brake (10)

(9)

(8)

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

step_4

Movement in z-axis dirrection

1250 1200

step_9

step_3 = step_10

1150 1100

step_5

step_1

1050

Column H

1000 950

step_2 step_6 = step_8

900 850

step_7

800 750 700 1000

sumbu-x

1100

1200

1300

1400

1500

Movement in x-axis dirrection

1600

1700

1800

PATH MAPPING IN Z-AXIS DIRRECTION (ON TOP OF VIEW) 165 164

Movement in y-axis dirrection

163 162 161 160 159 158

step_5 = step_6 = step_7 = step_8

step_1 = step_2 = step_3 = step_10

step_4 = step_9

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 Movement in z-axis dirrection

1400 1350 1300

step_4

1250 1200

step_9

1150

step_3 = step-10

1100

step_5

1050

step_1

1000

step_2 step_6

950 900

step_7

850 800 150

151

152

153

154

155

156

Movement in y-axis dirrection

157

158

159

160

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 MoveL Offs(pPick,0,0,-69), v150, fine 2 to 3 159 400 MoveLDO Offs(pPick,0,0,150), v500, z100 3 to 4 386.32 480 MoveL pHome, v600, z200 4 to 5 385.18 240 MoveL Offs(pPlace,0,0,200), v300, z100 5 to 6 150 480 MoveL pPlace, v600, z0 6 to 7 150 240 MoveL Offs(pPlace,0,0,-229), v300, fine 7 to 8 150 800 MoveL pPlace, v1000, z200 8 to 9 439.84 800 MoveL Offs(pHome,0,0,-20), v1000, z200 9 to 10 378.11 800 MoveL Offs(pPick,0,0,100), v1000, z100 10 to 1 90 480 Total of time when Robot moves ("Move" instruction) : Total of time when Robot doesn't move ("Wait Time" instruction) : Speed Setting :

0.58 0.4 0.8 1.6 0.31 0.63 0.19 0.55 0.47 0.19 5.53 1.2 80

150 500 600 300 600 300 1000 1000 1000 600 second second %

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

1000

380 650

655

976

275

275

858 1473

854

930 210 2204

210

1350

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

t

=

S 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

main

SIGMA robopac 150 - IRB 6620 Jackson Structured Programming Date : 20090114 Programmer : Dhyan Wicaksono Feature : Dry Run Mode Air Pressure Switch Sensor "Conveyor is Full of Bottles" Sensor "Case is ready" Sensor

TOP SEQUENCE ORDER OF EXECUTION

pressure_low

A

B

C

1 DOWN

pressure_low = 1

SetDO bottle_conv, 0

RIGHT

0

Stop;

ORDER OF EXECUTION

flag_stop

true

false

flag_stop = true

flag_emerg true

SetDO bottle_conv, 0

Stop;

LEFT

flag_stop = false

false

flag_emerg = true

flag_emerg = false WaitDI safety_door, 1

SetDO bottle_conv, 0

SetDO SetDO SetDO SetDO SetDO SetDO SetDO

flag_emerg = false

Stop;

gripper_on, 0 gripper_off, 0 Lamp_RED, 0 Lamp_GREEN, 1 case_conv, 1 case_stopper_1, 0 case_stopper_2, 1

stop_button, 1

Red_emergency, 1

stop_is_pressed

initial_condition

move_to_above_ pActual

move to point 1300 mm on z-axis dirrection

move to pHome (step-4) ………. step-10 home position normal

dry_run_ mode

step-4 until step-10 : copy of robot_movement (normal step)

dry_run_ mode=1

1 loop1

TPWrite "DRY RUN MODE"

Interrupt

safety_door, 0

step-1 ………. dry run without gripper o air pressure ch

pause, 1

emerg_is_pressed

pause_is_pressed

flag_stop = tru TPWrite "RESET PAUSE" flag_stop = true TPWrite "STOP"

flag_emerg = true TPWrite "EMER GENCY STOP"

StopMove\ Quick

Lamp_RED_is_ turn_on

ExitCycle

StopMove\ Quick

Without "Multitasking" : although bottle_sensor has become OFFover the limit time we have defined, but bottle_conveyor become still ON, because the checking

SetDO Lamp_GREEN, 0

TPWrite "PAUSE"

WaitDI reset_pause, 1

StartMove

SetDO Lamp_GREEN, 1

StopMove

SetDO lamp_GREE

bottle_conveyor become still ON, because the checking by Controller still wait until robot reach the position at step-1.

S31

Program execution in parallel

STATE / KEADAAN (OUTPUT)

CONTROL (INPUT)

S0

Sequential Function Chart (SFC) SIGMA robopac 150 MX

start

S11

(bottle_conv) ON

(case_conv) ON

S21

(step_1)

S31

(case_stopper_1) OFF bottle_sensor OFF

(case_stopper_2) ON stop_button ON

bottle_sensor OFF

case_sensor_1 ON (step_2)

S32

case_sensor_2 ON case_sensor_3 ON S12

(bottle_conv) OFF

……….. (step_5)

(case_conv) OFF

S22

case_sensor_1 ON

(centering) ON stop_ button ON

step_3

case_sensor_2 ON case_sensor_3 ON

(case_stopper_2) OFF

bottle_sensor 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_3 OFF

case_sensor_1 OFF case_sensor_2 OFF case_sensor_3 OFF

S34

(step_9) (step_10)

Case Conveyor System stop_ button OFF

stop_button ON

Note: Untuk sistem alarm juga harus menggunakan Multitasking. S35

(Exit Cycle)

Robot Movement

Without "Multitasking" : When Robot has finished step-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.

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-10 dry run per on/off & e checking true

= true

DO REEN, 0

step_1

flag_stop

bottle_sensor

step-1 normal false

GOTO loop1

ExitCycle

1

step-2 normal

WaitTime 0.5 pressure_low = 1

bottle_sensor = 0

WaitUntil bottle_sensor = 0

WaitUntil bottle_sensor = 1

GOTO step_2

> 2sec

SetDO bottle_conv, 1

1

0

bottle_sensor = 1

=< 2sec

SetDO bottle_conv, 0

step_2

GOTO step_1

=< 2sec

SetDO bottle_conv, 1

GOTO step_1

TPWrite "CONVEYOR IS NOT FULL ENOUGH OF BOTTLES

move_to_above_ pActual

> 2sec

SetDO bottle_conv, 0

GOTO step_2

move to point 1300 mm on z-axis dirrection

move to pHome (step-4) home position

StopMove

TPWrite "PRESSURE LOW"

SetDO Lamp_G

pressure_low 0 Without "Multitasking" : When Robot has finished step8 (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.

pressure_low = 0

DO mp_GREEN, 0

step_5

ExitCycle

SetDO gripper_on, 1 SetDO gripper_off, 0 PulseDO, bottle_stopper

case_sensor_1 = 1 AND case_sensor_2 = 1 AND case_sensor_3 = 1

step-3 ……… step-5 normal normal

WaitTime 0.5

0

1

SetDO gripper_on, 0

case_sensor_1 = 1 AND case_sensor_2 = 1 AND case_sensor_3 = 1

GOTO step_5

step_8

MoveLDO bottle_conv, 1 SetDO centering, 1 SetDO case_conv, 0 SetDO case_stopper_2, 0

step-6 normal

WaitTime 0.5

SetDO gripper_off, 1

step-8 normal

WaitTime 0.5

SetDO case_stopper_1, 1 SetDO centering, 0 SetDO case_conv, 1

case_sensor_1 = 0 AND case_sensor_2 = 0 AND case_sensor_3 = 0 0

1

case_sensor_1 = 0 AND case_sensor_2 = 0 AND case_sensor_3 = 0

GOTO step_8

step-7 normal

SetDO case_stopper_1, 0 SetDO case_stopper_2, 1

step-9 normal

flag_stop

step-10 normal

true

false

flag_stop = true

StopMove

GOTO loop

SetDO lamp_GREEN, 0

ExitCycle

off case-1 case-2

case-3

case-4

case-5

on

Sequence-1 M on

bottle_sensor on off

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

case-6

Sequence-2 M off

S1

bottle_conv on

S3

off case-2 case-3 case-4 case-5 case-6

case-1

reflector

S2

on

Sequence-3 M on

bottle_sensor off off case-4

case-5

case-6 on

case_stopper_1 bottle_conv off Bottle Conveyor System

Case Conveyor System

case_stopper_2

case-7

Sequence-4 M on

S1 : case-sensor-1 S2 : case-sensor-2 S3 : case-sensor-3 M : motor case_conv

SYSTEM I/0 IRC5 Connector X3 X3 X3 X3 X3 X3 X3 X3 X3 X3 X4 X4 X4 X4 X4 X4 X4 X4 X4 X4

PIN 1 2 3 4 5 6 7 8 0V NC 9 10 11 12 13 14 15 16 0V NC

INPUT Symbol CH0 CH1 CH2 CH3 CH4 CH5 CH6 CH7 0V NC CH8 CH9 CH10 CH11 CH12 CH13 CH14 CH15 0V NC

IRC5 Connector X1 X1 X1 X1 X1 X1 X1 X1 X1 X1 X2 X2 X2 X2 X2 X2 X2 X2 X2 X2

PIN 1 2 3 4 5 6 7 8 0V 24V 9 10 11 12 13 14 15 16 0V 24V

OUTPUT Symbol CH0 CH1 CH2 CH3 CH4 CH5 CH6 CH7 0V 24V CH8 CH9 CH10 CH11 CH12 CH13 CH14 CH15 0V 24V

EM I/0 Label START STOP EMERG DOOR RES EMERG PAUSE RES PAUSE

Description start / start_to_main stop_button Red_emergency safety_door reset_Red_emerg pause reset_pause

0V DRYRUN PRESS BOTTLE SEN CASE SEN1 CASE SEN2 CASE SEN3

dry_run_mode pressure_low bottle_sensor case_sensor_1 case_sensor_2 case_sensor_3

0V -

Label GRP ON GRP OFF TL RED TL GREEN BOT STOP BOT CONV CASE CONV CASE STOP1 0V 24V CASE STOP2 CENTER

Description gripper_on gripper_off Lamp_RED Lamp_GREEN bottle_stopper bottle_conv case_conv case_stopper_1

case_stopper_2 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 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 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; StopMove; TPWrite "PRESSURE LOW";

!Home Position

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]];