Вы находитесь на странице: 1из 25

Project: Culletprima_Robot_2 EPSON RC+ v5.4.

3 SP1 Page 1

Project Summary

Program Files:
Main.prg
Comm.prg
Main1.prg
Main2.prg

Include Files:

Point Files:
Points.pts
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1
File: Main.prg
Last revision: 05.02.2018 11:18:47

'---------- AUTOMATIK - PROGRAM -------------------------------------------------------------


' Project: Culletprima Indonesia Tangerang
' robot with fieldbus interface Profibus Adr. 10
' Date: 11.01.2018
' Author: M. Stark
' Company: WALTEC Germany
'--------------------------------------------------------------------------------------------

Global Real CycleTime


Global Preserve Real PickUpTime
Global Preserve Real GiveOffTime
Global Preserve Real GiveOffTime2
Global Boolean In_Save_Pos, In_Wait_Pos
Global Preserve Integer Vel, LimZAxis, Acc, Dec
Global Preserve Real Z_up, Z_down, Z_down_lim
Global Preserve Integer TrackingVelocity, TrackingAngle, SavedAngle

Real Temp

'-- user defined error numbers (starts at number 8000) --------------------------------------


#define F_ParNotValid 8000 ' parameter not valid -> check parameter range of t
#define F_MonitoringWaterCooling 8001 ' water cooling of robot is not working -> check wat
#define F_MonitoringAir 8002 ' air monitoring -> compressed air pressure too low o
#define F_MonitoringVacuum 8003 ' vacuum monitoring -> indicates that article coul
#define F_MonitoringClimatisation 8004 ' climatisation of cabinet monitoring -> indicates

Function main ' main function: start of automatic program which is selecte

Xqt main_UserInterface ' multitasking: starts user interface task Touch Pendant
Xqt main_Error_and_Status ' multitasking: starts error handling and status task
Xqt main_Communication ' multitasking: Sending and receiving data via Profibus

Integer u, errNum ' local variables


Real alpha, beta ' rotation angle between coordinate systems
Boolean In_Home_Pos ' flag indicating that robot is already in home position

Motor On ' Switch On motor power


Power High ' Power set to maximum
Arch 0, 20, 20 ' defines point to point movement (JUMP) with 20 mm
Arch 1, 30, 30 ' vertical up and 30mm vertical down movement
Weight 6, 400 ' defines weight and inertia settings for robot -> robot wil
' choose appropriate maximum accelerations for each joint
Fine 50, 50, 50, 50 ' higher position error allowed -> faster positioning

Acc = 50 ' presetting of parameters


Dec = 50
Vel = 10
Z_up = 0
Z_down = 0
PickUpTime = 0.1
GiveOffTime = 0.1
GiveOffTime2 = 0.1
TrackingVelocity = 100 ' tracking speed in [mm/s]
TrackingAngle = 8 ' tracking angle in [°]
LimZAxis = 0

In_Home_Pos = False
On Q_Enable ' enabling of external machine (option)
Off Q_start_turner ' preset of starting signal for turning device
Off Q_blow_out
Off Q_vacuum ' maybe inverse valve !!!

' OnErr GoTo errHandler ' automatic error handling routine -> stops robot !

Do ' endless do…loop


Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 2
File: Main.prg
Last revision: 05.02.2018 11:18:47

Halt main_UserInterface
Halt main_Communication ' to avoid error 2200
Resume main_UserInterface

TmReset 0 ' timer reset for cycle time measurement

LimZ LimZAxis ' height limiting -400mm..0mm -> value can be cha
Accel Acc, Dec ' acceleration 0..120% -> value can be changed also
Speed Vel ' velocity 0..100% -> value can be changed also in a

'------------------------- only used when picking needs circular movement -> circular
' coordinate transformation (rotation) to calculate ARC-points depending on TrackingA
' If TrackingAngle >= 0 And TrackingAngle < 45 And (SavedAngle <> TrackingAngle) Then
' SavedAngle = TrackingAngle
'
' alpha = -DegToRad(TrackingAngle) ' clockwise
' beta = alpha /2
'
' CX(P_pick_up_middle) = CX(P_pick_up_start) * Cos(beta) - CY(P_pick_up_start) * S
' CY(P_pick_up_middle) = CX(P_pick_up_start) * Sin(beta) + CY(P_pick_up_start) * C
' CU(P_pick_up_middle) = CU(P_pick_up_start) - TrackingAngle / 2
'
' CX(P_pick_up_end) = CX(P_pick_up_start) * Cos(alpha) - CY(P_pick_up_start) *
' CY(P_pick_up_end) = CX(P_pick_up_start) * Sin(alpha) + CY(P_pick_up_start) * Cos
' CU(P_pick_up_end) = CU(P_pick_up_start) - TrackingAngle
' EndIf
'------------------------------------------------------------------------------------

' robot is waiting in base_position (throw off pos.) and suction head is pointing upw
' If (Sw(I_Preselection) = 0 Or Sw(I_Monitor_Air) = 0) And Not (In_Home_Pos) Then
If Sw(I_Preselection) = 0 And In_Home_Pos = 0 Then ' if robot is not selected or c
Speed 5
Go P_base_pos
In_Home_Pos = InPos
On Q_Enable
EndIf

If Sw(I_Preselection) = 0 Then
GoTo End
EndIf

If Sw(I_Home) = 0 Or In_Home_Pos Then GoTo Start ' if robot has to move to base_po
Speed 5 ' else it will wait for starti
Go P_base_pos
In_Home_Pos = InPos
On Q_Enable
GoTo End

