# load realtime modules
# kinematics
loadrt trivkins
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]TRAJ_PERIOD key=[EMCMOT]SHMEM_KEY num_joints=[TRAJ]AXES
loadrt hal_parport cfg="0x378"
loadrt encoder num_chan=2
loadrt pid num_chan=2
loadrt pwmgen output_type=1,1
loadrt ddt count=4
loadrt constant count=1

# define the order of execution for RT code
addf parport.0.read            base-thread
addf encoder.update-counters   base-thread
addf pwmgen.make-pulses        base-thread
addf parport.0.write           base-thread

addf encoder.capture-position  servo-thread
addf motion-command-handler    servo-thread
addf motion-controller         servo-thread
addf pid.0.do-pid-calcs        servo-thread
addf pid.1.do-pid-calcs        servo-thread
addf constant.0                servo-thread
addf pwmgen.update             servo-thread
addf ddt.0                     servo-thread
addf ddt.1                     servo-thread
addf ddt.2                     servo-thread
addf ddt.3                     servo-thread

# hook stuff together
newsig Xpos-fb float
newsig Ypos-fb float

setp encoder.0.position-scale [AXIS_0]INPUT_SCALE
setp encoder.1.position-scale [AXIS_1]INPUT_SCALE

net enc0A encoder.0.phase-A <= parport.0.pin-10-in
net enc0B encoder.0.phase-B <= parport.0.pin-11-in
net enc1A encoder.1.phase-A <= parport.0.pin-12-in
net enc1B encoder.1.phase-B <= parport.0.pin-13-in

linksp Xpos-fb <= encoder.0.position
linksp Ypos-fb <= encoder.1.position

newsig Xvel-cmd float
newsig Yvel-cmd float

linksp Xpos-fb => pid.0.feedback
linksp Ypos-fb => pid.1.feedback
linksp Xpos-fb => axis.0.motor-pos-fb
linksp Ypos-fb => axis.1.motor-pos-fb

linksp Xvel-cmd <= pid.0.output
linksp Yvel-cmd <= pid.1.output

linksp Xvel-cmd => pwmgen.0.value
linksp Yvel-cmd => pwmgen.1.value

newsig Xpwm bit
newsig Ypwm bit
linkps pwmgen.0.pwm Xpwm
linkps pwmgen.1.pwm Ypwm

newsig Xdir bit
newsig Ydir bit
linkps pwmgen.0.dir => Xdir
linkps pwmgen.1.dir => Ydir

linksp Xdir    => parport.0.pin-02-out
linksp Xdir    => parport.0.pin-04-out
setp parport.0.pin-04-out-invert TRUE
linksp Xpwm => parport.0.pin-03-out

linksp Ydir    => parport.0.pin-05-out
linksp Ydir    => parport.0.pin-07-out
setp parport.0.pin-07-out-invert TRUE
linksp Ypwm => parport.0.pin-06-out

setp pid.0.maxoutput 1.0
setp pid.1.maxoutput 1.0

# the values below come from the ini
setp pid.0.Pgain [AXIS_0]P
setp pid.0.Igain [AXIS_0]I
setp pid.0.Dgain [AXIS_0]D
setp pid.0.bias [AXIS_0]BIAS
setp pid.0.FF0 [AXIS_0]FF0
setp pid.0.FF1 [AXIS_0]FF1
setp pid.0.FF2 [AXIS_0]FF2
# deadband should be just over 1 count
setp pid.0.deadband [AXIS_0]DEADBAND

setp pid.1.Pgain [AXIS_1]P
setp pid.1.Igain [AXIS_1]I
setp pid.1.Dgain [AXIS_1]D
setp pid.1.bias [AXIS_1]BIAS
setp pid.1.FF0 [AXIS_1]FF0
setp pid.1.FF1 [AXIS_1]FF1
setp pid.1.FF2 [AXIS_1]FF2
# deadband should be just over 1 count
setp pid.1.deadband [AXIS_1]DEADBAND

newsig Xpos-cmd float
newsig Ypos-cmd float
linksp Xpos-cmd <= axis.0.motor-pos-cmd
linksp Ypos-cmd <= axis.1.motor-pos-cmd
linkps pid.0.command <= Xpos-cmd
linkps pid.1.command <= Ypos-cmd
newsig Xenable bit
newsig Yenable bit
linkps axis.0.amp-enable-out => Xenable
linkps axis.1.amp-enable-out => Yenable
linksp Xenable => pid.0.enable 
linksp Xenable => pwmgen.0.enable
linksp Yenable => pid.1.enable
linksp Yenable => pwmgen.1.enable

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

newsig Xvel float
newsig Xacc float
linksp Xpos-cmd => ddt.0.in
linkps ddt.0.out => Xvel
linksp Xvel => ddt.1.in
linkps ddt.1.out => Xacc
newsig Yvel float
newsig Yacc float
linksp Ypos-cmd => ddt.2.in
linkps ddt.2.out => Yvel
linksp Yvel => ddt.3.in
linkps ddt.3.out => Yacc

net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed