loadrt trivkins
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES
loadrt pid num_chan=1
loadrt bldc cfg=qi # q: Incremental encoder input. i: Use the index of an incremental encoder as a home reference.
loadrt siggen
loadrt conv_float_s32 count=2
loadrt conv_s32_float count=1
loadrt sincos
loadrt clarkeinv
loadrt scale count=3

#loadusr -Wn anorad pyvcp -c anorad anorad.xml


loadrt eppfpga


addf eppfpga.write      	servo-thread


addf siggen.0.update		servo-thread
addf conv-float-s32.0		servo-thread
addf conv-float-s32.1		servo-thread
addf conv-s32-float.0		servo-thread

addf sincos.0			servo-thread
addf clarkeinv.0		servo-thread

addf scale.0			servo-thread
addf scale.1			servo-thread
addf scale.2			servo-thread
addf bldc.0			servo-thread

addf pid.0.do-pid-calcs		servo-thread


# bldc parameters
setp bldc.0.initvalue 		12000

# pid parameters
setp pid.0.maxoutput 		30000
setp pid.0.Pgain 		1500
setp pid.0.Igain 		100
setp pid.0.Dgain 		0.1
setp pid.0.bias			0.0

# FF0 : Zero order feed-forward term. For position loops, it should usually be left at zero.
# FF1 : First order feed-forward term. For position loops, the contribution is proportional to speed, and can be used to compensate for friction or motor CEMF.
# FF2 : Second order feed-forward term. For position loops, the contribution is proportional to acceleration, and can be used to compensate for inertia.
setp pid.0.FF0			0
setp pid.0.FF1			1
setp pid.0.FF2			0.0
setp pid.0.deadband		0.002

# bldc parameters
# one electrical revolution = 30 mm = 60000 encoder counts (measured)
setp bldc.0.scale 		60000 # The number of encoder counts per revolution.
setp bldc.0.poles		2 # The encoder scale will be divided by this value to determine the number of encoder counts per electrical revolution.





net dacA bldc.0.A-value => conv-float-s32.0.in
net dacB bldc.0.C-value => conv-float-s32.1.in
net Xcounts => bldc.0.rawcounts
net Xindex-enable eppfpga.Xindex-enable => bldc.0.index-enable

setp bldc.0.rev FALSE
setp bldc.0.lead-angle 90
setp bldc.0.encoder-offset -6687 # 60000*-0.111



#clarke
#net costheta sincos.0.cos => clarkeinv.0.x
#net sintheta sincos.0.sin => clarkeinv.0.y
#net dacA clarkeinv.0.a => scale.1.in
#net dacB clarkeinv.0.b => scale.2.in
#setp scale.1.gain		15000
#setp scale.2.gain		15000
#net dacAscale scale.1.out => conv-float-s32.0.in
#net dacBscale scale.2.out => conv-float-s32.1.in

net dacAint conv-float-s32.1.out => eppfpga.dacAX
net dacBint conv-float-s32.0.out => eppfpga.dacBX





net Xcounts <= eppfpga.Xcounts
net test <= eppfpga.test

setp scale.0.gain -1
net XposRelative eppfpga.Xposition => scale.0.offset
net XposOffset eppfpga.Xposition-latched => scale.0.in
net XAbs scale.0.out



net XAbs => pid.0.feedback
net Xsel => pid.0.command
net pidXout pid.0.output => bldc.0.value




# pyvcp
# pyvcp sintaksa: net <signal-name> <pin-name> <opt-direction> <opt-pin-name>signal-name

#net init anorad.init bldc.0.init anorad.initled
#net pin02 anorad.btn02 bldc.0.rev anorad.led-02
#net pin03 anorad.ServoAmpEnable eppfpga.ServoAmpEnable anorad.led-ServoAmpEnable
#net pin04 anorad.ServoAmpLimit eppfpga.ServoAmpLimit anorad.led-ServoAmpLimit
#net Xsel <= anorad.slider-f
#net XAbs => anorad.xposition
#net Xsen3 eppfpga.Xsen3 => anorad.Xsen3-led
#net initdone bldc.0.init-done => anorad.init-done
#net XPIDEnable anorad.XPIDEnable pid.0.enable anorad.led-XPIDEnable

net freq => siggen.0.frequency
sets freq 0.05
#net amp => siggen.0.amplitude

setp siggen.0.amplitude 		15
#setp siggen.0.frequency		0.05
#net triangle siggen.0.triangle => sincos.0.theta