Hey there,
I use this below code from kuka_experimental (ROS).
Now I want to extend it to also work in automatic (Currently I can only start in T1)
I tried already to insert some teached in positions (it did not help)
Problem
when I switch from T1 to Automatic and press and hold the start key for a long time, nothing happens and on the touch panel it says press start key. submit interpreter is not used but if it is selected nothing changes. Program is selected.
I think the problem is not, that I did start it wrong etc.(did not enable drives) it has to do something with the programm itself. E.g. for automatic mode it is required to have a real point teached in or something like this.
Robot Details
KUKA KR30 (HA), KRC4, KSS: 8.3.30
Code
&ACCESS RVO
def kuka_eki_hw_interface()
; Utilized system resources:
; Flags:
; $flag[1]: Indicates active client connection
; $timer_flag[1]: Used to trigger periodic send of robot state
; Interrupts:
; 15: Calls eki_hw_iface_reset() on falling edge of $flag[1]
; 16: Calls eki_hw_iface_send() on rising edge of $timer_flag[1]
eki_hw_iface_init()
; BCO (Block COincidence) run to current position
; (requied for below loop continue before first incoming command)
joint_pos_tgt = $axis_act_meas
ptp joint_pos_tgt
; Loop forever
$advance = 5
loop
elements_read = eki_hw_iface_get(joint_pos_tgt) ; Get new command from buffer if present
ptp joint_pos_tgt c_ptp ; PTP to most recent commanded position
endloop
; Note: EKI channels delete on reset or deselect of this program
; See <ENVIRONMENT>Program</ENVIRONMENT> EKI config element
end
def eki_hw_iface_init()
decl eki_status eki_ret
; Setup interrupts
; Interrupt 15 - Connection cleanup on client disconnect
global interrupt decl 15 when $flag[1]==false do eki_hw_iface_reset()
interrupt on 15
; Interrupt 16 - Timer interrupt for periodic state transmission
global interrupt decl 16 when $timer_flag[1]==true do eki_hw_iface_send()
interrupt on 16
wait sec 0.012 ; Wait for next interpolation cycle
$timer[1] = -200 ; Time in [ms] before first interrupt call
$timer_stop[1] = false ; Start timer 1
; Create and open EKI interface
eki_ret = eki_init("EkiHwInterface")
eki_ret = eki_open("EkiHwInterface")
end
def eki_hw_iface_send()
decl eki_status eki_ret
decl real vel_percent
if $flag[1] then ; If connection alive
; Load state values into xml structure
; position
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A1", $axis_act_meas.a1)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A2", $axis_act_meas.a2)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A3", $axis_act_meas.a3)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A4", $axis_act_meas.a4)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A5", $axis_act_meas.a5)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A6", $axis_act_meas.a6)
; velocity
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A1", $vel_axis_act[1] * $vel_axis_ma[1] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A2", $vel_axis_act[2] * $vel_axis_ma[2] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A3", $vel_axis_act[3] * $vel_axis_ma[3] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A4", $vel_axis_act[4] * $vel_axis_ma[4] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A5", $vel_axis_act[5] * $vel_axis_ma[5] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A6", $vel_axis_act[6] * $vel_axis_ma[6] / 100.0)
; effort
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A1", $torque_axis_act[1])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A2", $torque_axis_act[2])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A3", $torque_axis_act[3])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A4", $torque_axis_act[4])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A5", $torque_axis_act[5])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A6", $torque_axis_act[6])
; interface state
eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1")
eki_ret = eki_setint("EkiHwInterface", "RobotState/RobotCommand/@Size", eki_ret.buff)
; Send xml structure
if $flag[1] then ; Make sure connection hasn't died while updating xml structure
eki_ret = eki_send("EkiHwInterface", "RobotState")
endif
endif
; Set timer for next interrupt [ms]
$timer[1] = -10 ; ~10 ms for above send + 10 ms interrupt timer -> ~50 Hz state transmission
end
deffct int eki_hw_iface_available()
decl eki_status eki_ret
if not $flag[1] then
return 0
endif
eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1")
return eki_ret.buff
endfct
; eki_hw_iface_get
; Tries to read most recent elemnt from buffer. q left unchanged if empty.
; Returns number of elements read.
deffct int eki_hw_iface_get(joint_pos_cmd :out)
decl eki_status eki_ret
decl axis joint_pos_cmd
if not $flag[1] then
return 0
endif
eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1")
if eki_ret.buff <= 0 then
return 0
endif
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A1", joint_pos_cmd.a1)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A2", joint_pos_cmd.a2)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A3", joint_pos_cmd.a3)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A4", joint_pos_cmd.a4)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A5", joint_pos_cmd.a5)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A6", joint_pos_cmd.a6)
return 1
endfct
def eki_hw_iface_reset()
decl eki_status eki_ret
eki_ret = eki_clear("EkiHwInterface")
eki_ret = eki_init("EkiHwInterface")
eki_ret = eki_open("EkiHwInterface")
end
Display More