' {$STAMP BS2}
' {$PBASIC 2.5}
' **************************************************************
' *                        Pongwriter                          *
' *       By Vern Graner SSE, Texas Information Services       *
' **************************************************************
' * Code for writing on Ping Pong Ball with Parallax Serial    *
' * Inkjet printer system                                      *
' **************************************************************
' * Created:  V0.1 11-29-2007                                  *
' **************************************************************

' ************************
' * Conditonal Compile   *
' *************************************************************************
#SELECT $STAMP
  #CASE BS2, BS2E, BS2PE
    T2400       CON     396
    T4800       CON     188
    T9600       CON     84
    T19K2       CON     32
    T38K4       CON     6
  #CASE BS2SX, BS2P
    T2400       CON     1021
    T4800       CON     500
    T9600       CON     240
    T19K2       CON     110
    T38K4       CON     45
  #CASE BS2PX
    T2400       CON     1646
    T4800       CON     813
    T9600       CON     396
    T19K2       CON     188
    T38K4       CON     84
#ENDSELECT

' ************************
' * Pin allocation       *
' ************************
  ServoEject      CON 15 ' Turns to eject ball
  ServoRotate     CON 14 ' Rotates ball for printing
  ServoPrinthead  CON 13 ' Positions Print Head
  ServoIndexer    CON 12 ' Allows a single ball to drop for printing
  InkJet          CON 11 ' Serial Inkjet Printer


' ************************
' * Constants            *
' ************************
  Open            CON     $8000
  Baud            CON     Open + T9600
  Prompt          CON     ">"             ' ready prompt from HP driver
  STX             CON     2               ' start of text
  ETX             CON     3               ' end of text
  Esc             CON     27              ' OEM configuration command

  ServoEjectOut              CON  1100    '
  ServoEjectIn               CON  700

  ServoRotateStop            CON 760
  ServoRotateFwdSlow         CON 785
  ServoRotateFwdFast         CON 790

  ServoPrintheadTop          CON 1050
  ServoPrintheadCenter       CON 750
  ServoPrintheadBottom       CON 550

  ServoIndexerLoad           CON 825     '
  ServoIndexerRelease        CON 550     '


  Speed       CON 10 ' Set the step rate

'  Right  CON 1100 ' PWN value to move servo to "right" position
'  Left   CON 250  ' PWM for Left

' ************************
' * Data Variables       *
' ************************
  cntI         VAR Word  ' For loop variable
'  btnWrk       VAR Byte  ' Workspace for BUTTON
'  timer        VAR Byte  ' elapsed time of race


' Begin Sequence!
'around:
DEBUG "Start Sequence!",CR
DEBUG"----------------",CR


GOSUB PlaceInit ' Get all servos into start positions
PAUSE 2000


test2:

GOSUB PrintHeadSweep:

GOSUB PrintText

GOSUB  RotateBallFwdSlow


PAUSE 3000




END

GOTO test2


Around:
GOSUB DropBall
GOSUB PrintHeadTop

GOSUB PrintText
GOSUB RotateBallFwdSlow
GOSUB PrintHeadCenter

GOSUB PrintText
GOSUB RotateBallFwdFast
GOSUB PrintHeadBottom

GOSUB PrintText
GOSUB RotateBallFwdSlow
GOSUB EjectBall
DEBUG "Done!",CR


END

'GOTO around


'SubRoutines
'*******************************************************************

' **************************
' * Print Text             *
' **************************
PrintText:

Reset: ' Reset InkJet
  SEROUT Inkjet, Baud, [Esc, "C", 25]           ' intercolumn delay values 1-25 15=1.5 ms
  SERIN  Inkjet, Baud, 500, No_Inkjet, [WAIT(Prompt)]

Start:
  SEROUT Inkjet, Baud, [STX]
  SEROUT Inkjet, Baud, ["   WWW.TheRobotGroup.ORG < - > First Night Austin 2007 < - >"]
  SEROUT Inkjet, Baud, [ETX]
  RETURN

No_Inkjet:
DEBUG "No Inkjet Detected",CR
RETURN


' **************************
' * Servo Positions INIT   *
' **************************

PlaceInit:
DEBUG "Init Servo Positions",CR

FOR cntI = 1 TO 50
  PULSOUT ServoEject, ServoEjectIn
  PULSOUT ServoRotate, ServoRotateStop
  PULSOUT ServoPrinthead, ServoPrintheadBottom
  PULSOUT ServoIndexer, ServoIndexerRelease
  PAUSE 18
NEXT
DEBUG "Servos ready",CR

RETURN


