Head moves up, then turns left and right, head goes down?

[Nannuu]

I have what I think should be easy but I'm already lost writing the program.  I have 2 standard servos.  I want a pause of 5 seconds.  Then servo1 moves 90 degrees and stays.  Then servo2 moves +45 degres, -90, +45 (back to original position).  Servo1 then moves back 90 degrees to it's original position.  Then another pause of 5 seconds, repeat.  The program can start moving immediately or pause first it doesn't matter.  The end effect should be to move a dragon head up, turn the head left and right as if it's looking around, then move back down.  It is a baby dragon head in a egg.  Any help would be appreciated.

I keep changing stuff around and getting stuck in my if then statements (I think).  In it's current state, it does nothing at all.


' {$STAMP BS1}
'[Hardware setup}--------------------------------------
' P0 - Dragon Head up/down (Standard servo)
' P1 - Dragon Head left/right (standard servo)

'[Define variables and symbols]------------------------

SYMBOL  UPDOWN       = 0
SYMBOL  LEFTRIGHT    = 1
SYMBOL  posUPDOWN   = B0
SYMBOL  posLEFTRIGHT = B1
SYMBOL  TIMER        = W2

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

Reset:
  PINS = %00000000                              ' clear all
  DIRS = %00000011                              ' set outputs

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

Main:
  PULSOUT UPDOWN, posUPDOWN
  PULSOUT LEFTRIGHT, posLEFTRIGHT
  PAUSE 16
  timer = timer + 1                             ' timer is 20ms units

MOVEUPDOWN:
  IF TIMER < 5000 THEN Main                      ' if time is less than 5 seconds, dragon sits in egg
    posUPDOWN = 100                             ' set to move head up
    IF Timer > 9000 THEN RESETTIME               ' if time is above 9 seconds, time resets, head moves down

MOVELEFTRIGHT:
  IF TIMER < 6000 THEN Main                      ' if time is less 5 seconds, go back and wait
    IF TIMER >= 6000 THEN LEFT                   ' if time is above 6 seconds, turn head left

LEFT:
  IF TIMER > 7000 THEN RIGHT                     ' if time is above 7 seconds, turn head right
    posLEFTRIGHT = 100                          ' set to turn head left

RIGHT:
  IF TIMER > 8500 THEN CENTER                    ' if time is above 8.5 seconds, return head to center
    posLEFTRIGHT = 200                          ' set to turn head right

CENTER:
  posLEFTRIGHT = 150                            ' set to center head

RESETTIME:
  TIMER = 0                                     ' reset time to 0
  posUPDOWN = 200                               ' set to move head back down

[Nannuu]

I'm not having any better luck using FOR/NEXT loops.  I can get the servos to go back and forth but not move and hold while the other servo is doing it's movement.  I'm completely missing what the code is actually doing I guess because I can place the FOR/NEXT loops in any order and get the same result (which I don't understand).  Servo1 moves back and forth, then servo2 moves back and forth.  This repeats.  It doesn't matter what order the loops are in?

[JonnyMac]

Give this program a look.  It uses a subroutine called Servo_Delay that you will call in place of PAUSE.  You'll set the servo positions and timing (in "timer") before calling.  This might make construction of your program a little easier and will allow you to randomize timing and movements for a more life-like prop.

' =========================================================================
'
'   File......
'   Purpose...
'   Author....
'   E-mail....
'   Started...
'   Updated...
'
'   {$STAMP BS1}
'   {$PBASIC 1.0}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------


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


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

SYMBOL  ServoLfRt       = 1
SYMBOL  ServoUpDn       = 0


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

SYMBOL  IsOn            = 1                     ' for active-high in/out
SYMBOL  IsOff           = 0


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

SYMBOL  posUpDn         = B2
SYMBOL  posLfRt         = B3

SYMBOL  timer           = W5


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

Reset:
  PINS = %00000000                              ' clear all
  DIRS = %00000011                              ' set outputs


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

Main:
  posUpDn = 200
  posLfRt = 150
  timer = 5000
  GOSUB Servo_Delay

Head_Up:
  posUpDn = 100
  timer = 1000
  GOSUB Servo_Delay

Head_Left:
  posLfRt = 100
  timer = 1000
  GOSUB Servo_Delay

Head_Right:
  posLfRt = 200
  timer = 1000
  GOSUB Servo_Delay

Head_Center:
  posLfRt = 150
  timer = 500
  GOSUB Servo_Delay

  GOTO Main


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

' Call with timing (milliseconds) in "timer"

Servo_Delay:
  IF timer < 20 THEN Servo_Exit
    PULSOUT ServoUpDn, posUpDn
    PULSOUT ServoLfRt, posLfRt
    PAUSE 16
    timer = timer - 20
    GOTO Servo_Delay

Servo_Exit:
  RETURN

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


' -----[ User Data ]-------------------------------------------------------

[Nannuu]

Oh that's it!  That is exactly what I'm looking for.  Thank you!

[Nannuu]

Back to the garage to get it put together.  Thank you so much again.

[JonnyMac]

Here's what I mean by randomizing movement and timing -- note that the RANDOM instruction is added to the Servo_Delay loop so that the value of lottery is constantly changing.

' =========================================================================
'
'   File......
'   Purpose...
'   Author....
'   E-mail....
'   Started...
'   Updated...
'
'   {$STAMP BS1}
'   {$PBASIC 1.0}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------


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


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

SYMBOL  ServoLfRt       = 1
SYMBOL  ServoUpDn       = 0


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

SYMBOL  IsOn            = 1                     ' for active-high in/out
SYMBOL  IsOff           = 0


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

SYMBOL  posUpDn         = B2
SYMBOL  posLfRt         = B3

SYMBOL  timer           = W4
SYMBOL  lottery         = W5


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

Reset:
  PINS = %00000000                              ' clear all
  DIRS = %00000011                              ' set outputs


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

Main:
  posUpDn = 200
  posLfRt = 150
  timer = lottery // 2001 + 3000                ' 3 to 5 seconds
  GOSUB Servo_Delay

Head_Up:
  posUpDn = 100
  timer = lottery // 501 + 500                  ' 0.5 to 1.0 seconds
  GOSUB Servo_Delay

Head_Left:
  posLfrt = lottery // 41 + 100                 ' 100 to 140
  timer = lottery // 251 + 750                  ' 0.75 to 1.0 seconds
  GOSUB Servo_Delay

Head_Right:
  posLfrt = lottery // 41 + 160                 ' 160 to 200
  timer = lottery // 251 + 750                  ' 0.75 to 1.0 seconds
  GOSUB Servo_Delay

Head_Center:
  posLfRt = 150
  timer = lottery // 251 + 250                  ' 0.25 to 0.50 seconds
  GOSUB Servo_Delay

  GOTO Main


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

' Call with timing (milliseconds) in "timer"

Servo_Delay:
  IF timer < 20 THEN Servo_Exit
    PULSOUT ServoUpDn, posUpDn
    PULSOUT ServoLfRt, posLfRt
    PAUSE 16
    timer = timer - 20
    RANDOM lottery
    GOTO Servo_Delay

Servo_Exit:
  RETURN

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


' -----[ User Data ]-------------------------------------------------------

[Nannuu]

Awesome, that's even better!