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

'------------------------------------------------------
' HEX Servo Random Movement Generator
' By Vern Graner April 29th, 2005
' Any questions to vern@graner.com
' R-04.29f
'------------------------------------------------------
' -First attempt for Prop-1 controller
'------------------------------------------------------
' Hardware setup:
' Servos connected to pins 0-5


'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
SYMBOL  DEST5 = B10 ' value can be 0 to 255
SYMBOL   CUR5 = B11 ' value can be 0 to 255
SYMBOL  DEST6 = B12 ' value can be 0 to 255
SYMBOL   CUR6 = B13 ' 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
'CUR5    = 50
'DEST5   = 50
'CUR6    = 50
'DEST6   = 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 CUR5=DEST5 THEN FetchNewDest5
  GOTO NextTest5
FetchNewDest5:
  RANDOM RESULT
  RESULT=RESULT // 200
  DEST5=RESULT
  DEST5=DEST5 MIN 50
  DEST5=DEST5 MAX 200
'  DEBUG "Cur4=",#CUR4," Dest4=",#DEST4,CR
NextTest5:

IF CUR6=DEST6 THEN FetchNewDest6
  GOTO NextTest6
FetchNewDest6:
  RANDOM RESULT
  RESULT=RESULT // 200
  DEST6=RESULT
  DEST6=DEST6 MIN 50
  DEST6=DEST6 MAX 200
'  DEBUG "Cur4=",#CUR4," Dest4=",#DEST4,CR
NextTest6:


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:

IF CUR5<DEST5 THEN PlusCur5
CUR5=CUR5-1
GOTO TestDone5
PlusCur5:
CUR5=CUR5+1
TestDone5:

IF CUR6<DEST6 THEN PlusCur6
CUR6=CUR6-1
GOTO TestDone6
PlusCur6:
CUR6=CUR6+1
TestDone6:


'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)
PULSOUT 4,CUR5  'send a pulse to the servo on P4 (elbow)
PULSOUT 5,CUR6  'send a pulse to the servo on P5 (thumb)

PAUSE 20        'Use a value of 20 for slow servo motion, comment out for fastest motion
GOTO LOOP       'lets do it again! :)