Robot Linear Tracking Profibus

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

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
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 !
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 !
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

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
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)

CX(P_place_start) = InReal(I_Par_Data)

CY(P_place_start) = InReal(I_Par_Data)

CZ(P_place_start) = InReal(I_Par_Data)

CU(P_place_start) = InReal(I_Par_Data)

CX(P_base_pos) = InReal(I_Par_Data)

CY(P_base_pos) = InReal(I_Par_Data)

CZ(P_base_pos) = InReal(I_Par_Data)

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
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
P_pick_up_start = P_pick_up_start +Y(0.2) 'Y-Achse + x mm
P_pick_up_start = P_pick_up_start -Y(0.2) 'Y-Achse - x mm
P_pick_up_start = P_pick_up_start +Z(0.2) 'Z-Achse + x mm
P_pick_up_start = P_pick_up_start -Z(0.2) 'Z-Achse - x mm
P_pick_up_start = P_pick_up_start +U(1) 'U-Achse + x grad
P_pick_up_start = P_pick_up_start -U(1) 'U-Achse - x grad
GoTo L_end
Else
Print #24, "Wrong Input!"
EndIf
Cls #24
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
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
P_place = P_place -X(0.2) 'X-Achse - x mm
P_place = P_place +Y(0.2) 'Y-Achse + x mm
P_place = P_place -Y(0.2) 'Y-Achse - x mm
P_place = P_place +Z(0.2) 'Z-Achse + x mm
P_place = P_place -Z(0.2) 'Z-Achse - x mm
P_place = P_place +U(1) 'U-Achse + x grad
P_place = P_place -U(1) 'U-Achse - x grad
GoTo L_end
Else
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
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
LimZAxis = LimZAxis - 1 'LimZ - x mm
GoTo L_end
Else
EndIf
Print #24, "actual LimZ position"
Print #24, "LimZ = ", LimZAxis
GoTo L_P_LimZ
Else
EndIf
Cls #24
Print #24, "menu speed setup"
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
Vel = Vel - 1
If Vel <= 1 Then Vel = 1
If Vel >= 100 Then Vel = 100
GoTo L_end
Else
EndIf
Print #24, "actual speed"
Print #24, "speed = ", Vel
GoTo L_speed
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
Acc = Acc - 1
If Acc <= 1 Then Acc = 1
If Acc >= 100 Then Acc = 100
GoTo L_end
Else
EndIf
Print #24, "actual acceleration"
Print #24, "Acc = ", Acc
GoTo L_acc
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
Dec = Dec - 1
If Dec <= 1 Then Dec = 1
If Dec >= 100 Then Dec = 100
GoTo L_end
Else
EndIf
Print #24, "actual deceleration"
Print #24, "Dec = ", Dec
File: Main.prg
Last revision: 05.02.2018 11:18:47
GoTo L_dec
Else
EndIf
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
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
GoTo L_end
Else
EndIf
Print #24, "actual pickup time"
Print #24, "PickUpTime = ", PickUpTime
GoTo L_pickup
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
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
GoTo L_end
Else
EndIf
Print #24, "actual place time"
Print #24, "GiveOffTime = ", GiveOffTime
GoTo L_place
File: Main.prg
Last revision: 05.02.2018 11:18:47
Else
EndIf
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"
Cls #24
Print #24, "actual speed"
Print #24, "speed = ", Vel
Print #24, "acceleration = ", Acc, " %"
Print #24, "deceleration = ", Dec, " %"
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
EndIf
Wait 5
Print #24, "Save Points to File"
SavePoints "Points.pts"
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
Else
Vel = a
EndIf
Cls #24
Print #24, "set acceleration [0,100]"
Input #24, a
If a >= 100 Or a <= 0 Then
Else
Acc = a
File: Main.prg
Last revision: 05.02.2018 11:18:47
EndIf
Cls #24
Print #24, "set deceleration [0,100]"
Input #24, a
If a >= 100 Or a <= 0 Then
Else
Dec = a
EndIf
Cls #24
Print #24, "set LimZ [-180mm,0mm]"
Input #24, a
If a >= 0 Or a <= -180 Then
Else
LimZAxis = a
EndIf
Else
EndIf
Else
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
'
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)
'
' CX(P_place_start) = InReal(I_Par_Data)
'
' CY(P_place_start) = InReal(I_Par_Data)
'
' CZ(P_place_start) = InReal(I_Par_Data)
'
' CU(P_place_start) = InReal(I_Par_Data)
'
' CX(P_base_pos) = InReal(I_Par_Data)
'
' CY(P_base_pos) = InReal(I_Par_Data)
'
' CZ(P_base_pos) = InReal(I_Par_Data)
'
' 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)
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
File: Comm.prg
Last revision: 26.01.2012 13:23:36
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
Off Q_BlowOut_On
EndIf
' vacuum
If Sw(I_Vacuum_On) Then
On Q_vacuum
On Q_Vacuum_On
Else
Off Q_vacuum
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
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
Wait 2
Case 30 ' loads P2 (base or throw_off)

P_base_pos = Here ' aktuelle Position Punkt 2 (Härteanlage) zuordn
Wait 2
Default
On Q_ReadReady, 0.1 ' Quittung Robot hat gelesen

Send
EndIf
Loop
Fend
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
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

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
Point: 3, P_pick_up_middle, Teachpunkt Aufnahmeposition Mitte

Point: 4, P_pick_up_end, Teachpunkt Aufnahmeposition Ende

Point: 5, P_throw_off, Teachpunkt über Abwurfposition

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

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

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

Robot Linear Tracking Profibus

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robot Linear Tracking Profibus

Uploaded by

Copyright:

Available Formats

Project: Culletprima_Robot_2 EPSON RC+ v5.4.

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

Global Real CycleTime

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

Integer u, errNum ' local variables

Motor On ' Switch On motor power

Acc = 50 ' presetting of parameters

Do ' endless do…loop

TmReset 0 ' timer reset for cycle time measurement

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

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

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

Resume main_Communication ' to avoid error 2200 (not tested)

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

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

' input state of watercooling

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

' input state of watercooling

' input state of compressed air

' input state of vacuum monitoring

' output state of pneumatic valves blow out

' output state of pneumatic valves vacuum

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

' TEST SIMULATIOM -> wieder löschen

' multiplexed fieldbus inputs

Case 11 ' reads parameter P0.x coordinate

Case 12 ' reads parameter P0.y coordinate

Case 13 ' reads parameter P0.z coordinate

Case 14 ' reads parameter P0.u coordinate

Case 21 ' reads parameter P1.x coordinate

Case 22 ' reads parameter P1.y coordinate

Case 23 ' reads parameter P1.z coordinate

Case 24 ' reads parameter P1.u coordinate

Case 31 ' reads parameter P2.x coordinate

Case 32 ' reads parameter P2.y coordinate

Case 33 ' reads parameter P2.z coordinate

Case 34 ' reads parameter P2.u coordinate

On Q_ReadReady, 0.1 ' robot is ready to plc

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

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

Print #24, "actual position P_pick_up:"

Print #24, "1= change speed"

' TEST SIMULATIOM -> wieder löschen

' multiplexed fieldbus inputs

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

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

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

Power Low ' set power of robot low

' fieldbus input (used for Teaching with fieldbus)

Case 10 ' loads P0 (pick_up)

Case 20 ' loads P1 (place)

Case 30 ' loads P2 (base or throw_off)

On Q_ReadReady, 0.1 ' Quittung Robot hat gelesen

'Set the Z axis torque limit to 20%

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

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

Point: 2, P_place, Teachpunkt über Wender

Point: 3, P_pick_up_middle, Teachpunkt Aufnahmeposition Mitte

Point: 4, P_pick_up_end, Teachpunkt Aufnahmeposition Ende

Point: 5, P_throw_off, Teachpunkt über Abwurfposition

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