Thank you all very much for the support. The drives use the CiA402 protocol, with the jc2ktr setup I was able to see the drives and I can perform jogging, but I don't understand why I see Z1 on joint 0, which should be X. And when I try to jog joint 1, joint 0 always moves. I definitely made a lot of confusion in the HAL file.
Maybe your Hardware (ethercat ports, or motorcables) are wired wrong, your halfile looks okay to me?
I´ve had at work that if you swap in and out of the Ethercat ports you get weird behavior on the EC-Bus.
Thank you so much, now looks good.
Can someone help me with "setp cia402.X.pos-scale 3600" value?
I've this type of motor with the same encoder: EMMB-ST-87-M-SM (X), EMMB-ST-87-M-SMB (Z1,Z2,Y1,Y2).
Z1, Z2 5mm/rev; X, Y1, Y2 180mm/rev.
Many problems have been solved by assigning numbers instead of the axis letter (e.g. X) in the .hal file probably generated errors as there were two motors for one axis.
Code:
# --- load components ---
loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
# load EtherCAT xml then drivers
loadusr -W lcec_conf ethercat-conf.xml
loadrt lcec
loadrt cia402 count=5
# --- add functions to servo-thread ---
addf lcec.read-all servo-thread
# read from cia402 drivers (one per joint)
addf cia402.0.read-all servo-thread
addf cia402.1.read-all servo-thread
addf cia402.2.read-all servo-thread
addf cia402.3.read-all servo-thread
addf cia402.4.read-all servo-thread
# motion stack
addf motion-command-handler servo-thread
addf motion-controller servo-thread
# writes: cia402 then lcec
addf cia402.0.write-all servo-thread
addf cia402.1.write-all servo-thread
addf cia402.2.write-all servo-thread
addf cia402.3.write-all servo-thread
addf cia402.4.write-all servo-thread
addf lcec.write-all servo-thread
# estop
net estop-out iocontrol.0.user-enable-out => iocontrol.0.emc-enable-in
# =========================
# JOINT 0 (X) -> cia402.0 -> lcec.0.0
# =========================
#config
setp cia402.0.csp-mode 1
setp cia402.0.pos-scale -3600
#from servo(ethercat) to cia402
net 0-statusword lcec.0.0.srv-cia-statusword => cia402.0.statusword
net 0-opmode-display lcec.0.0.srv-opmode-display => cia402.0.opmode-display
net 0-drv-act-pos lcec.0.0.srv-actual-position => cia402.0.drv-actual-position
net 0-drv-act-velo lcec.0.0.srv-actual-velocity => cia402.0.drv-actual-velocity
#from cia402 to servo(ethercat)
net 0-controlword cia402.0.controlword => lcec.0.0.srv-cia-controlword
net 0-modes-of-operation cia402.0.opmode => lcec.0.0.srv-opmode
net 0-drv-target-pos cia402.0.drv-target-position => lcec.0.0.srv-target-position
#from motion to cia
net 0-enable <= joint.0.amp-enable-out => cia402.0.enable
net 0-amp-fault => joint.0.amp-fault-in <= cia402.0.drv-fault
net 0-pos-cmd <= joint.0.motor-pos-cmd => cia402.0.pos-cmd
net 0-pos-fb => joint.0.motor-pos-fb <= cia402.0.pos-fb
# =========================
# JOINT 1 (Y1) -> cia402.1 -> lcec.0.2
# =========================
#config
setp cia402.1.csp-mode 1
setp cia402.1.pos-scale -3600
#from servo(ethercat) to cia402
net 1-statusword lcec.0.2.srv-cia-statusword => cia402.1.statusword
net 1-opmode-display lcec.0.2.srv-opmode-display => cia402.1.opmode-display
net 1-drv-act-pos lcec.0.2.srv-actual-position => cia402.1.drv-actual-position
net 1-drv-act-velo lcec.0.2.srv-actual-velocity => cia402.1.drv-actual-velocity
#from cia402 to servo(ethercat)
net 1-controlword cia402.1.controlword => lcec.0.2.srv-cia-controlword
net 1-modes-of-operation cia402.1.opmode => lcec.0.2.srv-opmode
net 1-drv-target-pos cia402.1.drv-target-position => lcec.0.2.srv-target-position
#from motion to cia
net 1-enable <= joint.1.amp-enable-out => cia402.1.enable
net 1-amp-fault => joint.1.amp-fault-in <= cia402.1.drv-fault
net 1-pos-cmd <= joint.1.motor-pos-cmd => cia402.1.pos-cmd
net 1-pos-fb => joint.1.motor-pos-fb <= cia402.1.pos-fb
# =========================
# JOINT 2 (Y2) -> cia402.2 -> lcec.0.1
# =========================
#config
setp cia402.2.csp-mode 1
setp cia402.2.pos-scale 3600
#from servo(ethercat) to cia402
net 2-statusword lcec.0.1.srv-cia-statusword => cia402.2.statusword
net 2-opmode-display lcec.0.1.srv-opmode-display => cia402.2.opmode-display
net 2-drv-act-pos lcec.0.1.srv-actual-position => cia402.2.drv-actual-position
net 2-drv-act-velo lcec.0.1.srv-actual-velocity => cia402.2.drv-actual-velocity
#from cia402 to servo(ethercat)
net 2-controlword cia402.2.controlword => lcec.0.1.srv-cia-controlword
net 2-modes-of-operation cia402.2.opmode => lcec.0.1.srv-opmode
net 2-drv-target-pos cia402.2.drv-target-position => lcec.0.1.srv-target-position
#from motion to cia
net 2-enable <= joint.2.amp-enable-out => cia402.2.enable
net 2-amp-fault => joint.2.amp-fault-in <= cia402.2.drv-fault
net 2-pos-cmd <= joint.2.motor-pos-cmd => cia402.2.pos-cmd
net 2-pos-fb => joint.2.motor-pos-fb <= cia402.2.pos-fb
# =========================
# JOINT 3 (Z1) -> cia402.3 -> lcec.0.4
# =========================
#config
setp cia402.3.csp-mode 1
setp cia402.3.pos-scale 3600
#from servo(ethercat) to cia402
net 3-statusword lcec.0.4.srv-cia-statusword => cia402.3.statusword
net 3-opmode-display lcec.0.4.srv-opmode-display => cia402.3.opmode-display
net 3-drv-act-pos lcec.0.4.srv-actual-position => cia402.3.drv-actual-position
net 3-drv-act-velo lcec.0.4.srv-actual-velocity => cia402.3.drv-actual-velocity
#from cia402 to servo(ethercat)
net 3-controlword cia402.3.controlword => lcec.0.4.srv-cia-controlword
net 3-modes-of-operation cia402.3.opmode => lcec.0.4.srv-opmode
net 3-drv-target-pos cia402.3.drv-target-position => lcec.0.4.srv-target-position
#from motion to cia
net 3-enable <= joint.3.amp-enable-out => cia402.3.enable
net 3-amp-fault => joint.3.amp-fault-in <= cia402.3.drv-fault
net 3-pos-cmd <= joint.3.motor-pos-cmd => cia402.3.pos-cmd
net 3-pos-fb => joint.3.motor-pos-fb <= cia402.3.pos-fb
# =========================
# JOINT 4 (Z2) -> cia402.4 -> lcec.0.3
# =========================
#config
setp cia402.4.csp-mode 1
setp cia402.4.pos-scale 3600
#from servo(ethercat) to cia402
net 4-statusword lcec.0.3.srv-cia-statusword => cia402.4.statusword
net 4-opmode-display lcec.0.3.srv-opmode-display => cia402.4.opmode-display
net 4-drv-act-pos lcec.0.3.srv-actual-position => cia402.4.drv-actual-position
net 4-drv-act-velo lcec.0.3.srv-actual-velocity => cia402.4.drv-actual-velocity
#from cia402 to servo(ethercat)
net 4-controlword cia402.4.controlword => lcec.0.3.srv-cia-controlword
net 4-modes-of-operation cia402.4.opmode => lcec.0.3.srv-opmode
net 4-drv-target-pos cia402.4.drv-target-position => lcec.0.3.srv-target-position
#from motion to cia
net 4-enable <= joint.4.amp-enable-out => cia402.4.enable
net 4-amp-fault => joint.4.amp-fault-in <= cia402.4.drv-fault
net 4-pos-cmd <= joint.4.motor-pos-cmd => cia402.4.pos-cmd
net 4-pos-fb => joint.4.motor-pos-fb <= cia402.4.pos-fb
# End of HAL
