#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"