Start: If Sw(I_Start) = 0 Then GoTo End ' waiting for start impulse in automatic mode

LimZ LimZAxis ' height limiting -400mm..0mm -> value can be ch


Accel Acc, Dec ' acceleration 0..120% -> value can be changed
Speed Vel ' velocity 0..100% -> value can be changed also
AccelS 5000 ' acceleration setting for ARC-movement

In_Home_Pos = False

If Sw(I_Mode) = 0 Then ' mode-selection: 0 -> pick and place


Go P_pick_up_start ! D10; On Q_vacuum; D90; Off Q_Enable ! '
Wait PickUpTime
If Sw(I_Throw_Off) Then GoTo ThrowOff
Jump P_place LimZ LimZAxis ! D10; On Q_Enable; D80; Off Q_vacuum !
On Q_blow_out, 0.1, 0
Wait GiveOffTime
Jump P_base_pos C0 LimZ LimZAxis ! D50; On Q_Start_Turning, 0.1 !
Else ' mode-selection: 1 -> tracking
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 3
File: Main.prg
Last revision: 05.02.2018 11:18:47

Go P_pick_up_start CP ! D10; On Q_vacuum; D90; Off Q_Enable ! '


SpeedS TrackingVelocity
' Arc P_pick_up_middle, P_pick_up_end CP ' circular tracking: not used
Move P_pick_up_end CP ' linear tracking
If Sw(I_Throw_Off) Then GoTo ThrowOff
Jump P_place C0 LimZ LimZAxis ! D10; On Q_Enable; D80; Off Q_vacuum !
On Q_blow_out, 0.1, 0
Wait GiveOffTime
Jump P_base_pos C0 LimZ LimZAxis ! D50; On Q_Start_Turning, 0.1 !
EndIf
In_Home_Pos = True
GoTo End_2

ThrowOff:
On Q_Enable ' enable table turning of fpm
Jump P_throw_off C0 LimZ LimZAxis ! D10; On Q_Enable; D80; Off Q_vacuum !
On Q_blow_out, 0.1, 0
Wait GiveOffTime2 ' wait give_off time 2 -> value can be chang
Jump P_base_pos C0 LimZ LimZAxis
In_Home_Pos = True

End_2:
Off Q_vacuum
On Q_Acknowledge, 0.1, 0 ' acknowledge signal for press for cycle time
End:
CycleTime = Tmr(0) ' robot cycle time measurement
Print CycleTime

Resume main_Communication ' to avoid error 2200 (not tested)


Loop

Exit Function

errHandler: ' error handling -> each error will stop r


Select Err
Case F_ParNotValid
Print "Error: Parameter not valid ! "
EResume Next
Case F_MonitoringWaterCooling
Print "Error: Monitoring water cooling ! "
EResume Next
Case F_MonitoringAir
Print "Error: Monitoring compressed air ! "
EResume Next
Case F_MonitoringVacuum
Print "Error: Monitoring vacuum ! "
EResume Next
Case F_MonitoringClimatisation
Print "Error: Monitoring climatisation or robot! "
EResume Next
Send

Fend
Function main_Error_and_Status ' multitasking: starts error handling and status task
Do
If Oport(Q_Error) = 1 Or Oport(Q_SError) = 1 Then ' error active
Off Q_Enable ' when robot is in fault stat
Off Q_vacuum ' switch off vacuum
' ---> add additional functio
Else
EndIf

If Not (Oport(Q_Ready) = 1 Or Oport(Q_Running) = 1) Then ' if none of these bits i


EndIf

' input state of watercooling


Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 4
File: Main.prg
Last revision: 05.02.2018 11:18:47

If Sw(I_monitor_cool) Then ' state of input send to pl


On Q_CoolingWater
Else
Off Q_CoolingWater
Error F_MonitoringWaterCooling ' when water cooling is swi
EndIf

' input state of watercooling


If Sw(I_monitor_clima) Then ' state of input send to p
On Q_Clima_OK
Else
Off Q_Clima_OK
Error F_MonitoringClimatisation ' when climatisation o
EndIf

' input state of compressed air


If Sw(I_monitor_air) Then ' state of input send to plc
On Q_Air_OK
Else
Off Q_Air_OK
Error F_MonitoringAir ' when compressed air is swi
EndIf

' input state of vacuum monitoring


If Sw(I_monitor_vacuum) Then ' state of input send to plc
On Q_Vacuum_OK
Else
Off Q_Vacuum_OK
' Error F_MonitoringVacuum ' when compressed air is swi
EndIf

' output state of pneumatic valves blow out


If Oport(Q_blow_out) Then
On Q_BlowOut_On ' state of output send to
Else
Off Q_BlowOut_On
EndIf

' output state of pneumatic valves vacuum


If Oport(Q_vacuum) Then
On Q_Vacuum_On ' state of output send to p
Else
Off Q_Vacuum_On
EndIf
Loop
Fend
Function main_Communication ' communication via fieldbus interface (Profibus)
' add check for limits if necessary (but is already done in panel of plc side)
' this task will be executed only in automatic mode of robot -> teaching of whole points
Real Time1, Time2, Time3

