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