# Generated by PNCconf at Sat Sep 10 10:10:56 2016
# If you make changes to this file, they will be
# overwritten when you run PNCconf again

loadrt [KINS]KINEMATICS
loadrt bitslice count=1 personality=24
loadrt conv_s32_u32 count=1
loadrt classicladder_rt
#loadrt spindle count=1  #halui include spindle.0
#autoconverted  trivkins
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
loadusr -W lcec_conf ethercat-conf.xml
loadusr classicladder --nogui CNC3LD.clp
loadrt lcec
loadrt cia402 count=4

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 cia402.3.read-all        servo-thread
addf motion-command-handler   servo-thread
addf motion-controller        servo-thread
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 lcec.write-all           servo-thread
addf bitslice.0  			  servo-thread
addf conv-s32-u32.0			  servo-thread 
addf classicladder.0.refresh  servo-thread

#*******************
#  AXIS X
#*******************

# --- joint signals for motion

net x-pos-cmd    <= joint.0.motor-pos-cmd
net x-vel-cmd    <= joint.0.vel-cmd
net x-pos-fb     <= joint.0.motor-pos-fb
net x-enable     <= joint.0.amp-enable-out

# --- connect stepper driver to joint

net x-pos-cmd    => cia402.0.pos-cmd
net x-pos-fb     => cia402.0.pos-fb
net x-enable     => cia402.0.enable

# --- ect60 settings

setp cia402.0.csp-mode 1
setp cia402.0.pos-scale 4000

# --- from stepper(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
#net x-test1     <=      lcec.0.0.test1          

# --- from cia402 to stepper(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

#*******************
#  AXIS Y
#*******************

# --- joint signals for motion

net y-pos-cmd    <= joint.1.motor-pos-cmd
net y-vel-cmd    <= joint.1.vel-cmd
net y-pos-fb     <= joint.1.motor-pos-fb
net y-enable     <= joint.1.amp-enable-out

# --- connect stepper driver to the joint

net y-pos-cmd    => cia402.1.pos-cmd
net y-pos-fb     => cia402.1.pos-fb
net y-enable     => cia402.1.enable

# --- ect60 settings

setp cia402.1.csp-mode 1
setp cia402.1.pos-scale 4000

# --- from servo(ethercat) to cia402

net y-statusword      lcec.0.1.cia-statusword  => cia402.1.statusword
net y-opmode-display  lcec.0.1.opmode-display  => cia402.1.opmode-display
net y-drv-act-pos     lcec.0.1.actual-position => cia402.1.drv-actual-position
net y-drv-act-velo    lcec.0.1.actual-velocity => cia402.1.drv-actual-velocity

# --- from cia402 to servo(ethercat) 

net y-controlword         cia402.1.controlword         => lcec.0.1.cia-controlword
net y-modes-of-operation  cia402.1.opmode              => lcec.0.1.opmode
net y-drv-target-pos      cia402.1.drv-target-position => lcec.0.1.target-position
net y-drv-target-velo     cia402.1.drv-target-velocity => lcec.0.1.target-velocity

#*******************
#  AXIS Z
#*******************

# --- joint signals for motion

net z-pos-cmd    <= joint.2.motor-pos-cmd
net z-vel-cmd    <= joint.2.vel-cmd
net z-pos-fb     <= joint.2.motor-pos-fb
net z-enable     <= joint.2.amp-enable-out

# --- connect stepper driver to the joint

net z-pos-cmd    => cia402.2.pos-cmd
net z-pos-fb     => cia402.2.pos-fb
net z-enable     => cia402.2.enable

# --- ect60 settings

setp cia402.2.csp-mode 1
setp cia402.2.pos-scale 4000

# --- from servo(ethercat) to cia402

net z-statusword      lcec.0.2.cia-statusword  => cia402.2.statusword
net z-opmode-display  lcec.0.2.opmode-display  => cia402.2.opmode-display
net z-drv-act-pos     lcec.0.2.actual-position => cia402.2.drv-actual-position
net z-drv-act-velo    lcec.0.2.actual-velocity => cia402.2.drv-actual-velocity

# --- from cia402 to servo(ethercat) 

net z-controlword         cia402.2.controlword         => lcec.0.2.cia-controlword
net z-modes-of-operation  cia402.2.opmode              => lcec.0.2.opmode
net z-drv-target-pos      cia402.2.drv-target-position => lcec.0.2.target-position
net z-drv-target-velo     cia402.2.drv-target-velocity => lcec.0.2.target-velocity

#*******************
#  SPINDLE
#*******************

# --- joint signals for motion

net spindle-speed-cmd spindle.0.speed-out => cia402.3.velocity-cmd
net spindle-speed-fb cia402.3.velocity-fb => spindle.0.speed-in
net spindle-on spindle.0.on => cia402.3.enable

# --- ect60 settings

setp cia402.3.csp-mode 0
setp cia402.3.pos-scale 4000
setp cia402.3.velo-scale 400

# --- from servo(ethercat) to cia402

net s-statusword      lcec.0.3.cia-statusword  => cia402.3.statusword
net s-opmode-display  lcec.0.3.opmode-display  => cia402.3.opmode-display
net s-drv-act-pos     lcec.0.3.actual-position => cia402.3.drv-actual-position
net s-drv-act-velo    lcec.0.3.actual-velocity => cia402.3.drv-actual-velocity

# --- from cia402 to servo(ethercat) 

net s-controlword         cia402.3.controlword         => lcec.0.3.cia-controlword
net s-modes-of-operation  cia402.3.opmode              => lcec.0.3.opmode
net s-drv-target-pos      cia402.3.drv-target-position => lcec.0.3.target-position
net s-drv-target-velo     cia402.3.drv-target-velocity => lcec.0.3.target-velocity

#*********************
#   E-STOP
#*********************

#setp iocontrol.0.emc-enable-in 1
net lcec-z-stop <= lcec.0.2.z-estop
net lcec-z-stop => bitslice.0.in 
net HE-estopbit bitslice.0.out-23 => iocontrol.0.emc-enable-in


