Professional Documents
Culture Documents
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
Real Temp
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
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 !
Halt main_UserInterface
Halt main_Communication ' to avoid error 2200
Resume main_UserInterface
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
In_Home_Pos = False
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
Exit Function
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
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
Default
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
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"
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
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
Real Temp
Function main1 ' main1 function: start of teach program which is selected by fieldbus inpu
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
Default
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
'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
11 I_monitor_clima Klimagerätüberwachung
13 I_monitor_air Druckluftü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
531 I_Mode Betriebsart: Pick and Place (0) oder Tracking (1)
32 16 Steuerbits
33 16 weitere Steuerbits
37 I_Z_lift_down Abgabe
38 I_Z_lift_up Aufnahme
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
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
538 Q_ErrorCode1024
539 Q_ErrorCode2048
540 Q_ErrorCode4096
541 Q_ErrorCode8192
542 Q_InBox1
543 Q_InBox2
705 Q_ReadReady Robot hat Daten gelesen -> evtl. noch CRC-check
32 16 Statusbits ...
35 X
37 Y
39 Z
41 U
44 16 weitere Statusbits
47 Parameterdaten
Project: Culletprima_Robot_2 EPSON RC+ v5.4.3 SP1 Page 1