Do
' fieldbus inputs
Vel = InW(I_V) ' read speed 0...100% (integer)
Acc = InW(I_A) ' read acceleration 0...120% (integer)
Dec = InW(I_A) ' read deceleration 0...120% (integer)
TrackingVelocity = InW(I_V_tracking) ' read tracking speed 0...400 mm/s (301 mm/s
TrackingAngle = InW(I_S_tracking) ' read tracking angle 0...45 °
Time1 = InW(I_T_pick_up) ' read pick up time and make conversion to x
PickUpTime = Time1 /100
Time2 = InW(I_T_place) ' read give_off time and make conversion t
GiveOffTime = Time2 /100
Time3 = InW(I_T_throw_off) ' read give_off 2 time and make conversion
GiveOffTime2 = Time3 /100
Z_up = InW(I_Z_lift_up) / 10 ' read z_lift_up in mm and make conversion (i
Z_down = InW(I_Z_lift_down) / 10 ' read z_lift_down in mm and make conversion
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 5
File: Main.prg
Last revision: 05.02.2018 11:18:47

LimZAxis = -(65536 - InW(I_Z_limit)) / 10 ' negative value

' TEST SIMULATIOM -> wieder löschen


' Vel = 100
' Acc = 100
' Dec = 100
' TrackingVelocity = 100
' TrackingAngle = 8
' PickUpTime = 1
' GiveOffTime = 1
' GiveOffTime2 = 1
' Z_up = 20
' Z_down = 30
' LimZAxis = -40
' ENDE TEST

' multiplexed fieldbus inputs


If Sw(I_LoadPar) Then ' if PLC sends data
Select InW(I_Par_Nr) ' !!! reads parameter number

Case 11 ' reads parameter P0.x coordinate


CX(P_pick_up) = InReal(I_Par_Data)

Case 12 ' reads parameter P0.y coordinate


CY(P_pick_up) = InReal(I_Par_Data)

Case 13 ' reads parameter P0.z coordinate


CZ(P_pick_up) = InReal(I_Par_Data)

Case 14 ' reads parameter P0.u coordinate


CU(P_pick_up) = InReal(I_Par_Data)

Case 21 ' reads parameter P1.x coordinate


CX(P_place_start) = InReal(I_Par_Data)

Case 22 ' reads parameter P1.y coordinate


CY(P_place_start) = InReal(I_Par_Data)

Case 23 ' reads parameter P1.z coordinate


CZ(P_place_start) = InReal(I_Par_Data)

Case 24 ' reads parameter P1.u coordinate


CU(P_place_start) = InReal(I_Par_Data)

Case 31 ' reads parameter P2.x coordinate


CX(P_base_pos) = InReal(I_Par_Data)

Case 32 ' reads parameter P2.y coordinate


CY(P_base_pos) = InReal(I_Par_Data)

Case 33 ' reads parameter P2.z coordinate


CZ(P_base_pos) = InReal(I_Par_Data)

Case 34 ' reads parameter P2.u coordinate


CU(P_base_pos) = InReal(I_Par_Data)

Default

On Q_ReadReady, 0.1 ' robot is ready to plc


' SavePoints "Points.pts" -> evtl. noch Punkt
Send

EndIf

' fieldbus output -> reading of position in run-mode can cause error 2200 -> uncommen
OutReal Q_x, CX(Here) ' sends actual x-value (real) (is only updated by robot
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 6
File: Main.prg
Last revision: 05.02.2018 11:18:47

OutReal Q_y, CY(Here) ' sends actual y-value (real)


OutReal Q_z, CZ(Here) ' sends actual z-value (real)
OutReal Q_u, CU(Here) ' sends actual u-value (real)
OutW Q_v_ist, Vel ' sends actual speed (int)
OutW Q_a_ist, Acc ' sends actual acceleration (int)
Wait 0.5 ' do every 0.5 s (delay had to be added otherwise EPSON c

Loop
Fend

Function main_UserInterface
String a$
Integer a
Do
Cls #24
Print #24, "- WALTEC Robot -- Auto-Menu -----" 'Port-Nummer 24 definiert Touch Pendant
Print #24, "1= position setup"
Print #24, "2= speed setup"
Print #24, "3= timing setup"
Print #24, "4= display actual values"
Print #24, "5= save positions"
Print #24, "6= set values"
Print #24, "-------- actual values ----------- "
Print #24, "t_cycle=", CycleTime, " s"

'Wichtig: Noch Bewegungsgrenzen definieren und Überprüfen ->


' z.B: +-10mm um Aufnahmepunkt für Z-Achsen

Input #24, a$
If a$ = "1" Then
Cls #24
Print #24, "menu position setup"
Print #24, "1= change P_pick_up"
Print #24, "2= change P_place"
Print #24, "3= change LimZ"
Input #24, a$
If a$ = "1" Then
L_P_pick_up: Cls #24
Print #24, "change position P_pick_up"
Print #24, "1= x+0.2mm ", "2=x-0.2mm "
Print #24, "3= y+0.2mm ", "4=y-0.2mm "
Print #24, "5= z+0.2mm ", "6=z-0.2mm "
Print #24, "7= u+1grad ", "8=u-1grad "
Input #24, a$
If a$ = "1" Then
P_pick_up_start = P_pick_up_start +X(0.2) 'X-Achse + x mm
ElseIf a$ = "2" Then
P_pick_up_start = P_pick_up_start -X(0.2) 'X-Achse - x mm
ElseIf a$ = "3" Then
P_pick_up_start = P_pick_up_start +Y(0.2) 'Y-Achse + x mm
ElseIf a$ = "4" Then
P_pick_up_start = P_pick_up_start -Y(0.2) 'Y-Achse - x mm
ElseIf a$ = "5" Then
P_pick_up_start = P_pick_up_start +Z(0.2) 'Z-Achse + x mm
ElseIf a$ = "6" Then
P_pick_up_start = P_pick_up_start -Z(0.2) 'Z-Achse - x mm
ElseIf a$ = "7" Then
P_pick_up_start = P_pick_up_start +U(1) 'U-Achse + x grad
ElseIf a$ = "8" Then
P_pick_up_start = P_pick_up_start -U(1) 'U-Achse - x grad
ElseIf a$ = "0" Then
GoTo L_end
Else
Print #24, "Wrong Input!"
EndIf
Cls #24
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 7
File: Main.prg
Last revision: 05.02.2018 11:18:47

