'this simple code drives a servo
' a complete help of Annex32 can be found here
' https://cicciocb.com/annex32help/V1.442/
'Extended by Electroguard
DIRECTION = 2 : STEPS = 15: POSpin = 16: ALARMpin = 27
POSITION = 0: LEFTLIMIT = 35: SET = 4: RESET = 32: RIGHTLIMIT = 5 
JOYoffsetpin = 39: BRAKEpin = 36: I2Cdta=26: I2Cclk=25
BRAKE = 0: JOYoffset = 0: SPEEDSET = 0 ': ledoff = 1
SETpressed = 0: FAST = 90
pin.mode DIRECTION, output 'dir
pin.mode STEPS, output 'step
pin.mode ALARMpin, output 'alarm
pin.mode POSpin, input 'pulse counter
pin.mode SET, input, pullup 'Home
pin.mode RESET, input, pullup 'Reset
pin.mode LEFTLIMIT, input, pullup 'Left limit sw
pin.mode RIGHTLIMIT, input, pullup 'Right limit sw
L=0: R=0
interrupt SET, SETpressed  'Home
interrupt RESET, Reset  'Reset
interrupt LEFTLIMIT,  Llimit 'Left limit sw
interrupt RIGHTLIMIT, Rlimit 'Right limit sw
pin(ALARMpin) = 0
ratio = 017
dir = 1
I2C.SETUP I2Cdta,I2Cclk
LCD.INIT &h27, 20, 4
LCD.print 1,1, "Started "
LCD.print 1,2, "Speed=" + str$(ratio)
LCD.print 1,3, "Direction=" + str$(dir)'
timer0 150, getJOY
pwm.setup STEPS, OFF
freq_p = -1
wait


SETpressed:
pause 40
if pin(SET) = 1 then return
SPEEDSET = ratio
print "SPEED SET " + str$(freq)
return

Reset:
pause 40
if pin(RESET) = 1 then return
SPEEDSET = 0
L=0: R=0
pin(ALARMpin) = 0
print "RESET pressed"
return

Llimit:
'pause 40
if pin(LEFTLIMIT) = 1 then return
if pin(RESET) <> 0 then
 L=1: R=0
 pin(ALARMpin) = 1
endif
print "LEFT limit"
return

Rlimit:
'pause 40
if pin(RIGHTLIMIT) = 1 then return
if pin(RESET) <> 0 then
R=1: L=0
pin(ALARMpin) = 1
endif
print "RIGHT limit"
return

getJOY:
joyoffset = (adc(JOYoffsetpin)-2048)/20
'print joyoffset
v = adc(34) + JOYoffset
BRAKE = adc(BRAKEpin)
'BRAKE = convert.map(BRAKE,0,4096,1,99)
BRAKE=(BRAKE+10)/4096: BRAKE=0.001+(BRAKE/1)
'if brake > 1 then brake = 1
'print BRAKE
' computes the
if ((SPEEDSET=0) and (L=0) and (R=0)) or (pin(SET)=0) then ratio = (v - 2048)/2048
if (L=1) and (ratio < 0) then ratio = ratio + BRAKE: if ratio > 0 then ratio = 0 
if (R=1) and (ratio > 0) then ratio = ratio - BRAKE: if ratio < 0 then ratio = 0
'else 
 freq = abs(ratio * 100)
 dir = ratio > 0
'endif
pin(DIRECTION) = dir
'print ratio, freq
if (abs(ratio) < 0.1) then
  pwm.setup STEPS, OFF
  msg$ = "Paused"
  if L=1 then msg$ = msg$ + " at LEFT end"
  if R=1 then msg$ = msg$ + " at RIGHT end"
  msg$ = msg$ + string$((20-len(msg$))," ")
 LCD.print 1,1, msg$ 
else
  if (freq_p <> freq) then
    pwm.setup STEPS, 1, 100, freq, 12
    freq_p = freq
    msg$ = "Running"
  if L=1 then msg$ = msg$ + " at LEFT end"
  if R=1 then msg$ = msg$ + " at RIGHT end"
  msg$ = msg$ + string$((20-len(msg$))," ")
 LCD.print 1,1, msg$ 

'    LCD.print 1,1, msg$ + string$(20-len(msg$)," ")
  endif
endif
if dir=0 then dir$="Left  " else dir$="Right  "
msg$="Speed=" + str$(abs(ratio)*100,"%3.0f") + "%" 
if SPEEDSET<>0 then msg$ = msg$ + "  SET"
LCD.print 1,2, msg$ + string$(20-len(msg$)," ")
LCD.print 1,3, "Direction=" + dir$
LCD.print 1,4, "Brake=" + str$(BRAKE*100,"%3.0f") + "%        "
return