# 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
#autoconverted  trivkins
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=2
loadrt metalmusings_encoder

addf lcec.read-all            servo-thread
addf cia402.0.read-all        servo-thread
addf cia402.1.read-all        servo-thread
addf metalmusings-encoder.0     servo-thread
addf motion-command-handler   servo-thread
addf motion-controller        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 5000

# --- from stepper(ethercat) to cia402

net x-statusword      lcec.0.4.cia-statusword  => cia402.0.statusword
net x-opmode-display  lcec.0.4.opmode-display  => cia402.0.opmode-display
net x-drv-act-pos     lcec.0.4.actual-position => cia402.0.drv-actual-position
net x-drv-act-velo    lcec.0.4.actual-velocity => cia402.0.drv-actual-velocity

# --- from cia402 to stepper(ethercat) 

net x-controlword         cia402.0.controlword         => lcec.0.4.cia-controlword
net x-modes-of-operation  cia402.0.opmode              => lcec.0.4.opmode
net x-drv-target-pos      cia402.0.drv-target-position => lcec.0.4.target-position
net x-drv-target-velo     cia402.0.drv-target-velocity => lcec.0.4.target-velocity

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

# --- joint signals for motion

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

# --- connect stepper driver to the joint

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

# --- ect60 settings

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

# --- from servo(ethercat) to cia402

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

# --- from cia402 to servo(ethercat) 

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

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

setp iocontrol.0.emc-enable-in 1
#net estop iocontrol.0.emc-enable-in lcec.0.1.din-0

#setp lcec.0.3.enc-pos-scale [SPINDLE_9]ENCODER_SCALE
setp lcec.0.6.enc-pos-scale [SPINDLE_9]ENCODER_SCALE

#net spindle-revs <= lcec.0.3.enc-pos
net spindle-revs <= lcec.0.6.enc-position
loadrt invert 
loadrt mult2    names=mult2.rps,mult2.rpm
addf   invert.0   servo-thread
addf   mult2.rps  servo-thread
addf   mult2.rpm  servo-thread

setp invert.0.in          [SPINDLE_9]ENCODER_SCALE
setp mult2.rpm.in0        -60.0
setp mult2.rpm.in0        60.0
#net  enc-invert-pos-scale mult2.rps.in0  <= invert.0.out
#net  enc-get-freq-rps     mult2.rps.in1  <= lcec.0.3.enc-frequency
#net  spindle-vel-fb-rps   mult2.rpm.in1  <= mult2.rps.out
#net  spindle-vel-fb-rpm   mult2.rpm.out
#net  spindle-index-enable lcec.0.3.enc-index-c-enable

net  spindle-vel-fb-rps   mult2.rpm.in1  <= lcec.0.6.enc-frequency
net  spindle-vel-fb-rpm   mult2.rpm.out

net  to_encoder           metalmusings-encoder.0.index-latch-enable lcec.0.6.enc-index-latch-enable
net  from_encoder         metalmusings-encoder.0.index-status       lcec.0.6.index-status
net  spindle-index-enable metalmusings-encoder.0.index-c-enable

# ---setup spindle control signals---

net spindle-vel-cmd-rps        <=  spindle.0.speed-out-rps
net spindle-vel-cmd-rps-abs    <=  spindle.0.speed-out-rps-abs
net spindle-vel-cmd-rpm        <=  spindle.0.speed-out
net spindle-vel-cmd-rpm-abs    <=  spindle.0.speed-out-abs
net spindle-enable             <=  spindle.0.on
net spindle-cw                 <=  spindle.0.forward
net spindle-ccw                <=  spindle.0.reverse
net spindle-brake              <=  spindle.0.brake
net spindle-revs               =>  spindle.0.revs
net spindle-at-speed           =>  spindle.0.at-speed
net spindle-vel-fb-rps         =>  spindle.0.speed-in
net spindle-index-enable      <=>  spindle.0.index-enable


#******************************
# connect miscellaneous signals
#******************************

#  ---HALUI signals---

net joint-select-a        halui.axis.x.select
net x-is-homed            halui.joint.0.is-homed
net jog-x-pos             halui.axis.x.plus
net jog-x-neg             halui.axis.x.minus
net jog-x-analog          halui.axis.x.analog
net joint-select-b        halui.axis.z.select
net z-is-homed            halui.joint.1.is-homed
net jog-z-pos             halui.axis.z.plus
net jog-z-neg             halui.axis.z.minus
net jog-z-analog          halui.axis.z.analog
net jog-selected-pos      halui.axis.selected.plus
net jog-selected-neg      halui.axis.selected.minus
net spindle-manual-cw     halui.spindle.0.forward
net spindle-manual-ccw    halui.spindle.0.reverse
net spindle-manual-stop   halui.spindle.0.stop
net machine-is-on         halui.machine.is-on
net jog-speed             halui.axis.jog-speed 
net MDI-mode              halui.mode.is-mdi

#  ---coolant signals---

net coolant-mist      <=  iocontrol.0.coolant-mist
net coolant-flood     <=  iocontrol.0.coolant-flood

#  ---probe signal---

net probe-in     =>  motion.probe-input

#  ---motion control signals---

net in-position               <=  motion.in-position
net machine-is-enabled        <=  motion.motion-enabled

#  ---digital in / out signals---

#  ---estop signals---

net estop-out     <=  iocontrol.0.user-enable-out
net estop-ext     =>  iocontrol.0.emc-enable-in

#  ---manual tool change signals---

loadusr -W hal_manualtoolchange
#net tool-change-request     iocontrol.0.tool-change       =>  hal_manualtoolchange.change
#net tool-change-confirmed   iocontrol.0.tool-changed      <=  hal_manualtoolchange.changed
#net tool-number             iocontrol.0.tool-prep-number  =>  hal_manualtoolchange.number
#net tool-prepare-loopback   iocontrol.0.tool-prepare      =>  iocontrol.0.tool-prepared

addf cia402.0.write-all       servo-thread
addf cia402.1.write-all       servo-thread
addf lcec.write-all           servo-thread