Print #24, "actual position P_pick_up:"


Print #24, "x = ", CX(P_pick_up_start)
Print #24, "y = ", CY(P_pick_up_start)
Print #24, "z = ", CZ(P_pick_up_start)
Print #24, "u = ", CU(P_pick_up_start)
GoTo L_P_pick_up
ElseIf a$ = "2" Then
L_P_place: Cls #24
Print #24, "change position P_place"
Print #24, "1= x+0.2mm ", "2=x-0.2mm "
Print #24, "3= y+0.2mm ", "4=y-0.2mm "
Print #24, "5= z+0.2mm ", "6=z-0.2mm "
Print #24, "7= u+1grad ", "8=u-1grad "
Print #24, "0= quit"
Input #24, a$
If a$ = "1" Then
P_place = P_place +X(0.2) 'X-Achse + x mm
ElseIf a$ = "2" Then
P_place = P_place -X(0.2) 'X-Achse - x mm
ElseIf a$ = "3" Then
P_place = P_place +Y(0.2) 'Y-Achse + x mm
ElseIf a$ = "4" Then
P_place = P_place -Y(0.2) 'Y-Achse - x mm
ElseIf a$ = "5" Then
P_place = P_place +Z(0.2) 'Z-Achse + x mm
ElseIf a$ = "6" Then
P_place = P_place -Z(0.2) 'Z-Achse - x mm
ElseIf a$ = "7" Then
P_place = P_place +U(1) 'U-Achse + x grad
ElseIf a$ = "8" Then
P_place = P_place -U(1) 'U-Achse - x grad
ElseIf a$ = "0" Then
GoTo L_end
Else
Print #24, "Wrong Input!"
EndIf
Print #24, "actual position P_place:"
Print #24, "x = ", CX(P_place)
Print #24, "y = ", CY(P_place)
Print #24, "z = ", CZ(P_place)
Print #24, "u = ", CU(P_place)
GoTo L_P_place
ElseIf a$ = "3" Then
L_P_LimZ: Cls #24
Print #24, "change LimZ position "
Print #24, "1= LimZ+1mm ", "2= LimZ-1mm "
Print #24, "0= quit"
Input #24, a$
If a$ = "1" Then
LimZAxis = LimZAxis + 1 'LimZ + x mm
ElseIf a$ = "2" Then
LimZAxis = LimZAxis - 1 'LimZ - x mm
ElseIf a$ = "0" Then
GoTo L_end
Else
Print #24, "Wrong Input!"
EndIf
Print #24, "actual LimZ position"
Print #24, "LimZ = ", LimZAxis
GoTo L_P_LimZ
Else
Print #24, "Wrong Input!"
EndIf
ElseIf a$ = "2" Then
Cls #24
Print #24, "menu speed setup"
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 8
File: Main.prg
Last revision: 05.02.2018 11:18:47

Print #24, "1= change speed"


Print #24, "2= change acceleration"
Print #24, "3= change deceleration"
Input #24, a$
If a$ = "1" Then
L_speed: Cls #24
Print #24, "change speed"
Print #24, "1= speed + 1% ", "2= speed - 1%"
Input #24, a$
If a$ = "1" Then
Vel = Vel + 1
If Vel <= 1 Then Vel = 1
If Vel >= 100 Then Vel = 100
ElseIf a$ = "2" Then
Vel = Vel - 1
If Vel <= 1 Then Vel = 1
If Vel >= 100 Then Vel = 100
ElseIf a$ = "0" Then
GoTo L_end
Else
Print #24, "Wrong Input!"
EndIf
Print #24, "actual speed"
Print #24, "speed = ", Vel
GoTo L_speed
ElseIf a$ = "2" Then
L_acc: Cls #24
Print #24, "change acceleration"
Print #24, "1= Acc + 1% ", "2= Acc - 1%"
Input #24, a$
If a$ = "1" Then
Acc = Acc + 1
If Acc <= 1 Then Acc = 1
If Acc >= 100 Then Acc = 100
ElseIf a$ = "2" Then
Acc = Acc - 1
If Acc <= 1 Then Acc = 1
If Acc >= 100 Then Acc = 100
ElseIf a$ = "0" Then
GoTo L_end
Else
Print #24, "Wrong Input!"
EndIf
Print #24, "actual acceleration"
Print #24, "Acc = ", Acc
GoTo L_acc
ElseIf a$ = "3" Then
L_dec: Cls #24
Print #24, "change deceleration"
Print #24, "1= Dec + 1% ", "2= Dec - 1%"
Input #24, a$
If a$ = "1" Then
Dec = Dec + 1
If Dec <= 1 Then Dec = 1
If Dec >= 100 Then Dec = 100
ElseIf a$ = "2" Then
Dec = Dec - 1
If Dec <= 1 Then Dec = 1
If Dec >= 100 Then Dec = 100
ElseIf a$ = "0" Then
GoTo L_end
Else
Print #24, "Wrong Input!"
EndIf
Print #24, "actual deceleration"
Print #24, "Dec = ", Dec
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 9
File: Main.prg
Last revision: 05.02.2018 11:18:47

GoTo L_dec
Else
Print #24, "Wrong Input!"
EndIf
ElseIf a$ = "3" Then
Cls #24
Print #24, "menu timing setup"
Print #24, "1= change pickup time"
Print #24, "2= change place time"
Input #24, a$
If a$ = "1" Then
L_pickup: Cls #24
Print #24, "change pickup time"
Print #24, "1= pickup time + 0.01s "
Print #24, "2= pickup time - 0.01s "
Input #24, a$
If a$ = "1" Then
If PickUpTime < 1 Then 'Zeitgrenzen prüfen
PickUpTime = PickUpTime + 0.01
Else
PickUpTime = 1
Print #24, "T_pickup upper limit = 1 s !"
EndIf
ElseIf a$ = "2" Then
If PickUpTime < 0.1 Then 'Zeitgrenzen prüfen
PickUpTime = 0.05 'Aufnahmezeit nicht kleiner als 0.1 s zulassen
Print #24, "T_pickup lower limit = 0.05 s !"
Else
PickUpTime = PickUpTime - 0.01
EndIf
ElseIf a$ = "0" Then
GoTo L_end
Else
Print #24, "Wrong Input!"
EndIf
Print #24, "actual pickup time"
Print #24, "PickUpTime = ", PickUpTime
GoTo L_pickup
ElseIf a$ = "2" Then
L_place: Cls #24
Print #24, "change place time"
Print #24, "1= place time + 0.01s "
Print #24, "2= place time - 0.01s "
Input #24, a$
If a$ = "1" Then
If GiveOffTime < 1 Then 'Zeitgrenzen prüfen
GiveOffTime = GiveOffTime + 0.01
Else
GiveOffTime = 1
Print #24, "place time upper limit = 1 s !"
EndIf
ElseIf a$ = "2" Then
If GiveOffTime < 0.1 Then 'Zeitgrenzen prüfen
GiveOffTime = 0.05 'Aufnahmezeit nicht kleiner als 0.1 s zulassen
Print #24, "place time lower limit = 0.05 s !"
Else
GiveOffTime = GiveOffTime - 0.01
EndIf
ElseIf a$ = "0" Then
GoTo L_end
Else
Print #24, "Wrong Input!"
EndIf
Print #24, "actual place time"
Print #24, "GiveOffTime = ", GiveOffTime
GoTo L_place
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 10
File: Main.prg
Last revision: 05.02.2018 11:18:47

Else
Print #24, "Wrong Input!"
EndIf
ElseIf a$ = "4" Then
Cls #24
Print #24, "menu actual values"
Print #24, "1= display positions"
Print #24, "2= display speed, acc"
Print #24, "3= display timing"
Input #24, a$
If a$ = "1" Then
Cls #24
Print #24, "actual position P_pick_up:"
Print #24, "x = ", CX(P_pick_up_start), " mm"
Print #24, "y = ", CY(P_pick_up_start), " mm"
Print #24, "z = ", CZ(P_pick_up_start), " mm"
Print #24, "u = ", CU(P_pick_up_start), " grad"
Print #24, "actual position P_place:"
Print #24, "x = ", CX(P_place), " mm"
Print #24, "y = ", CY(P_place), " mm"
Print #24, "z = ", CZ(P_place), " mm"
Print #24, "u = ", CU(P_place), " grad"
ElseIf a$ = "2" Then
Cls #24
Print #24, "actual speed"
Print #24, "speed = ", Vel
Print #24, "acceleration = ", Acc, " %"
Print #24, "deceleration = ", Dec, " %"
ElseIf a$ = "3" Then
Cls #24
Print #24, "actual timing"
Print #24, "pickup time = ", PickUpTime, " s"
Print #24, "place time = ", GiveOffTime, " s"
Print #24, "cylce time = ", CycleTime, " s"
Else
Print #24, "Wrong Input!"
EndIf
Wait 5
ElseIf a$ = "5" Then
Print #24, "Save Points to File"
SavePoints "Points.pts"
ElseIf a$ = "6" Then
Cls #24
Print #24, "menu setup values"
Print #24, "1= speed"
Print #24, "2= acceleration"
Print #24, "3= deceleration"
Print #24, "4= LimZ"
Input #24, a$
If a$ = "1" Then
Cls #24
Print #24, "set speed [0,100]"
Input #24, a
If a >= 100 Or a <= 0 Then
Print #24, "Wrong Input!"
Else
Vel = a
EndIf
ElseIf a$ = "2" Then
Cls #24
Print #24, "set acceleration [0,100]"
Input #24, a
If a >= 100 Or a <= 0 Then
Print #24, "Wrong Input!"
Else
Acc = a
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 11
File: Main.prg
Last revision: 05.02.2018 11:18:47

EndIf
ElseIf a$ = "3" Then
Cls #24
Print #24, "set deceleration [0,100]"
Input #24, a
If a >= 100 Or a <= 0 Then
Print #24, "Wrong Input!"
Else
Dec = a
EndIf
ElseIf a$ = "4" Then
Cls #24
Print #24, "set LimZ [-180mm,0mm]"
Input #24, a
If a >= 0 Or a <= -180 Then
Print #24, "Wrong Input!"
Else
LimZAxis = a
EndIf
Else
Print #24, "Wrong Input!"
EndIf
Else
Print #24, "Wrong Input!"
EndIf
L_end: a$ = ""