' **************************
' * Rotate Ball STOP       *
' **************************
   RotateBallStop:
   DEBUG "Rotate STOP",CR
   FOR cntI = 1 TO 100 ' Hold in place
     PULSOUT ServoRotate,ServoRotateStop
     PAUSE 20
   NEXT
   RETURN

   RotateBallFwdSlow:
   DEBUG "Rotate Slow",CR
   FOR cntI = 1 TO 91 ' Hold in place
     PULSOUT ServoRotate,ServoRotateFwdSlow
     PAUSE 20
   NEXT
   RETURN

   RotateBallFwdFast:
   DEBUG "Rotate Fast",CR
   FOR cntI = 1 TO 65 ' Hold in place
     PULSOUT ServoRotate,ServoRotateFwdFast
     PAUSE 20
   NEXT
   RETURN


' **************************
' * Load Ball Sequence    *
' **************************
  DropBall:
  DEBUG "Load ball=",DEC ServoIndexerLoad,CR
   FOR cntI = ServoIndexerRelease TO ServoIndexerLoad STEP SPEED
     PULSOUT ServoIndexer,cntI
     PAUSE 20
    NEXT

   FOR cntI = 1 TO 25
     PULSOUT ServoIndexer,ServoIndexerLoad
     PAUSE 20
    NEXT


  DEBUG "Release ball=",DEC ServoIndexerRelease,CR
   FOR cntI = ServoIndexerLoad TO ServoIndexerRelease STEP SPEED
     PULSOUT ServoIndexer,cntI
     PAUSE 20
    NEXT

   FOR cntI = 1 TO 25
     PULSOUT ServoIndexer,ServoIndexerRelease
     PAUSE 20
    NEXT
  RETURN




' **************************
' * Print Head Top         *
' **************************
PrintHeadTop:
  DEBUG "Print Head Up",CR
   FOR cntI = ServoPrintHeadBottom TO ServoPrintHeadTop STEP SPEED
     PULSOUT ServoPrinthead,cntI
     PAUSE 20
    NEXT

   FOR cntI = 1 TO 20 ' Hold in place
     PULSOUT ServoPrinthead,ServoPrintHeadTop
     PAUSE 20
    NEXT
'    PAUSE 2000
  RETURN


' **************************
' * Print Head Center      *
' **************************
PrintHeadCenter:
  DEBUG "Print Head Center",CR
   FOR cntI = ServoPrintHeadTop TO ServoPrintHeadCenter STEP SPEED
     PULSOUT ServoPrinthead,cntI
     PAUSE 20
    NEXT

    FOR cntI = 1 TO 20 ' Hold in place
     PULSOUT ServoPrinthead,ServoPrintHeadCenter
     PAUSE 20
    NEXT
'    PAUSE 2000
  RETURN

' **************************
' * Print Head Bottom      *
' **************************
PrintHeadBottom:
  DEBUG "Print Head Bottom",CR
   FOR cntI = ServoPrintHeadCenter TO ServoPrintHeadBottom STEP SPEED
     PULSOUT ServoPrinthead,cntI
     PAUSE 20
    NEXT

   FOR cntI = 1 TO 20 ' Hold in place
     PULSOUT ServoPrinthead,ServoPrintHeadBottom
     PAUSE 20
    NEXT
'    PAUSE 2000
  RETURN



' **************************
' * Print Head Top->Bottom *
' **************************
PrintHeadSweep:
  DEBUG "Print Head Sweep",CR

   FOR cntI = 1 TO 20 ' Hold in place
     PULSOUT ServoPrinthead,ServoPrintHeadtop
     PAUSE 20
    NEXT

   FOR cntI = ServoPrintHeadtop TO ServoPrintHeadBottom STEP 1
     PULSOUT ServoPrinthead,cntI
     PULSOUT ServoRotate,ServoRotateFwdSlow
     PAUSE 19
    NEXT

   FOR cntI = 1 TO 20 ' Hold in place
     PULSOUT ServoPrinthead,ServoPrintHeadBottom
     PAUSE 20
    NEXT
  RETURN


' **************************
' * Eject Ball             *
' **************************

EjectBall:
  PAUSE 1000
  DEBUG "Eject ball",CR
   FOR cntI = ServoEjectIn TO ServoEjectOut STEP SPEED
     PULSOUT ServoEject,cntI
     PAUSE 20
    NEXT

  '  PAUSE 200

  DEBUG "Return to Catch",CR
   FOR cntI = ServoEjectOut TO ServoEjectIn STEP SPEED
     PULSOUT ServoEject,cntI
     PAUSE 20
    NEXT

    PAUSE 3000
  RETURN