#com 3
#picaxe 20M2
#no_data
init:
setfreq m16
dirsB=%11111111
dirsC=%00000000
; b0, servo direction of travel flags
read 0,b0
symbol svo1_dot=bit1
symbol svo2_dot=bit2
symbol svo3_dot=bit3
symbol svo4_dot=bit4
symbol svo5_dot=bit5
symbol svo6_dot=bit6
; b1, servo is_moving flags
symbol is_anything_moving=b1
symbol svo1_ismoving=bit9
symbol svo2_ismoving=bit10
symbol svo3_ismoving=bit11
symbol svo4_ismoving=bit12
symbol svo5_ismoving=bit13
symbol svo6_ismoving=bit14
symbol svoSpeed=1
; servo positions
symbol svo1_pos=b2
symbol svo2_pos=b3
symbol svo3_pos=b4
symbol svo4_pos=b5
symbol svo5_pos=b6
symbol svo6_pos=b7
; b10-b15, minimum servo position values
read 1,b8 : symbol svo1_min=b8
read 2,b9 : symbol svo2_min=b9
read 3,b10 : symbol svo3_min=b10
read 4,b11 : symbol svo4_min=b11
read 5,b12 : symbol svo5_min=b12
read 6,b13 : symbol svo6_min=b13
; b16-b21, maximum servo position values
read 7,b14 : symbol svo1_max=b14
read 8,b15 : symbol svo2_max=b15
read 9,b16 : symbol svo3_max=b16
read 10,b17 : symbol svo4_max=b17
read 11,b18 : symbol svo5_max=b18
read 12,b19 : symbol svo6_max=b19
; setup svo's starting positions
if svo1_dot=0 then:svo1_pos=svo1_min:else:svo1_pos=svo1_max:endif
if svo2_dot=0 then:svo2_pos=svo2_min:else:svo2_pos=svo2_max:endif
if svo3_dot=0 then:svo3_pos=svo3_min:else:svo3_pos=svo3_max:endif
if svo4_dot=0 then:svo4_pos=svo4_min:else:svo4_pos=svo4_max:endif
if svo5_dot=0 then:svo5_pos=svo5_min:else:svo5_pos=svo5_max:endif
if svo6_dot=0 then:svo6_pos=svo6_min:else:svo6_pos=svo6_max:endif
servo 1,svo1_pos
servo 2,svo2_pos
servo 3,svo3_pos
servo 4,svo4_pos
servo 5,svo5_pos
servo 6,svo6_pos
symbol ledY=B.7 ; Yellow LED
symbol ledR=B.0 ; Red LED
; PROGRAMMING MODE
symbol progFlag=bit0
symbol selectedSvo=b4
symbol selected_direction=b5
symbol svo_pos_to_update=b6
symbol loop_for=b7
symbol loop_var=b8
; goto prog mode - buttons 5 and 6
main: ; LABEL - Main Program
high ledY
do
if pinsC=96 or progFlag=1 then ; buttons 5 & 6
low ledY
progFlag=1
if is_anything_moving=0 then goto prog_level_1
endif
if pinsC=42 then ; buttons 1, 4 & 5
gosub flash_red
reset
endif
if progFlag=0 then
if svo1_ismoving=0 and pinC.1=1 then ; svo1 not moving, and button pressed
svo1_ismoving=1
endif
if svo2_ismoving=0 and pinC.2=1 then ; svo2 not moving, and button pressed
svo2_ismoving=1
endif
if svo3_ismoving=0 and pinC.3=1 then ; svo3 not moving, and button pressed
svo3_ismoving=1
endif
if svo4_ismoving=0 and pinC.4=1 then ; svo4 not moving, and button pressed
svo4_ismoving=1
endif
if svo5_ismoving=0 and pinC.5=1 then ; svo5 not moving, and button pressed
svo5_ismoving=1
endif
if svo6_ismoving=0 and pinC.6=1 then ; svo6 not moving, and button pressed
svo6_ismoving=1
endif
endif
if is_anything_moving>0 then
if svo1_ismoving=1 then ; movement flag is set
if svo1_dot=0 then ; move in pos direction
if svo1_pos<svo1_max then ; not at max travel yet
svo1_pos=svo1_pos+svoSpeed ; add servo speed
else
svo1_pos = svo1_pos -1 ; at max travel
svo1_dot=1 ; update direction of travel flag
svo1_ismoving=0 ; reset moving flag
gosub saveSvoDOT
sertxd("Servo 1 is open",13,10)
endif
elseif svo1_dot=1 then ; move servo in neg direction
if svo1_pos>svo1_min then
svo1_pos=svo1_pos-svoSpeed
else
svo1_pos = svo1_pos +1
svo1_dot=0
svo1_ismoving=0
gosub saveSvoDOT
sertxd("Servo 1 is closed",13,10)
endif
endif
servopos 1,svo1_pos
endif
if svo2_ismoving=1 then ; movement flag is set
if svo2_dot=0 then ; move in pos direction
if svo2_pos<svo2_max then ; not at max travel yet
svo2_pos=svo2_pos+svoSpeed ; add servo speed
else ; at max travel
svo2_pos = svo2_pos -1
svo2_dot=1 ; update direction of travel flag
svo2_ismoving=0 ; reset moving flag
gosub saveSvoDOT
sertxd("Servo 2 is open",13,10)
endif
elseif svo2_dot=1 then ; move servo in neg direction
if svo2_pos>svo2_min then
svo2_pos=svo2_pos-svoSpeed
else
svo2_pos = svo2_pos +1
svo2_dot=0
svo2_ismoving=0
gosub saveSvoDOT
sertxd("Servo 2 is closed",13,10)
endif
endif
servopos 2,svo2_pos
endif
if svo3_ismoving=1 then ; movement flag is set
if svo3_dot=0 then ; move in pos direction
if svo3_pos<svo3_max then ; not at max travel yet
svo3_pos=svo3_pos+svoSpeed ; add servo speed
else ; at max travel
svo3_pos = svo3_pos -1
svo3_dot=1 ; update direction of travel flag
svo3_ismoving=0 ; reset moving flag
gosub saveSvoDOT
endif
elseif svo3_dot=1 then ; move servo in neg direction
if svo3_pos>svo3_min then
svo3_pos=svo3_pos-svoSpeed
else
svo3_pos = svo3_pos +1
svo3_dot=0
svo3_ismoving=0
gosub saveSvoDOT
endif
endif
servopos 3,svo3_pos
endif
if svo4_ismoving=1 then ; movement flag is set
if svo4_dot=0 then ; move in pos direction
if svo4_pos<svo4_max then ; not at max travel yet
svo4_pos=svo4_pos+svoSpeed ; add servo speed
else ; at max travel
svo4_pos = svo4_pos -1
svo4_dot=1 ; update direction of travel flag
svo4_ismoving=0 ; reset moving flag
gosub saveSvoDOT
endif
elseif svo4_dot=1 then ; move servo in neg direction
if svo4_pos>svo4_min then
svo4_pos=svo4_pos-svoSpeed
else
svo3_pos = svo3_pos +1
svo4_dot=0
svo4_ismoving=0
gosub saveSvoDOT
endif
endif
servopos 4,svo4_pos
endif
if svo5_ismoving=1 then ; movement flag is set
if svo5_dot=0 then ; move in pos direction
if svo5_pos<svo5_max then ; not at max travel yet
svo5_pos=svo5_pos+svoSpeed ; add servo speed
else ; at max travel
svo5_pos = svo5_pos -1
svo5_dot=1 ; update direction of travel flag
svo5_ismoving=0 ; reset moving flag
gosub saveSvoDOT
endif
elseif svo5_dot=1 then ; move servo in neg direction
if svo5_pos>svo5_min then
svo5_pos=svo5_pos-svoSpeed
else
svo5_pos = svo5_pos +1
svo5_dot=0
svo5_ismoving=0
gosub saveSvoDOT
endif
endif
servopos 5,svo5_pos
endif
if svo6_ismoving=1 then ; movement flag is set
if svo6_dot=0 then ; move in pos direction
if svo6_pos<svo6_max then ; not at max travel yet
svo6_pos=svo6_pos+svoSpeed ; add servo speed
else ; at max travel
svo6_pos = svo6_pos -1
svo6_dot=1 ; update direction of travel flag
svo6_ismoving=0 ; reset moving flag
gosub saveSvoDOT
endif
elseif svo6_dot=1 then ; move servo in neg direction
if svo6_pos>svo6_min then
svo6_pos=svo6_pos-svoSpeed
else
svo6_pos = svo6_pos +1
svo6_dot=0
svo6_ismoving=0
gosub saveSvoDOT
endif
endif
servopos 6,svo6_pos
endif
endif
pause 100
loop
saveSvoDOT:
write 0,b0
return
prog_level_1:
low ledY
gosub flash_red
high ledR
do
if pinC.6=1 then
gosub flash_red
reset
endif
loop
flash_red:
do until pinsC = 0
high ledR
pause 100
low ledR
pause 100
loop
return