# load realtime modules
# kinematics
loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
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

setp encoder.0.position-scale [JOINT_0]INPUT_SCALE
setp encoder.1.position-scale [JOINT_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

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


net Xpos-fb => pid.0.feedback
net Ypos-fb => pid.1.feedback
net Xpos-fb => joint.0.motor-pos-fb
net Ypos-fb => joint.1.motor-pos-fb

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

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

net Xpwm pwmgen.0.pwm
net Ypwm pwmgen.1.pwm

net Xdir <= pwmgen.0.dir
net Ydir <= pwmgen.1.dir

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

net Ydir => parport.0.pin-05-out
net Ydir => parport.0.pin-07-out
setp parport.0.pin-07-out-invert TRUE
net 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 [JOINT_0]P
setp pid.0.Igain [JOINT_0]I
setp pid.0.Dgain [JOINT_0]D
setp pid.0.bias [JOINT_0]BIAS
setp pid.0.FF0 [JOINT_0]FF0
setp pid.0.FF1 [JOINT_0]FF1
setp pid.0.FF2 [JOINT_0]FF2
# deadband should be just over 1 count
setp pid.0.deadband [JOINT_0]DEADBAND

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

net Xpos-cmd <= joint.0.motor-pos-cmd
net Ypos-cmd <= joint.1.motor-pos-cmd
net Xpos-cmd => pid.0.command
net Ypos-cmd => pid.1.command
net Xenable <= joint.0.amp-enable-out
net Yenable <= joint.1.amp-enable-out
net Xenable => pid.0.enable
net Xenable => pwmgen.0.enable
net Yenable => pid.1.enable
net Yenable => pwmgen.1.enable

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

net Xpos-cmd => ddt.0.in
net Xvel <= ddt.0.out
net Xvel => ddt.1.in
net Xacc <= ddt.1.out
net Ypos-cmd => ddt.2.in
net Yvel <= ddt.2.out
net Yvel => ddt.3.in
net Yacc <= ddt.3.out

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