' {$STAMP BS2}

   ' h-bridge v2

   ' Control Monotronics Mini H-Bridge Circuit #3-301
   ' Use 4 LED's to indicate what signals are being sent to the H-Bridge circuit

   LED_PROGRAM_RUNNING CON 15 ' LED to indicate that the program is running

   LED_A1 CON 11   ' LED for H-Bridge A1 input
   LED_A2 CON 12   ' LED for H-Bridge A2 input

   LED_B1 CON 13   ' LED for H-Bridge B1 input
   LED_B2 CON 14   ' LED for H-Bridge B2 input

   H_BRIDGE_B1 CON 4   ' H-Bridge input B1 for Motor 1
   H_BRIDGE_B2 CON 1   ' H-Bridge input B2 for Motor 1

   H_BRIDGE_A1 CON 3   ' H-Bridge input A1 for Motor 2
   H_BRIDGE_A2 CON 2   ' H-Bridge input A2 for Motor 2

   ONE_SECOND   CON 5000   ' Delay for PAUSE of 1 second
   FIVE_SECONDS CON 25000  ' Delay for PAUSE of 5 seconds
   TEN_SECONDS  CON 50000  ' Delay for PAUSE of 10 seconds

   loop    VAR Byte
   led_pin VAR Byte

   ' Set output mode status LED and motor control pins
   OUTPUT LED_A1
   OUTPUT LED_A2
   OUTPUT LED_B1
   OUTPUT LED_B2
   OUTPUT H_BRIDGE_A1
   OUTPUT H_BRIDGE_A2
   OUTPUT H_BRIDGE_B1
   OUTPUT H_BRIDGE_B2

   ' Turn LED on to indicate program is now running

   led_pin = LED_PROGRAM_RUNNING
   GOSUB led_on

   ' Indicate program is initiatizing

   GOSUB cylon_hello_world

   'Make sure motors are off

   GOSUB motors_stop
   PAUSE ONE_SECOND

   '************************************
   '*         DUAL MOTOR TESTS         *
   '*   USING HIGH LEVEL SUBROUTINES   *
   '************************************

   DEBUG "Dual Motors Test - High Level", CR

   ' Make both motors go clockwise

   DEBUG "Both CW", CR
   GOSUB motors_forward
   PAUSE FIVE_SECONDS

   ' Stop both motors

   DEBUG "All Stop", CR
   GOSUB motors_stop
   PAUSE FIVE_SECONDS

   ' Make both motors go counter clockwise

   DEBUG "Both CCW", CR
   GOSUB motors_backward
   PAUSE FIVE_SECONDS

   ' Stop both motors

   DEBUG "All Stop", CR
   GOSUB motors_stop
   PAUSE ONE_SECOND

   ' Make motor A turn clockwise and motor B counter clockwise

   DEBUG "A CW; B CCW", CR
   GOSUB motors_turn_left
   PAUSE ONE_SECOND

   ' Stop both motors

   DEBUG "All Stop", CR
   GOSUB motors_stop
   PAUSE ONE_SECOND

   ' Make motor A turn counter clockwise and motor B clockwise

   DEBUG "A CCW; B CW", CR
   GOSUB motors_turn_right
   PAUSE ONE_SECOND

   ' Stop both motors

   DEBUG "All Stop", CR
   GOSUB motors_stop

   '=========== END OF TESTS ===========

   ' Indicate program is about to end

   GOSUB cylon_hello_world

   ' Turn off LED to indicate the program has ended

   led_pin = LED_PROGRAM_RUNNING
   GOSUB led_off

   DEBUG "End.", CR

   STOP

   ' Subroutine to turn LED on.  Input variable is 'led_pin'
   ' Note: LOW = ON
   led_on:
      ' DEBUG "LED ", DEC led_pin, " on", CR
      LOW led_pin
      RETURN

   ' Subroutine to turn LED off.  Input variable is 'led_pin'
   ' Note: HIGH = OFF
   led_off:
      ' DEBUG "LED ", DEC led_pin, " off", CR
      HIGH led_pin
      RETURN

   motor_A_stop:           ' Make motor A stop
      ' DEBUG "A: Stop", CR
      LOW H_BRIDGE_A1
      LOW H_BRIDGE_A2
      led_pin = LED_A1
      GOSUB led_off
      led_pin = LED_A2
      GOSUB led_off
      RETURN

  motor_B_stop:            ' Make motor B stop
      ' DEBUG "B: Stop", CR
      LOW H_BRIDGE_B1
      LOW H_BRIDGE_B2
      led_pin = LED_B1
      GOSUB led_off
      led_pin = LED_B2
      GOSUB led_off
      RETURN

  motor_A_cw:              ' Make motor A turn ClockWise
      ' DEBUG "A: CW", CR
      LOW  H_BRIDGE_A1
      HIGH H_BRIDGE_A2
      led_pin = LED_A1
      GOSUB led_off
      led_pin = LED_A2
      GOSUB led_on
      RETURN

  motor_B_cw:             ' Make motor B turn ClockWise
      ' DEBUG "B: CW", CR
      HIGH H_BRIDGE_B1
      LOW  H_BRIDGE_B2
      led_pin = LED_B1
      GOSUB led_on
      led_pin = LED_B2
      GOSUB led_off
      RETURN

  motor_A_ccw:             ' Make motor A turn Counter ClockWise
      ' DEBUG "A: CCW", CR
      HIGH H_BRIDGE_A1
      LOW  H_BRIDGE_A2
      led_pin = LED_A1
      GOSUB led_on
      led_pin = LED_A2
      GOSUB led_off
      RETURN

  motor_B_ccw:              ' Make motor B turn Counter ClockWise
      ' DEBUG "B: CCW", CR
      LOW  H_BRIDGE_B1
      HIGH H_BRIDGE_B2
      led_pin = LED_B1
      GOSUB led_off
      led_pin = LED_B2
      GOSUB led_on
      RETURN

  motors_forward:          ' Make both motors turn clockwise to move forward
      ' DEBUG "Forward", CR
      GOSUB motor_A_cw
      GOSUB motor_B_cw
      RETURN

  motors_backward:        ' Make both motors turn counter clockwise to move backward
      ' DEBUG "Backward", CR
      GOSUB motor_A_ccw
      GOSUB motor_B_ccw
      RETURN

  motors_turn_left:        ' Make motor A turn clockwise and motor B counterclockwise to turn left
      ' DEBUG "Left Turn", CR
      GOSUB motor_A_cw
      GOSUB motor_B_ccw
      RETURN

  motors_turn_right:       ' Make motor A turn counterclockwise and motor B clockwise to turn right
      ' DEBUG "Right Turn", CR
      GOSUB motor_A_ccw
      GOSUB motor_B_cw
      RETURN

  motors_stop:             ' Make both motors stop
      ' DEBUG "All Stop", CR
      GOSUB motor_A_stop
      GOSUB motor_B_stop
      RETURN

  cylon_hello_world:   ' Make LED's do a cylon eye pattern to indicate startup

      CYLON_TIME_DELAY CON 125

      cylon_loop VAR Byte

      LOW  LED_A1
      HIGH LED_A2
      HIGH LED_B1
      HIGH LED_B2

      DEBUG "Start: Cylon Hello World.", CR

      FOR cylon_loop = 1 TO 4
         ' DEBUG "Iteration: ", DEC cylon_loop, CR
         PAUSE CYLON_TIME_DELAY
         HIGH LED_A1
         LOW  LED_A2
         PAUSE CYLON_TIME_DELAY
         HIGH LED_A2
         LOW  LED_B1
         PAUSE CYLON_TIME_DELAY
         HIGH LED_B1
         LOW  LED_B2
         PAUSE CYLON_TIME_DELAY
         LOW  LED_B1
         HIGH LED_B2
         PAUSE CYLON_TIME_DELAY
         LOW  LED_A2
         HIGH LED_B1
         PAUSE CYLON_TIME_DELAY
         LOW  LED_A1
         HIGH LED_A2
      NEXT

      DEBUG "End: Cylon Hello World.", CR

      HIGH LED_A1

      RETURN

   END