'{$STAMP BS2}
'Sample Code for the Parallax BS2 using the
'Solarbotics CMD Motor driver
'Rev 1.0
'(c) Solarbotics Ltd., 2006
'by Dan Gates June 12 2006

'----------------------------- Program Description -------------------------

' This CMD Sample program is for testing your CMD motor driver with a Basic
' Stamp2 or compatable.
' First we declare the motor connection pins in the "Pin Allocation category
' and create a temp register for good measure.
' Then in MAIN we turn OFF both motor enable lines for about 1 second and
' Proceed to turn each motor on in the following sequence as if they were
' driving a robot:
' Forward, Backward, LEFT, and RIGHT each for a brief period of time (about
' two and a half seconds).
' A last we jump back to the beginning of the code and start over in a never
' ending loop.
'
' This program assumes that you connect the CMD to the following BS2 pins;
' Pin7 = E 1-2
' Pin8 = E 3-4
' Pin9 = L1
' Pin10= L2
' Pin11= L3
' Pin12= L4
' Be sure to connect at least 6v to the power input and connect the motors
' to the outputs.


'----------------------------- Pin Allocations -----------------------------

'Motor control
Enable_Right        CON 7        'Enable high, disable low
Enable_Left         CON 8        'Enable high, disable low

                             '------------ Direction Truth Table -------------
                             'L1 & L3 | L2 & L4 | M1,M2,M3,M4 Outputs
                             '--------|---------|-----------------------------
M1a_Direction       CON 9     '  HIGH  |   HIGH  | Outputs = High (Motor Break)
M1b_Direction       CON 10    '  LOW   |   LOW   | Outputs = LOW  (Motor Coast)
M2a_Direction       CON 11    '  HIGH  |   LOW   | Current flows POS.(Direction 1)
M2b_Direction       CON 12    '  LOW   |   HIGH  | Current flows Neg.(Direction 2)
                             '------------------------------------------------


Temp                VAR Word      'Temp register for manipulating bits

'------------------------------ Start of main program ---------------------
MAIN:
   LOW Enable_Right               'Turn off Right Motor
   LOW Enable_Left                'Turn off Left Motor
   PAUSE 400                      'Wait 1 second
   GOSUB FORWARD                  'Jump to Forward Subroutine
   PAUSE 1000                     'Do it for 2.5 seconds
   GOSUB BACKWARD                 'Jump to Backward Subroutine
   PAUSE 1000                     'Do it for 2.5 seconds
   GOSUB LEFT                     'Jump to Left Subroutine
   PAUSE 1000                     'Do it for 2.5 seconds
   GOSUB RIGHT                    'Jump to right subroutine
   PAUSE 1000                     'Do it for 2.5 seconds

GOTO MAIN                         'Go back and start again

'----------------------------- Subroutines ---------------------------------
FORWARD:
   HIGH Enable_Right              'Turn on right motor
   HIGH Enable_Left               'Turn on left motor
   HIGH M1a_Direction             'This comination makes the right
   LOW M1b_Direction              'motor turn clockwise
   HIGH M2a_Direction             'This combination makes the left
   LOW M2b_Direction              'motor turn Counterclockwise
RETURN                            'return from this subroutine to the main
                                  'program

BACKWARD:
   HIGH Enable_Right              'Make sure motors are still on
   HIGH Enable_Left
   LOW M1a_Direction              'This combination makes the right motor
   HIGH M1b_Direction             'turn counterclockwise
   LOW M2a_Direction              'This combination makes the left motor
   HIGH M2b_Direction             'turn clockwise
RETURN

LEFT:
   HIGH Enable_Right              'Make sure motors are still on
   HIGH Enable_Left
   LOW M1a_Direction              'This combination makes the right motor
   HIGH M1b_Direction             'turn counterclockwise
   HIGH M2a_Direction             'This combination makes the left motor
   LOW M2b_Direction              'turn counterclockwise
RETURN

RIGHT:
   HIGH Enable_Right             'Make sure motors are still on
   HIGH Enable_Left              'This combination makes the right motor
   HIGH M1a_Direction            'turn clockwise
   LOW M1b_Direction             'This combination makes the left motor
   LOW M2a_Direction             'turn clockwise
   HIGH M2b_Direction
RETURN

GOTO main                        'We should never reach this point, but
                                 'it's a good failsafe

'--------------------------------------------------------------------------------
' Now it's your turn to play around with this code and see if you can make it do
' other cool stuff. Try using the "Break" mode to stop the motors between direction
' changes.
'---------------------------------------------------------------------------------