#########################################################
# MOTION SECTION                                        #                                                  
######################################################################################################################################

#ENCODER A & B REVERSED ON Z AXIS
net enc0A encoder.0.phase-A <= parport.0.pin-03-in
net enc0B encoder.0.phase-B <= parport.0.pin-02-in
net enc1A encoder.1.phase-A <= parport.0.pin-05-in
net enc1B encoder.1.phase-B <= parport.0.pin-04-in
net enc2A encoder.2.phase-A <= parport.0.pin-07-in
net enc2B encoder.2.phase-B <= parport.0.pin-06-in

# CONNECT POSITION FEEDBACK SIGNALS TO ENCODERS
net Xpos-fb <= encoder.0.position
net Ypos-fb <= encoder.1.position
net Zpos-fb <= encoder.2.position

# ENCODER FEEDBACK SCALING FROM INI FILE
setp encoder.0.position-scale [AXIS_0]INPUT_SCALE 
setp encoder.1.position-scale [AXIS_1]INPUT_SCALE 
setp encoder.2.position-scale [AXIS_2]INPUT_SCALE

# CONNECT PID OUTPUT SIGNALS TO EL4132 OUTPUT'S
net Xoutput => lcec.0.D12.aout-0-value
net Youtput => lcec.0.D12.aout-1-value
net Zoutput => lcec.0.D13.aout-0-value

# SET OUTPUT SCALING FROM INI FILE (not sure if correct on lcec side)
setp lcec.0.D12.aout-0-scale [AXIS_0]OUTPUT_SCALE
setp lcec.0.D12.aout-1-scale [AXIS_1]OUTPUT_SCALE
setp lcec.0.D13.aout-0-scale [AXIS_2]OUTPUT_SCALE

# SET OUTPUT OFFSET TO ZERO
setp lcec.0.D12.aout-0-offset [AXIS_0]OUTPUT_OFFSET
setp lcec.0.D12.aout-1-offset [AXIS_1]OUTPUT_OFFSET
setp lcec.0.D13.aout-0-offset [AXIS_2]OUTPUT_OFFSET

#######################
# CORE SERVO SECTION  #
######################################################################################################################################

# POSITION FEEDBACK CONNECTION
net Xpos-fb axis.0.motor-pos-fb => pid.0.feedback
net Ypos-fb axis.1.motor-pos-fb => pid.1.feedback
net Zpos-fb axis.2.motor-pos-fb => pid.2.feedback

# PID to DAC OUTPUT SIGNALS
net Xoutput <= pid.0.output
net Youtput <= pid.1.output
net Zoutput <= pid.2.output

# SET PID LOOP OUTPUT LIMITS TO +/-1.00
setp pid.0.maxoutput [AXIS_0]MAX_VELOCITY
setp pid.1.maxoutput [AXIS_1]MAX_VELOCITY
setp pid.2.maxoutput [AXIS_2]MAX_VELOCITY
#setp pid.0.maxoutput 1.0
#setp pid.1.maxoutput 1.0
#setp pid.2.maxoutput 1.0

#SET PID VALUES IN THE INI FILE
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
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
setp pid.1.deadband [AXIS_1]DEADBAND

setp pid.2.Pgain [AXIS_2]P
setp pid.2.Igain [AXIS_2]I
setp pid.2.Dgain [AXIS_2]D
setp pid.2.bias [AXIS_2]BIAS
setp pid.2.FF0 [AXIS_2]FF0
setp pid.2.FF1 [AXIS_2]FF1
setp pid.2.FF2 [AXIS_2]FF2
setp pid.2.deadband [AXIS_2]DEADBAND

# POSITION COMMAND SIGNALS
net Xpos-cmd axis.0.motor-pos-cmd => pid.0.command
net Ypos-cmd axis.1.motor-pos-cmd => pid.1.command
net Zpos-cmd axis.2.motor-pos-cmd => pid.2.command

# AXIS ENABLE SIGNALS
net Xenable axis.0.amp-enable-out => pid.0.enable
net Yenable axis.1.amp-enable-out => pid.1.enable
net Zenable axis.2.amp-enable-out => pid.2.enable
net Xenable => lcec.0.D12.aout-0-enable
net Yenable => lcec.0.D12.aout-1-enable
net Zenable => lcec.0.D13.aout-0-enable