SYMBOL pos = w0 rem current position. Need 16-bit counter. About 3000 counts
SYMBOL target_pos = w1 rem target position. Stored in eeprom for each preset.
SYMBOL pos_change = b4 rem
SYMBOL enc_state = b5 rem current encoder state
SYMBOL ir = b6 rem received ir-code
SYMBOL preset_addr = b8 rem eeprom address for the presets = received ir-code x 2
SYMBOL target_error = b9 rem target position considered reached when within target_pos +/- target_error
SYMBOL pos_addr = 32 rem eeprom-address for storing the current position
SYMBOL stop_offset = 15 rem approx. number of counts during motor stop (motor power off)
SYMBOL preset_address = b11 rem the ir-code is used as a lookup index for the preset addresses.
eeprom 0,(0,127,0,127,0,127,0,127,0,127,0,127) rem preload eeprom with (2^16)/2
eeprom 12,(0,127,0,127,0,127,0,127,0,127,0,127) rem for all presets and pos to
eeprom 24,(0,127,0,127,0,127,0,127,0,127) rem avoid pos counter rollover at mask extremes
read pos_addr, word pos rem load position from previous session
sertxd("Saved pos: ",#pos,13,10) rem write position to serial port for debugging
enc_state = pinsB & %00000011 rem read initial encoder state
main:
setfreq m32 rem set clock-speed to 32MHz
irin [1000,main],B.6,ir rem wait for IR. Timeout after 1, or read ir-code from B.6
preset_addr = ir*2 rem calculate eeprom address for preset (use ir-code as index)
setfreq m4
sertxd("IR-code: ",#ir,13,10) rem write received ir-code to serial port for debugging
setfreq m32
if ir = 43 then rem save preset-button is pressed (time/clock button)
irin [4000,flashred],B.6,ir rem wait for a valid preset-button press, or timeout after 4s,
if ir <9 then rem flash red led if a valid preset-button is not pressed, or nothing is pressed
preset_addr = ir*2 rem calculate preset_addr for the pressed preset-button
write preset_addr, word pos rem write current pos into preset_addr
goto flashgreen rem LEDs only on controller #1, so no red or green flash
endif
endif
if ir < 9 then movto_preset rem move mask to preset if ir-code received is a valid preset-button
if ir=37 or ir=38 then jog rem jog left/right ir-code 37: input 38: balance -
goto main rem unknown IR-code, go back to main and wait for new ir-code
movto_preset:
high C.2 rem power on encoder
pause 500
read preset_addr, word target_pos rem read target position from eeprom
if target_pos > pos then
target_error = target_pos - pos
if target_error < 20 then main rem error less than 20 counts, don`t move
goto extend rem mask need to extend
endif
if target_pos < pos then
target_error = pos - target_pos
if target_error < 20 then main rem error less than 20 counts, don`t move
goto retract rem mask need to retract
endif
if target_pos = pos then main rem already on target, no movement, go back to main
jog:
high C.2 rem power on encoder
pause 500
if ir = 37 then rem extend
pause 1000
high B.4 rem motor rotates CCW seen from cable end.
pause 1000
high B.5 rem start motor
goto monitor_jog
endif
if ir = 38 then rem retract
pause 1000
low B.4 rem motor rotates CW seen from cable end.
pause 1000
high B.5 rem start motor
goto monitor_jog
endif
extend: rem start motor ccw (seen from cable end)
target_pos = target_pos - stop_offset rem cut power stop_offset counts before reaching target_pos
pause 1000
high B.4
pause 1000
high B.5
goto monitor_preset
retract: rem start motor cw (seen from cable end)
target_pos = target_pos + stop_offset rem cut power stop_offset counts before reaching target_pos
pause 1000
low B.4
pause 1000
high B.5
goto monitor_preset
goto main
monitor_preset:
if pos = target_pos or pinC.6 = 1 then stop_movement rem preset reached or key pressed (one-shot keeps pinC.6=1 if ir) to interrupt movement?
enc_state = enc_state & %00000011 * 4 rem shift the two LSB bits containing current enc_state two bits left
enc_state = pinsB & %00000011 | enc_state rem read new enc_state into the two LSB-bits, OR with previous enc_state to get the current LOOKUP-index
LOOKUP enc_state,(1,2,0,1,0,1,1,2,2,1,1,0,1,0,2,1),pos_change rem use the enc_state as an index in the LOOKUP-table, and put the result into pos_change
pos = pos -1 + pos_change rem calculate the new pos (count). The LOOKUP-table can`t contain negative numbers.
GOTO monitor_preset rem repeat until target_pos reached, or an arbitrary button is pressed (emergency stop)
monitor_jog:
if pinC.6 = 0 then stop_movement rem jog-key released?? (one-shot keeps pinC.6 high during ir-activity)
enc_state = enc_state & %00000011 * 4
enc_state = pinsB & %00000011 | enc_state
LOOKUP enc_state,(1,2,0,1,0,1,1,2,2,1,1,0,1,0,2,1),pos_change
pos = pos -1 + pos_change
GOTO monitor_jog
stop_movement:
low B.5 rem turn off motor-power
for b10=0 to 200 rem continue to count encoder pulses until motor has stopped completely.
enc_state = enc_state & %00000011 * 4
enc_state = pinsB & %00000011 | enc_state
LOOKUP enc_state,(1,2,0,1,0,1,1,2,2,1,1,0,1,0,2,1),pos_change
pos = pos -1 + pos_change
next b10
setfreq m4
sertxd("Stop at pos: ",#pos,13,10)
pause 1000
write pos_addr, word pos rem update memory with current position
low C.2 rem power off encoder
goto main
flashgreen:
high C.0
pause 800
low C.0
pause 800
high C.0
pause 800
low C.0
goto main
flashred:
high C.1
pause 800
low C.1
pause 800
high C.1
pause 800
low C.1
goto main
