diff --git a/example/cia402.hal b/example/cia402.hal new file mode 100644 index 0000000..ae7671e --- /dev/null +++ b/example/cia402.hal @@ -0,0 +1,72 @@ +########################################################### +# +# CIA 402 example snippet Hal +# +########################################################### + +########################################################### +# Setup +########################################################### + +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +loadusr -W lcec_conf ethercat-conf.xml +loadrt lcec +loadrt cia402 count=3 +loadrt pid names=x-pid,y-pid,z-pid + + +########################################################### +# Functions servo-thread +########################################################### + +addf lcec.read-all servo-thread +addf cia402.0.read-all servo-thread +addf cia402.1.read-all servo-thread +addf cia402.2.read-all servo-thread + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +addf x-pid.do-pid-calcs servo-thread +addf y-pid.do-pid-calcs servo-thread +addf z-pid.do-pid-calcs servo-thread + +addf cia402.0.write-all servo-thread +addf cia402.1.write-all servo-thread +addf cia402.2.write-all servo-thread +addf lcec.write-all servo-thread + +######################################### +#nets +######################################### +net emc-enable => iocontrol.0.emc-enable-in +sets emc-enable 1 + +#config +setp cia402.0.csp-mode 1 +setp cia402.0.pos-scale 3600 + +#from servo(ethercat) to cia402 +net x-statusword lcec.0.0.cia-statusword => cia402.0.statusword +net x-opmode-display lcec.0.0.opmode-display => cia402.0.opmode-display +net x-drv-act-pos lcec.0.0.actual-position => cia402.0.drv-actual-position +net x-drv-act-velo lcec.0.0.actual-velocity => cia402.0.drv-actual-velocity + +#from cia402 to servo(ethercat) +net x-controlword cia402.0.controlword => lcec.0.0.cia-controlword +net x-modes-of-operation cia402.0.opmode => lcec.0.0.opmode +net x-drv-target-pos cia402.0.drv-target-position => lcec.0.0.target-position +net x-drv-target-velo cia402.0.drv-target-velocity => lcec.0.0.target-velocity + +#from motion to cia +net x-enable <= joint.0.amp-enable-out => cia402.0.enable +net x-amp-fault => joint.0.amp-fault-in <= cia402.0.drv-fault +net x-pos-cmd <= joint.0.motor-pos-cmd => cia402.0.pos-cmd +net x-pos-fb => joint.0.motor-pos-fb <= cia402.0.pos-fb + +#homing +net x-home-index <= joint.0.index-enable => cia402.0.home + + diff --git a/example/ethercat-conf.xml b/example/ethercat-conf.xml new file mode 100644 index 0000000..9323b83 --- /dev/null +++ b/example/ethercat-conf.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + +