dim Ierror as integer
dim Igain as integer
dim Mamp as integer
dim bvolt as integer
dim Iset as integer
dim counter as integer

setcommand(_MS,1)

'reuse counter
print("\n\rStarting (V1)")
for counter=10 andwhile counter >= 0 evaluate counter -= 1
 wait(1000)
 print(counter," ")
next counter
print("\n\r")

counter=10
Iset=20
Igain = 100     'proportional gain

setcommand(_VAR,1,Iset) 'save in VAR 1 in tenths of Amps
setcommand(_VAR,2,Igain)

while 1


'begin current loop
  Igain = getvalue(_VAR,2)

  Mamp=getvalue(_MOTAMPS,1)

  bvolt = getvalue(_V, 2)
  
  Iset=getvalue(_VAR,1)
  
  Ierror = Iset - Mamp

  output = Ierror * Igain

'end current loop

'begin output clipping
  if output > 1000 then
    output = 1000
  end if
    
  if output < -1000 then 
    output = -1000
  end if
    
  if output < 0 then
    output=0
  end if
'end output clipping

  setcommand(_GO,1,output)

  counter++
  if counter >= 1 then  'time to send an update
    pwr=getvalue(_MOTPWR,1)
    print(Mamp,",",Ierror,",",pwr,"\n\r")
    counter=0
  end if

  wait(10)
end while

halt:                  'hard fault, estop
  SetCommand(_ESTOP,1)
  setcommand(_GO,1,0)          
haltloop:  
  print("ERROR: ",errorCode," : ",errorValue,"\n\r")
  wait(1000)
  goto haltloop