' Theremin Vision Basic Stamp Control Program  Version 1.00
'{$PBASIC 2.5}
' December 18, 2003  Terry Fritz
'{$STAMP  BS2P}        'STAMP directive (specifies a BS2P)

' Setup variables
' sensor inputs

sense1c VAR Word
sense2c VAR Word
sense3c VAR Word
sense4c VAR Word

' comparison inputs
comp1 VAR Word
comp2 VAR Word
comp3 VAR Word
comp4 VAR Word

' Calibrations
fac1 VAR Byte
fac2 VAR Byte
fac3 VAR Byte
fac4 VAR Byte
fac0 VAR Byte
t VAR Word

' scale is like a sensitivity factor
' calm keeps the output zeroed
scale VAR Byte
calm VAR Byte
inport VAR Byte

scale = 5
calm = 1
inport = 9

fac0 = 0
DEBUG CLS

' Main program start

' Turn off all sensors for 5 Sec
HIGH 15
HIGH 14
HIGH 13
HIGH 12
PAUSE 5000

StartProgram:

' Scan sensor 1
LOW 12
PAUSE 2
'STOP
PULSIN inport,1,sense1c
HIGH 12

' Scan sensor 2
LOW 13
PAUSE 1
'STOP
PULSIN inport,1,sense2c
HIGH 13

' Scan sensor 3
LOW 14
PAUSE 1
'STOP
PULSIN inport,1,sense3c
HIGH 14

' Scan sensor 4
LOW 15
PAUSE 1
'STOP
PULSIN inport,1,sense4c
HIGH 15

DEBUG CRSRXY,0,0,"1=",DEC5 sense1c,CR
DEBUG "2=",DEC5 sense2c,CR
DEBUG "3=",DEC5 sense3c,CR
DEBUG "4=",DEC5 sense4c,CR,CR


IF fac0 = 1 THEN SkipCal
' Calibration
fac1 = 30000/sense1c
fac2 = 30000/sense2c
fac3 = 30000/sense3c
fac4 = 30000/sense4c

fac0 = 1

SkipCal:
sense1c=sense1c  *fac1
sense2c=sense2c  *fac2
sense3c=sense3c  *fac3
sense4c=sense4c  *fac4

comp1=comp1-calm
comp2=comp2-calm
comp3=comp3-calm
comp4=comp4-calm

IF comp1 >30000 THEN comp1 = 0
IF comp2 >30000 THEN comp2 = 0
IF comp3 >30000 THEN comp3 = 0
IF comp4 >30000 THEN comp4 = 0

IF comp1 - sense1c > 30000 THEN comp1 = sense1c
IF comp2 - sense2c > 30000 THEN comp2 = sense2c
IF comp3 - sense3c > 30000 THEN comp3 = sense3c
IF comp4 - sense4c > 30000 THEN comp4 = sense4c

' Display results to computer screen
'DEBUG "S1 ", DEC5 sense1c,CR
'DEBUG "S2 ", DEC5 sense2c,CR
'DEBUG "S3 ", DEC5 sense3c,CR
'DEBUG "S4 ", DEC5 sense4c,CR,CR
DEBUG DEC5 t
t=t+1

DEBUG CRSRXY,60,11,DEC4 (comp2 - sense2c)/scale
DEBUG CRSRXY,50,11,DEC4 (comp1 - sense1c)/scale
DEBUG CRSRXY,50,7,DEC4 (comp4 - sense4c)/scale
DEBUG CRSRXY,60,7,DEC4 (comp3 - sense3c)/scale

'PAUSE 200

GOTO StartProgram