Trying to blend servo movement

[brad g]

I recently built a Scary Terry skull and decided to up the ante by adding a pan/tilt mechanism using 2 other servos. Now my problem is trying to come up with a program using the Prop1 to control the movements as well as turn on the little mp3 player to kick off the Terry audio board.  Another haunter has come up with a method to trigger the mp3 using the GND connection of the Prop1 and the P O-7 pin, so hook up is no problem. That just leaves programming. From start to finish, the routine I'm using runs 2 min., 28 sec. (Stephen Lynch's song "Halloween") and I'm just trying to come up with something that will drive the other 2 servos for pan/tilt in a random pattern, nothing complex, just "start player - left turn - center - right turn - center- down" etc. for the duration of the song and then shut back down until re-triggered. Thats the good news, bad news is I'm totally new to programming, and frankly, don't have a clue.....help.....the mechanism was a LOT of work, and I'd hate to have it fail for a lack of a program to run it. Thanks!

[JonnyMac]

This one is a little tricky as servos need constant refreshing; that said, I think I can take a page from Vern Graner's random servo movement code and put the position/refresh elements in a loop that always runs.  The idea is that you would change the X (pan) and Y (tilt) values as you need and the servos would move there.  The trick is doing it in the small memory footprint of the Prop-1.  Stay tuned....

[JonnyMac]

After considering it a few days I think a table-driven approach is going to give the best results.  Below you'll find a program that will start an audio player with a "blipped" output and then manage three servos.  The table hold position values for the three servos and a repeats value.  Timing for each position record is bases on the repeats entry (last value of each record).  The movement engine will put the servos into that position and then check the repeats value; if zero it moves to the next record, otherwise it decrements the repeats value and refreshes the servos again.  So, each record timing is roughly 20 milliseconds x (repeats + 1) -- this lets you have from 20 ms to 5.1 seconds hold time.  Remember, though, that servos move slowly so you don't want to use values that are too low as the servo may never actually reach that position (mechanically, that is) before the repeats timing expires.

You designate the end of your sequence by putting 255 in the servo 1 position value (normal values are 100 to 200).  I have built a table as big as it can be with three servos and the associated code.  Yes, it can be updated for fewer or more servos, but you'll have to change things from top to bottom.

I know this code is a little advanced -- push yourself to learn how it works and you'll be rewarded, I promise; using table-driven programs is a great way to squeeze a lot of movement into a small program space of the Prop-1. 


' =========================================================================
'
'   File...... Servo_Blender.BS1
'   Purpose...
'   Author.... Jon Williams, EFX-TEK
'   E-mail.... jwilliams@efx-tek.com
'   Started...
'   Updated... 25 MAR 2007
'
'   {$STAMP BS1}
'   {$PBASIC 1.0}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------
'
' Starts an external audio player and then controls the simultaneous
' movement of three servos.  Servo movement is stored in a EEPROM table
' with a "repeats" value for timing at the positions specified in the
' record (last byte).  The timing is approximately:
'
' 20 milliseconds x (repeats + 1)
'
' Mimimum: ~0.02 seconds
' Maximum: ~5.12 seconds
'
' Note that servos are mechanical and move slowly, so very small "repeats"
' values may expire before the servos reaches their target positions.


' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------

SYMBOL  Audio           = 7
SYMBOL  Trigger         = PIN6

SYMBOL  Servo3          = 2
SYMBOL  Servo2          = 1
SYMBOL  Servo1          = 0


' -----[ Constants ]-------------------------------------------------------

SYMBOL  IsOn            = 1
SYMBOL  IsOff           = 0

SYMBOL  RecLen          = 4                     ' record length (servos + 1)


' -----[ Variables ]-------------------------------------------------------

SYMBOL  record          = B0                    ' movement record
SYMBOL  pntr            = B1                    ' table byte pointer
SYMBOL  s1Pos           = B2                    ' servo 1 position
SYMBOL  s2Pos           = B3                    ' servo 2 position
SYMBOL  s3Pos           = B4                    ' servo 3 position
SYMBOL  repeats         = B5                    ' repeats for record data


' -----[ Initialization ]--------------------------------------------------

Reset:
  DIRS = %10000111                              ' make servo pins outputs

  record = 0
  GOSUB Get_Servo_Pos                           ' get "home" positions


' -----[ Program Code ]----------------------------------------------------

Main:
  GOSUB Refresh_Servos
  IF Trigger = IsOff THEN Main                  ' wait for trigger

Start_Audio:
  PULSOUT Audio, 2500                           ' 25 ms blip on audio pin

Reset_Moves:
  record = 1                                    ' point to start of moves

Move_Engine:
  GOSUB Get_Servo_Pos                           ' get servo positions
  IF s1Pos = 255 THEN Reset                     ' abort if at end

Servo_Hold:
  GOSUB Refresh_Servos
  IF repeats = 0 THEN New_Record                ' time left at this pos?
    repeats = repeats - 1                       ' yes, decrement timing
    GOTO Servo_Hold

New_Record:
  record = record + 1
  GOTO Move_Engine


' -----[ Subroutines ]-----------------------------------------------------

' Call with "record" set to table line to read

Get_Servo_Pos:
  pntr = record * RecLen                        ' point into table
  READ pntr, s1Pos                              ' read servo 1 value
  pntr = pntr + 1                               ' point to servo 2
  READ pntr, s2Pos
  pntr = pntr + 1
  READ pntr, s3Pos
  pntr = pntr + 1                               ' point to repeats
  READ pntr, repeats                            ' read repeats
  RETURN

' -------------------------------------------------------------------------

' Update servos -- time consumed is 18 to 21 ms

Refresh_Servos:
  PULSOUT Servo1, s1Pos
  PULSOUT Servo2, s2Pos
  PULSOUT Servo3, s3Pos
  PAUSE 15                                      ' pad for loop timing
  RETURN


' -----[ EEPROM Data ]-----------------------------------------------------

' Note on servo movement timing:
'
' Time spent on each record is 20 ms x (repeats + 1) so a repeats value
' of 0 is ~20 ms and a repeats value of 255 (max) is ~5.1 sseconds at that
' position.
'
'         s1   s2   s3   rpts

Move_Table:
  EEPROM (150, 150, 150, 255)                   ' home position
  EEPROM (100, 100, 150,  50)                   ' start of movements
  EEPROM (100, 200, 150,  50)
  EEPROM (200, 200, 150,  50)
  EEPROM (200, 100, 150,  50)
  EEPROM (255, 150, 150, 255)                   ' end here

  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)
  EEPROM (150, 150, 150, 255)                   ' last possible move
  EEPROM (255, 255, 255, 255)                   ' define end of table

[JonnyMac]

Okay, I know that the logical reply is going to be, "But what if I want to move servos and control solenoids or LEDs at the same time?"  In the version below I traded one of the servos for four output bits.  Note that the servos have been moved to pins 4 and 5 so that the digital outputs use the low bits of the port (P0..P3) -- this simplifies the code which gives more room for table data.  Timing per record is the same as before; now you can have blinky LED eyes in your moving skull.

' =========================================================================
'
'   File...... Servo_Blender_x2d.BS1
'   Purpose...
'   Author.... Jon Williams, EFX-TEK
'   E-mail.... jwilliams@efx-tek.com
'   Started...
'   Updated... 25 MAR 2007
'
'   {$STAMP BS1}
'   {$PBASIC 1.0}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------
'
' Starts an external audio player and then controls the simultaneous
' movement of two servos and four digital output channels.  Servo movement
' and digital outputs are stored in a EEPROM table with a "repeats" value
' for timing at the positions specified in the record (last byte).  The
' timing is approximately:
'
' 20 milliseconds x (repeats + 1)
'
' Mimimum: ~0.02 seconds
' Maximum: ~5.12 seconds
'
' Note that servos are mechanical and move slowly, so very small "repeats"
' values may expire before the servos reaches their target positions.


' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------

SYMBOL  Audio           = 7
SYMBOL  Trigger         = PIN6
SYMBOL  Servo2          = 5
SYMBOL  Servo1          = 4
SYMBOL  DigOut4         = 3
SYMBOL  DigOut3         = 2
SYMBOL  DigOut2         = 1
SYMBOL  DigOut1         = 0

' -----[ Constants ]-------------------------------------------------------

SYMBOL  IsOn            = 1
SYMBOL  IsOff           = 0

SYMBOL  RecLen          = 3                     ' record length (servos + 1)


' -----[ Variables ]-------------------------------------------------------

SYMBOL  record          = B0                    ' movement record
SYMBOL  pntr            = B1                    ' table byte pointer
SYMBOL  s1Pos           = B2                    ' servo 1 position
SYMBOL  s2Pos           = B3                    ' servo 2 position
SYMBOL  repeats         = B4                    ' repeats for record data


' -----[ Initialization ]--------------------------------------------------

Reset:
  DIRS = %10111111                              ' set outputs

  record = 0
  GOSUB Get_Servo_Pos                           ' get "home" positions