Loop
Fend
Function main_Communication_TCPIP ' communication via Ethnernet

String data$
OpenNet #201 As Client

WaitNet #201
Input #201, Vel, Acc, Dec, TrackingVelocity, PickUpTime, GiveOffTime, GiveOffTime2, Z_up, Z_d
' add at end of last code line ...,
' ..... to be added P_pick_up_X, P_pick_up_Y, P_pick_up_Z, P_pick_up_U, P_place_start_X, P_pl
' ..... missing is control-word(s) (containing inputs like on/off switch, vacuum on manual) a
Print "received data from host 1"
CloseNet #201
' Wait 1

' data needs to be checked for boundaries and consistency by CRC-check or similar !!!

' add check for limits if necessary (but is already done in panel of plc side)
' this task will be executed only in automatic mode of robot -> teaching of whole points
' Real Time1, Time2, Time3
'
' Do
' ' fieldbus inputs
' Vel = InW(I_V) ' read speed 0...100% (integer)
' Acc = InW(I_A) ' read acceleration 0...120% (integer)
' Dec = InW(I_A) ' read deceleration 0...120% (integer)
' TrackingVelocity = InW(I_V_tracking) ' read tracking speed 0...400 mm/s (301 mm/s
' TrackingAngle = InW(I_S_tracking) ' read tracking angle 0...45 °
' Time1 = InW(I_T_pick_up) ' read pick up time and make conversion to
' PickUpTime = Time1 /100
' Time2 = InW(I_T_place) ' read give_off time and make conversion
' GiveOffTime = Time2 /100
' Time3 = InW(I_T_throw_off) ' read give_off 2 time and make conversio
' GiveOffTime2 = Time3 /100
' Z_up = InW(I_Z_lift_up) / 10 ' read z_lift_up in mm and make conversion (
' Z_down = InW(I_Z_lift_down) / 10 ' read z_lift_down in mm and make conversion
' LimZAxis = -(65536 - InW(I_Z_limit)) / 10 ' negative value
'
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 12
File: Main.prg
Last revision: 05.02.2018 11:18:47

' TEST SIMULATIOM -> wieder löschen


' Vel = 100
' Acc = 100
' Dec = 100
' TrackingVelocity = 100
' TrackingAngle = 8
' PickUpTime = 1
' GiveOffTime = 1
' GiveOffTime2 = 1
' Z_up = 20
' Z_down = 30
' LimZAxis = -40
' ENDE TEST

' multiplexed fieldbus inputs


' If Sw(I_LoadPar) Then ' if PLC sends data
' Select InW(I_Par_Nr) ' !!! reads parameter number
'
' Case 11 ' reads parameter P0.x coordinate
' CX(P_pick_up) = InReal(I_Par_Data)
'
' Case 12 ' reads parameter P0.y coordinate
' CY(P_pick_up) = InReal(I_Par_Data)
'
' Case 13 ' reads parameter P0.z coordinate
' CZ(P_pick_up) = InReal(I_Par_Data)
'
' Case 14 ' reads parameter P0.u coordinate
' CU(P_pick_up) = InReal(I_Par_Data)
'
' Case 21 ' reads parameter P1.x coordinate
' CX(P_place_start) = InReal(I_Par_Data)
'
' Case 22 ' reads parameter P1.y coordinate
' CY(P_place_start) = InReal(I_Par_Data)
'
' Case 23 ' reads parameter P1.z coordinate
' CZ(P_place_start) = InReal(I_Par_Data)
'
' Case 24 ' reads parameter P1.u coordinate
' CU(P_place_start) = InReal(I_Par_Data)
'
' Case 31 ' reads parameter P2.x coordinate
' CX(P_base_pos) = InReal(I_Par_Data)
'
' Case 32 ' reads parameter P2.y coordinate
' CY(P_base_pos) = InReal(I_Par_Data)
'
' Case 33 ' reads parameter P2.z coordinate
' CZ(P_base_pos) = InReal(I_Par_Data)
'
' Case 34 ' reads parameter P2.u coordinate
' CU(P_base_pos) = InReal(I_Par_Data)
'
' Default
'
' On Q_ReadReady, 0.1 ' robot is ready to plc
' ' SavePoints "Points.pts" -> evtl. noch Punk
' Send
'
' EndIf
'
' ' fieldbus output -> reading of position in run-mode can cause error 2200 -> uncomme
' OutReal Q_x, CX(Here) ' sends actual x-value (real) (is only updated by robo
' OutReal Q_y, CY(Here) ' sends actual y-value (real)
' OutReal Q_z, CZ(Here) ' sends actual z-value (real)
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 13
File: Main.prg
Last revision: 05.02.2018 11:18:47

' OutReal Q_u, CU(Here) ' sends actual u-value (real)


' OutW Q_v_ist, Vel ' sends actual speed (int)
' OutW Q_a_ist, Acc ' sends actual acceleration (int)
' Wait 0.5 ' do every 0.5 s (delay had to be added otherwise EPSON
'
' Loop
Fend
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1
File: Comm.prg
Last revision: 26.01.2012 13:23:36
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1
File: Main1.prg
Last revision: 05.02.2018 10:11:40

'---------- TEACH - PROGRAM -----------------------------------------------------------------


' Project: Culletprima Indonesia Tangerang
' robot with fieldbus interface Profibus Adr. 10
' Date: 11.01.2018
' Author: M. Stark
' Company: WALTEC Germany
'--------------------------------------------------------------------------------------------

'!!!!! Achtung noch nicht durchprojektiert !!!!!!!!!!

Real Temp

Function main1 ' main1 function: start of teach program which is selected by fieldbus inpu

Power Low ' set power of robot low


Motor Off ' switch motor Off -> robots points will be teached manu
' if operator wants to teach with TP then do not switch m

Do
' fieldbus output
OutReal Q_x, CX(Here) ' actual x-positon of robot
OutReal Q_y, CY(Here) ' actual y-positon of robot
OutReal Q_z, CZ(Here) ' actual z-positon of robot
OutReal Q_u, CU(Here) ' actual u-positon of robot
OutW Q_v_ist, Vel ' actual speed
OutW Q_a_ist, Acc ' actual acceleration
Wait 0.5 ' delay to prevent EPSON controller error

' switch on / off (softkeys on robot page panel) of robot pneumatic valves
' pneumatic valve turning
' blow_out
If Sw(I_Blow_out) Then
On Q_blow_out
' send state of outputs to plc
On Q_BlowOut_On
Else
Off Q_blow_out
' send state of outputs to plc
Off Q_BlowOut_On
EndIf
' vacuum
If Sw(I_Vacuum_On) Then
On Q_vacuum
' send state of outputs to plc
On Q_Vacuum_On
Else
Off Q_vacuum
' send state of outputs to plc
Off Q_Vacuum_On
EndIf
' switch off of vacuum and blow_out in emergency_stop state
If Oport(Q_EStopOn) = 1 Then
Off Q_vacuum
Off Q_blow_out
EndIf

' fieldbus input (used for Teaching with fieldbus)


If Sw(I_LoadPar) Then ' if plc is sending
Select InW(I_Par_Nr) ' read parameter number

Case 10 ' loads P0 (pick_up)


P_pick_up = Here ' aktuelle Position Punkt 0 (Aufnahme) zuordnen
SavePoints "Points.pts" ' Punktedatei abspeichern im Flash-Spe
Wait 2
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 2
File: Main1.prg
Last revision: 05.02.2018 10:11:40

Case 20 ' loads P1 (place)


P_place = Here ' aktuelle Position Punkt 1 (Ofen) zuordnen
SavePoints "Points.pts" ' Punktedatei abspeichern im Flash-Spe
Wait 2

Case 30 ' loads P2 (base or throw_off)


P_base_pos = Here ' aktuelle Position Punkt 2 (Härteanlage) zuordn
SavePoints "Points.pts" ' Punktedatei abspeichern im Flash-Spe
Wait 2

Default

On Q_ReadReady, 0.1 ' Quittung Robot hat gelesen


Send
EndIf

Loop

Fend
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1
File: Main2.prg
Last revision: 27.12.2013 15:16:35

' Torquecontrol
' evtl. benutzen um bei Aufnahme (Herabfahren der Z-Achse und Kontakt mit Artikel) Abschalten

'Speed 5
'Go ApproachPoint

'Set the Z axis torque limit to 20%


'TCLim -1, -1, 20, -1
'Set the speed in torque control to 5%
'TcSpeed 5

'TC On
'Go ContactPoint
'Wait 3
'Go ApproachPoint
'TC Off
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1
File: Points.pts
Last revision: 27.12.2013 15:15:16

Point: 0, P_pick_up_start, Teachpunkt Aufnahmeposition Beginn (3 Punkte definieren Kr


Coordinates: X: -346.169 Y: -51.645 Z: -136.998 U: 188.705 V: 0.000 W: 0.000
Attributes: Local:0 Hand:Righty Elbow:Above Wrist:NoFlip J4Flag:0 J6Flag:0

Point: 1, P_base_pos, Teachpunkt über Aufnahmeposition = Grundstellung


Coordinates: X: -346.169 Y: -51.645 Z: -130.903 U: 188.705 V: 0.000 W: 0.000
Attributes: Local:0 Hand:Righty Elbow:Above Wrist:NoFlip J4Flag:0 J6Flag:0

Point: 2, P_place, Teachpunkt über Wender


Coordinates: X: -650.000 Y: 0.000 Z: -84.000 U: 180.000 V: 0.000 W: 0.000
Attributes: Local:0 Hand:Righty Elbow:Above Wrist:NoFlip J4Flag:0 J6Flag:0

Point: 3, P_pick_up_middle, Teachpunkt Aufnahmeposition Mitte


Coordinates: X: -349.332 Y: -21.612 Z: -136.998 U: 183.540 V: 0.000 W: 0.000
Attributes: Local:0 Hand:Righty Elbow:Above Wrist:NoFlip J4Flag:0 J6Flag:0

Point: 4, P_pick_up_end, Teachpunkt Aufnahmeposition Ende


Coordinates: X: -350.000 Y: 0.000 Z: -136.998 U: 180.000 V: 0.000 W: 0.000
Attributes: Local:0 Hand:Righty Elbow:Above Wrist:NoFlip J4Flag:0 J6Flag:0

Point: 5, P_throw_off, Teachpunkt über Abwurfposition


Coordinates: X: -500.000 Y: 0.000 Z: -136.998 U: 180.000 V: 0.000 W: 0.000
Attributes: Local:0 Hand:Righty Elbow:Above Wrist:NoFlip J4Flag:0 J6Flag:0
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1

Input / Output Labels

Input Bit Label Description

0 I_start_2 Start wenn Profibus ausfällt

10 I_vacuum_or_grip Vorwahlschalter Vakuum oder Greifer

11 I_monitor_clima Klimagerätüberwachung

12 I_vaccum_on Option Vakuum manuell einschalten

13 I_monitor_air Druckluftüberwachung

14 I_monitor_vacuum Option Vakuumüberwachung

15 I_monitor_cool Kühlwasserüberwachung

512 I_Start

513 I_SelProg1

514 I_SelProg2

515 I_SelProg4

516 I_Stop

517 I_Pause

518 I_Continue

519 I_Reset

520 I_SetMotorOn

521 I_SetMotorOff

522 I_Home

523 I_Throw_Off Abwerfen

525 I_Blow_out Manuell: Ausblasen ein/aus

526 I_Vacuum_On Manuell: Vakuum ein/aus

528 I_Preselection Vorwahl Robot

531 I_Mode Betriebsart: Pick and Place (0) oder Tracking (1)

542 I_PLCReadReady SPS hat Parameter gelesen

543 I_LoadPar SPS will Parmaeter lesen


Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1

Input / Output Labels

Input Word Label Description

32 16 Steuerbits

33 16 weitere Steuerbits

34 I_Par_Nr Zeiger auf Parameter INT

35 I_Par_Data Parameterdaten die von SPS an Robot übergeben werden

37 I_Z_lift_down Abgabe

38 I_Z_lift_up Aufnahme

39 I_Z_limit obere Grenze der z-Achse INT für Jump-Befehl

40 I_S_tracking Trackingwinkel oder -weg

41 I_V_tracking Trackinggeschwindigkeit = Umlaufgeschwindigkeit der VM

43 I_V Robotgeschwindigkeit

44 I_A Robotbeschleunigung

45 I_T_pick_up Aufnahmezeit

46 I_T_place Abgabezeit

47 I_T_throw_off Abwurfzeit
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1

Input / Output Labels

Output Bit Label Description

10 Q_start_turner Option Startimpuls Wender

11 Q_gripper Ventil Greifer

12 Q_blow_out Ventil Ausblasen

13 Q_vacuum Ventil Vakuum 1 (Pumpe)

14 Q_vacuum_2 Ventil Vakuum 2 Aus (Zyklus) -> evtl. inverses Ventil

15 Q_fault Fehlerausgang -> kann auch als optische Anzeige genutzt werden

512 Q_Ready

513 Q_Running

514 Q_Paused

515 Q_Error

516 Q_EStopOn

517 Q_SafeguardON

518 Q_SError

519 Q_Warning

520 Q_MotorsOn

521 Q_AtHome

522 Q_CurrProg1

523 Q_CurrProg2

524 Q_CurrProg4

525 Q_AutoMode

526 Q_TeachMode

527 Q_PowerHigh

528 Q_ErrorCode1

529 Q_ErrorCode2

530 Q_ErrorCode4

531 Q_ErrorCode8

532 Q_ErrorCode16

533 Q_ErrorCode32

534 Q_ErrorCode64

535 Q_ErrorCode128

536 Q_ErrorCode256

537 Q_ErrorCode512
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 2

Input / Output Labels

Output Bit Label Description

538 Q_ErrorCode1024

539 Q_ErrorCode2048

540 Q_ErrorCode4096

541 Q_ErrorCode8192

542 Q_InBox1

543 Q_InBox2

704 Q_SendPar Robot sendet Daten

705 Q_ReadReady Robot hat Daten gelesen -> evtl. noch CRC-check

711 Q_Clima_OK Kühlgerätüberwachung

712 Q_Start_Turning Startsignal für Wender an SPS

713 Q_Acknowledge Quittierungssignal an SPS (Zykluszeitmessung)

714 Q_CoolingWater Kühlwasserstatus an SPS melden

715 Q_Air_OK Druckluftstatus an SPS melden

716 Q_Vacuum_OK Vacuumstatus an SPS melden

717 Q_BlowOut_On Blowoutventilstatus an SPS melden

718 Q_Vacuum_On Vacuumventilstatus an SPS melden

719 Q_Enable Freigabe an externe Maschine


Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1

Input / Output Labels

Output Word Label Description

32 16 Statusbits ...

33 16 weitere Statusbits ...

34 Q_x X Istposition im Real-Format (WKS)

35 X

36 Q_y Y Istposition im Real-Format

37 Y

38 Q_z Z Istposition im Real-Format

39 Z

40 Q_u U Istposition im Real-Format

41 U

42 Q_v_ist V Istgeschwindigkeit INT-Format [0..100%]

43 Q_a_ist A Istbeschleunigung INT-Format [0..100%]

44 16 weitere Statusbits

45 Q_Par_Nr Parameterzeiger INT

46 Q_Par_Data Parameterdaten im beleibigen Format die von Robot an SPS übergeb

47 Parameterdaten
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1

Error # Label Message


8000 F_Par_Not_Valid parameter not valid -> check parameter range of type
8001 F_No_Watercoolin water cooling of robot is not working -> check water flow or flow m
8002 F_MonitoringAir air monitoring -> compressed air pressure too low or pressure switc
8003 F_MonitoringVac vacuum monitoring -> indicates that article could not be taken by r
8004 F_MonitoringClim climatisation monitoring of robot cabinet -> indicates failure of c