' {$STAMP BS1}
' {$PBASIC 1.0}

'------------------------------------------------------
' Quad Servo Random Movement Generator
' By Vern Graner 10-26-2004
' Any questions to vern@graner.com
' R-04.28b
'------------------------------------------------------
' -First attaempt for Prop-1 controller
'------------------------------------------------------
' Hardware setup:
' Servos connected to pins 0-3


'Declare Variables
'------------------------------------------------------
SYMBOL RESULT = W0 ' value can be 0 to 65535
SYMBOL  DEST1 = B2 ' value can be 0 to 255
SYMBOL   CUR1 = B3 ' value can be 0 to 255
SYMBOL  DEST2 = B4 ' value can be 0 to 255
SYMBOL   CUR2 = B5 ' value can be 0 to 255
SYMBOL  DEST3 = B6 ' value can be 0 to 255
SYMBOL   CUR3 = B7 ' value can be 0 to 255
SYMBOL  DEST4 = B8 ' value can be 0 to 255
SYMBOL   CUR4 = B9 ' value can be 0 to 255


'Pre set values in variables
'------------------------------------------------------
RESULT  = 11010     ' set initial "seed" value
CUR1    = 50        ' Set all current and destination values to match
DEST1   = 50        ' to force the cur=dest routines to generate a random
CUR2    = 50        ' destination for all servos on first pass
DEST2   = 50
CUR3    = 50
DEST3   = 50
CUR4    = 50
DEST4   = 50
'------------------------------------------------------

'Main loop begin
LOOP:

IF CUR1=DEST1 THEN FetchNewDest1   'If the servo has reached it's destination fetch a new one
  GOTO NextTest1                   ' if not, skip to next test
FetchNewDest1:
  RANDOM RESULT                    'Use RANDOM to find a new destination
  RESULT=RESULT // 200             'Cut it down to a usable size
  DEST1=RESULT                     'Stuff the new destination into DEST variable
  DEST1=DEST1 MIN 50               'Set limits so servos do not exceed positional
  DEST1=DEST1 MAX 200              ' requiremenmts
'  DEBUG "Cur1=",#CUR1," Dest1=",#DEST1,CR
NextTest1:

IF CUR2=DEST2 THEN FetchNewDest2
  GOTO NextTest2:
FetchNewDest2:
  RANDOM RESULT
  RESULT=RESULT // 200
  DEST2=RESULT
  DEST2=DEST2 MIN 50
  DEST2=DEST2 MAX 200
'  DEBUG "Cur2=",#CUR2," Dest2=",#DEST2,CR
NextTest2:

IF CUR3=DEST3 THEN FetchNewDest3
  GOTO NextTest3
FetchNewDest3:
  RANDOM RESULT
  RESULT=RESULT // 200
  DEST3=RESULT
  DEST3=DEST3 MIN 50
  DEST3=DEST3 MAX 200
'  DEBUG "Cur3=",#CUR3," Dest3=",#DEST3,CR
NextTest3:

IF CUR4=DEST4 THEN FetchNewDest4
  GOTO NextTest4
FetchNewDest4:
  RANDOM RESULT
  RESULT=RESULT // 200
  DEST4=RESULT
  DEST4=DEST4 MIN 50
  DEST4=DEST4 MAX 200
'  DEBUG "Cur4=",#CUR4," Dest4=",#DEST4,CR
NextTest4:


IF CUR1<DEST1 THEN PlusCur1        'Check to see if CUR is less than DEST
CUR1=CUR1-1                        'If NOT the decrement CUR
GOTO TestDone1                     'Jump out of the test
PlusCur1:
CUR1=CUR1+1                        'If SO then increment CUR
TestDone1:

IF CUR2<DEST2 THEN PlusCur2
CUR2=CUR2-1
GOTO TestDone2
PlusCur2:
CUR2=CUR2+1
TestDone2:

IF CUR3<DEST3 THEN PlusCur3
CUR3=CUR3-1
GOTO TestDone3
PlusCur3:
CUR3=CUR3+1
TestDone3:

IF CUR4<DEST4 THEN PlusCur4
CUR4=CUR4-1
GOTO TestDone4
PlusCur4:
CUR4=CUR4+1
TestDone4:

'DEBUG CLS
'DEBUG "CUR1=",#CUR1," DEST1=",#DEST1,CR
'DEBUG "CUR2=",#CUR2," DEST2=",#DEST2,CR
'DEBUG "CUR1=",#CUR1," CUR2=",#CUR2,"CUR3=",#CUR3," CUR4=",#CUR4,CR

PULSOUT 0,CUR1  'send a pulse to the servo on P0 (eyes)
PULSOUT 1,CUR2  'send a pulse to the servo on P1 (head tilt)
PULSOUT 2,CUR3  'send a pulse to the servo on P2 (head pan)
PULSOUT 3,CUR4  'send a pulse to the servo on P3 (wrist)
PAUSE 20        'Use a value of 20 for slow servo motion, comment out for fastest motion
GOTO LOOP       'lets do it again! :)