84 lines
2.4 KiB
Tcl
84 lines
2.4 KiB
Tcl
#TODO Version stamp this code and write the version number to the generated code
|
|
# axis = A B C D E F
|
|
# encodercounts = number of encoder counts per revolution
|
|
# quadcounts = number of quadrature counts per revolution
|
|
# tolquadcounts = tolerance in quadrature counts
|
|
proc genMOVE {axis encodercounts quadcounts tolquadcounts} {
|
|
puts "NO CLOSE MOTION LOOP FOR ${axis} AXIS
|
|
NO If the motor is within ${axis}TOLQC quadrature counts of the target
|
|
NO position then adjust it.
|
|
${axis}DIFF=${axis}QTARGET - _TP${axis}
|
|
IF ( ((100*${axis}TOLQC) > @ABS\[${axis}DIFF\]) & (@ABS\[${axis}DIFF\] > ${axis}TOLQC) )
|
|
${axis}SHLDFIX=1
|
|
ELSE
|
|
${axis}SHLDFIX=0
|
|
ENDIF
|
|
IF (${axis}FIXPOS=1)
|
|
${axis}OLDQT=${axis}QTARGET
|
|
${axis}count=10
|
|
SH${axis}
|
|
#${axis}LOOP
|
|
NO Abort if target position changes
|
|
IF (${axis}OLDQT <> ${axis}QTARGET)
|
|
JP#${axis}ENDCLP
|
|
ENDIF
|
|
${axis}count=${axis}count-1
|
|
PR${axis}=${encodercounts}*(${axis}DIFF/${quadcounts})
|
|
IF (_SP${axis}>${axis}MINSP)
|
|
SP${axis}=@ABS\[_PR${axis}\]/2.0
|
|
IF (_SP${axis} < ${axis}MINSP)
|
|
SP${axis}=${axis}MINSP
|
|
ENDIF
|
|
IF (_SP${axis} > ${axis}MAXSP)
|
|
SP${axis}=${axis}MAXSP
|
|
ENDIF
|
|
ENDIF
|
|
BG${axis}
|
|
MC${axis}
|
|
${axis}DIFF=${axis}QTARGET - _TP${axis}
|
|
IF ( ((100*${axis}TOLQC) > @ABS\[${axis}DIFF\]) & (@ABS\[${axis}DIFF\] > ${axis}TOLQC) )
|
|
JP#${axis}LOOP,${axis}count>0
|
|
ENDIF
|
|
#${axis}ENDCLP
|
|
NO Restore speed to maximum
|
|
${axis}FIXPOS=0
|
|
${axis}SHLDFIX=0
|
|
SP${axis}=${axis}MAXSP
|
|
ENDIF"
|
|
}
|
|
|
|
|
|
# Generate subroutine to start the servo loop thread
|
|
puts "#CLSLOOP"
|
|
set tolquadcounts 1
|
|
set encodercounts 25000
|
|
set quadcounts 8192
|
|
puts "NO Initilise tolerance, max and min speeds
|
|
ATOLQC=100
|
|
AMAXSP=${encodercounts}
|
|
AMINSP=100*(${encodercounts}/93207)
|
|
AFIXPOS=0"
|
|
foreach {axis} {B C D E F} {
|
|
puts "${axis}TOLQC=${tolquadcounts}
|
|
${axis}MAXSP=${encodercounts}
|
|
${axis}MINSP=100*(${encodercounts}/$quadcounts)
|
|
${axis}FIXPOS=0"
|
|
}
|
|
foreach {axis} {A B C D E F} {
|
|
puts "${axis}DIFF=${axis}QTARGET - _TP${axis}
|
|
IF ( ((100*${axis}TOLQC) > @ABS\[${axis}DIFF\]) & (@ABS\[${axis}DIFF\] > ${axis}TOLQC) )
|
|
${axis}SHLDFIX=1
|
|
ELSE
|
|
${axis}SHLDFIX=0
|
|
ENDIF"
|
|
}
|
|
puts "XQ#SERVOLP,1
|
|
EN"
|
|
# Generate servo loop
|
|
puts "#SERVOLP"
|
|
genMOVE A $encodercounts -93165 $tolquadcounts
|
|
foreach {axis} {B C D E F} {
|
|
genMOVE $axis $encodercounts $quadcounts $tolquadcounts
|
|
}
|
|
puts "JP#SERVOLP"
|