' Robotics with the Boe-Bot - TestServoSpeed.bs2
' Enter pulse width, then count revolutions of the wheel.
' The wheel will run for 6 seconds
' Multiply by 10 to get revolutions per minute (RPM).

'{$STAMP BS2}
'{$PBASIC 2.5}

counter        VAR     Word
pulseWidth     VAR     Word
pulseWidthComp VAR     Word

FREQOUT 4, 2000, 3000                        ' Signal program start/reset.

DO

  DEBUG "Enter pulse width: "

  DEBUGIN DEC pulseWidth

  pulseWidthComp = 1500 - pulseWidth

  FOR counter = 1 TO 244
    PULSOUT 12, pulseWidth
    PULSOUT 13, pulseWidthComp
    PAUSE 20
  NEXT

LOOP