Festo CMMT-ST

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