' -----[ Program Code ]----------------------------------------------------

Main:
  GOSUB Refresh_Servos
  IF Trigger = IsOff THEN Main                  ' wait for trigger

Start_Audio:
  PULSOUT Audio, 2500                           ' 25 ms blip on audio pin

Reset_Moves:
  record = 1                                    ' point to start of moves

Move_Engine:
  GOSUB Get_Servo_Pos                           ' get servo positions
  IF s1Pos = 255 THEN Reset                     ' abort if at end

Servo_Hold:
  GOSUB Refresh_Servos
  IF repeats = 0 THEN New_Record                ' time left at this pos?
    repeats = repeats - 1                       ' yes, decrement timing
    GOTO Servo_Hold

New_Record:
  record = record + 1
  GOTO Move_Engine


' -----[ Subroutines ]-----------------------------------------------------

' Call with "record" set to table line to read

Get_Servo_Pos:
  pntr = record * RecLen                        ' point into table
  READ pntr, s1Pos                              ' read servo 1 value
  pntr = pntr + 1                               ' point to servo 2
  READ pntr, s2Pos                              ' read servo 2 value
  pntr = pntr + 1                               ' point to digital outputs
  READ pntr, PINS                               ' set digital outputs
  pntr = pntr + 1                               ' point to repeats
  READ pntr, repeats                            ' read repeats
  RETURN

' -------------------------------------------------------------------------

' Update servos -- time consumed is 18 to 21 ms

Refresh_Servos:
  PULSOUT Servo1, s1Pos
  PULSOUT Servo2, s2Pos
  PAUSE 16                                      ' pad for loop timing
  RETURN


' -----[ EEPROM Data ]-----------------------------------------------------

' Note on servo movement record timing:
'
' Time spent on each record is 20 ms x (repeats + 1) so a repeats value
' of 0 is ~20 ms and a repeats value of 255 (max) is ~5.1 sseconds at that
' position.
'
'         s1   s2    outs  rpts
'                    4321

Move_Table:
  EEPROM (150, 150, %0000, 255)                 ' home position
  EEPROM (100, 100, %0001,  50)                 ' start of movements
  EEPROM (100, 200, %0010,  50)
  EEPROM (200, 200, %0100,  50)
  EEPROM (200, 100, %1000,  50)
  EEPROM (255, 150, %0000, 255)                 ' end demo here

  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)
  EEPROM (150, 150, %0000, 255)                 ' last possible move
  EEPROM (255, 255, %0000, 255)                 ' define end of table

[brad g]

Actually, the routine I plan to use will be even simpler as it will only need to drive two servos and start/stop the mp3 player. The jaw servo will be driven by the ST100 audio board. The way I have the device wired, both boards will be energized simultaneously when the trigger is tripped. After that, the Prop1 will start the mp3, which will in turn let the ST100 operate the jaw servo and the LED's while the Prop1 continues on with it's program running the pan servo and the tilt servo for the 2min, 28sec of song running time. At that point the program will shut down the mp3 and reset. I'll probably just use a remote trigger to activate power to the whole system just so I can retain some control of how long power is applied to the boards. I can't tell you how much I appreciate your efforts on this Jon, this is WAAAY out of my league!

[JonnyMac]

You're welcome, Brad.  And it's only out of your league now because you don't have any practice with PBASIC -- I have 13 years!, and 12 years of programming other dialects of BASIC before that.  Everything is learned and can be learned, including prop programming by those who don't presently do it.  If you'll just spend about 15 or 20 minutes a day with PBASIC you'll be an expert in no time.  There is plenty of time between now and haunt season to become an expert -- it's up to you to do it.

If you want to use one of the programs with your MPJA MP3 player you'll need a couple adjustments:  Start the audio like this:

Start_Audio:
  LOW Audio
  PAUSE 2000
  INPUT Audio

Then, at the end of your sequence, instead of going back to Reset have the program go to a line called Stop_Audio which looks like this:

Stop_Audio:
  LOW Audio
  PAUSE 2000
  INPUT Audio
  GOTO Reset

With the extra code you'll loose memory for a couple table entries, but that will let you start and stop the MP3 player and have blended servo movements.  You just have to breakdown the audio and determine where each servo is and for how long.  Remember, divide your position timing (in milliseconds) by 20, and use that value for the timing element (last byte) in each record.  If, for example, you want the servos to hold for 1.5 seconds, the math works out to be:

  1500 / 20 = 75 --> 75 is the timing byte for that record

If you need to go longer than 5 seconds use two records with the same position values -- this simply stacks the time spent at those positions.