' {$STAMP BS2sx}
' {$PBASIC 2.5}

'------------------------------------------------------
' Servo Random Movement Generator
' By Vern Graner 10-26-2004
' Any questions to vern@graner.com
' R-10.25b
'------------------------------------------------------
' -Added code to alter the stepping "speed" using an
'  array with a seperate "step" rate for each servo
' -Added code to accel/decel servo seek speed
' -Added code to place motion limits on each servo
' -Added "controlled" start/stop
'------------------------------------------------------
' Hardware setup:
' Servos connected to pins 0-3
' RC time POT ON PIN 7 (used BSAC values)

'Declare Variables
'------------------------------------------------------
SERVO     VAR Nib      'Store the servo number
I         VAR Nib      'Standard "for" counter
RESULT    VAR Word     'Store the random number result
RAMP      VAR Bit  (4) 'Store speed ramp direction
SPEED     VAR Byte (4) 'Seek speed for the servos
CUR       VAR Word (4) 'Array to hold the current servo position
DEST      VAR Word (4) 'Array to hold the destination servo position

'initialize the seek speed for each servo
'------------------------------------------------------
FOR I = 0 TO 3
  SPEED(0)=1
NEXT I

'Set the "start position" for each servo
'------------------------------------------------------
GOSUB LIMITS            'Position servos within their "safe" area
FOR I = 0 TO 3          '(usually center). by "fetching" the value
  CUR(I) = DEST(I)      ' from the "limits" subroutine and pre-stuffing
NEXT                    ' them in to the "CURrent" value for each servo

' Use RCTIME POT position to generate a random seed
'------------------------------------------------------
HIGH 7
PAUSE 1
RCTIME 7, 1, RESULT

'Main Loop that "seeks" servos to destination
'------------------------------------------------------
DO

SERVO=SERVO+1                            'cycle through the four servos
IF SERVO=4 THEN
  PAUSE 20                               'AFTER sending four pulses to the four servos
  SERVO = 0                              'wait for 20us to allow the pulse to
ENDIF                                    'be "digested" by servo

IF RAMP(SERVO) = 1 THEN                  'Based on "ramp" direction, either increase
  SPEED(SERVO)=SPEED(SERVO)+1            'or decrease the servo seek speed by 1
ELSE
  SPEED(SERVO)=SPEED(SERVO)-1
ENDIF

SPEED(SERVO)=SPEED(SERVO) MAX 50         'Enforce a maximum and minimum seek speed
SPEED(SERVO)=SPEED(SERVO) MIN 10

IF CUR(SERVO)=DEST(SERVO) THEN           'If the servo has reached
  GOSUB FetchNewDest                     'its destination then fetch
ENDIF                                    'a new destination

IF CUR(SERVO)<DEST(SERVO) THEN           'If the current servo position is LESS
  CUR(SERVO)=CUR(SERVO)+SPEED(SERVO)     'then incremment by "SPEED"
  IF CUR(SERVO)>DEST(SERVO) THEN         'Check to be sure the SPEED increment
      CUR(SERVO)=DEST(SERVO)             'does NOT "overshoot" the destination
  ENDIF                                  'value
ENDIF

IF CUR(SERVO)>DEST(SERVO) THEN           'Same as above, but decrement
  CUR(SERVO)=CUR(SERVO)-SPEED(SERVO)
  IF CUR(SERVO)<DEST(SERVO) THEN
    CUR(SERVO)=DEST(SERVO)
  ENDIF
ENDIF

'Uncomment to observe the values
'If you don't have servos
'-------------------------------------------------
'DEBUG CLS
'DEBUG "Ramp : 0=", DEC RAMP(0)," 1=", DEC RAMP(1)," 2=", DEC RAMP(2)," 3=",DEC RAMP(3),CR
'DEBUG "Speed: 0=", DEC SPEED(0)," 1=", DEC SPEED(1)," 2=", DEC SPEED(2)," 3=",DEC SPEED(3),CR
'DEBUG "Dests: 0=", DEC DEST(0)," 1=", DEC DEST(1)," 2=", DEC DEST(2)," 3=",DEC DEST(3),CR
'DEBUG "Curs : 0=", DEC CUR(0)," 1=", DEC CUR(1)," 2=", DEC CUR(2)," 3=",DEC CUR(3),CR

PULSOUT SERVO,CUR(SERVO)         'send a pulse to the servo
LOOP


'SUBROUTINE: Generate new destination value
'--------------------------------------------------
'Note: rather than using "RANDOM" you could create
'an array of predefined servo positions and step through
'them to create a repeatable series of motions

FetchNewDest:
HIGH 7                         'Use RC time to gather a new "Seed" for RANDOM
PAUSE 1                        'move the POT around on occasion to change
RCTIME 7, 1, RESULT            'the up the show. Due to "jitter" the RC time value
                               'can introduce it's own "variances" to the seed
                               'value resulting in more "randomness"

IF RESULT = 0 THEN RETURN      'Controlled stop/start. Added this so I could
                               'move the POT to "zero" and the motions all finish
                               'their last seek to destination and then halt. makes
                               'for a more "natural" halt than the sudden "freeze"
                               'of all axis of motion. Move POT away from zero and
                               'the motion restarts. If you don't have a POT for
                               'RC time, this could be replaced with a button command.

RANDOM RESULT                  'Fetch "random" value and place in "RESULT"
RESULT=RESULT // 2700          'Limit the result to 0-2700
DEST(SERVO)=RESULT             'place the "clean" new value as new destination
GOSUB LIMITS

IF RAMP(SERVO) = 0 THEN        ' Change the speed ramp direction
  RAMP(SERVO) = 1              ' 0 causes the speed to increase
ELSE                           ' 1 causes speed to decrease
  RAMP(SERVO)=0
ENDIF
RETURN

'SUBROUTINE: Enforce the servo limits
'--------------------------------------------------
Limits:
DEST(0)=DEST(0) MAX 2700       'Servo 0 MAX  (eye pan)
DEST(0)=DEST(0) MIN 320        'Servo 0 MIN

DEST(1)=DEST(1) MAX 520        'Servo 1 MAX  (head tilt)
DEST(1)=DEST(1) MIN 320        'Servo 1 MIN

DEST(2)=DEST(2) MAX 1000       'Servo 2 MAX  (head pan)
DEST(2)=DEST(2) MIN 700        'Servo 2 MIN

DEST(3)=DEST(3) MAX 2700       'Servo 3 MAX  (arm reach)
DEST(3)=DEST(3) MIN 320        'Servo 3 MIN

RETURN