loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS num_dio=16 num_aio=8
loadrt hal_parport
loadrt classicladder_rt


addf classicladder.0.refresh servo-thread

loadusr -W deltagui

# zakomentowac czesc przy gladepvc
loadusr classicladder paleta.clp

# connect I/O signals to classicladder


net Q0 motion.digital-in-00 <= classicladder.0.out-00
net Q1 motion.digital-in-01 <= classicladder.0.out-01
net Q2 motion.digital-in-02 <= classicladder.0.out-02
net Q3 motion.digital-in-03 <= classicladder.0.out-03
#net I4 motion.digital-out-04 <= classicladder.0.in-04
#net I5 motion.digital-out-05 <= classicladder.0.in-05
#net I6 motion.digital-out-06 <= classicladder.0.in-06
net Q10 motion.digital-in-10 <= classicladder.0.out-10
net Q11 motion.digital-in-11 <= classicladder.0.out-11
net Q12 motion.digital-in-12 <= classicladder.0.out-12
net Q13 motion.digital-in-13 <= classicladder.0.out-13




# add motion controller functions to servo thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread




# create HAL signals for position commands from motion module
# loop position commands back to motion module feedback
net J0pos joint.0.motor-pos-cmd => joint.0.motor-pos-fb
net J1pos joint.1.motor-pos-cmd => joint.1.motor-pos-fb
net J2pos joint.2.motor-pos-cmd => joint.2.motor-pos-fb

#net J3pos joint.3.motor-pos-cmd => joint.3.motor-pos-fb
#net J4pos joint.4.motor-pos-cmd => joint.4.motor-pos-fb
#net J5pos joint.5.motor-pos-cmd => joint.5.motor-pos-fb
#net J6pos joint.6.motor-pos-cmd => joint.6.motor-pos-fb
#net J7pos joint.7.motor-pos-cmd => joint.7.motor-pos-fb
#net J8pos joint.8.motor-pos-cmd => joint.8.motor-pos-fb

net J0cmd joint.0.pos-cmd => deltagui.joint0
net J1cmd joint.1.pos-cmd => deltagui.joint1
net J2cmd joint.2.pos-cmd => deltagui.joint2

net pfr rotarydeltakins.platformradius => deltagui.pfr
net tl rotarydeltakins.thighlength => deltagui.tl
net sl rotarydeltakins.shinlength => deltagui.sl
net fr rotarydeltakins.footradius => deltagui.fr

sets pfr 485
sets tl 422
sets sl 640
sets fr 87



# estop loopback
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in

# create signals for tool loading loopback
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed