' Stepper_Motor_Test_01a.BAS
'
' {$STAMP BS2}
' {$PBASIC 2.5}

' -----[ Description ]--------------------------------------------------------
'
' This is a simple program that interfaces the BS2 with a KP56LM2 Stepper
' Motor.  Stepper motor wire colors Black and White are connected to 5 VDC.
' Stepper motor wire colors White/Red, White/Green, Red, Green are switched
' to ground through high current switching NPN transistors.  The motor
' provides 1.8 degrees per step with 80oz-in. of holding torque (6V, 1.2A).
' (The motor windings could also be switched with high current MOSFETs.)
' With delay set to 2 ~66rpm accomplished. At 4 ~46rpm with better torque.
' At 60rpm and a 20 turns/inch leadscrew 3 inches/minute is accomplished.
'
' -----[ Declarations ]-------------------------------------------------------

delay       VAR    Word
flip        VAR    Bit

' -----[ Main Routine ]-------------------------------------------------------

init:
delay=3
flip=0
OUTPUT 0                           'Stepper Motor White/Red
OUTPUT 1                           'Stepper Motor White/Green
OUTPUT 2                           'Stepper Motor Red
OUTPUT 3                           'Stepper Motor Green
INPUT 4                               '0 = Disable Motor, 1 = Enable Motor
INPUT 5                               'Selects a ramp up and down for motor start and stop
INPUT 6                               '0 = Low Torque, 1 = High Torque
INPUT 7                               'Direction (forward or backward)
GOSUB motoroff

main:
DO

IF IN4=1 THEN                      'enables motor operation
  IF IN7=0 THEN                    'selects forward or backward operation
    IF IN6=0 THEN                  'selects low or high torque
      GOSUB forwardlow          'forward low torque
    ELSE
      GOSUB forwardhigh          'forward high torque
    ENDIF
  ELSE
    IF IN6=0 THEN
      GOSUB backwardlow        'backward low torque
    ELSE
      GOSUB backwardhigh        'backward high torque
    ENDIF
  ENDIF

ELSE                                         'disables motor operation, turns off drivers
  GOSUB motoroff
ENDIF

' This section selects a ramp up and down for motor start and stop
IF IN5=0 THEN
  IF flip=1 THEN
    delay=delay-10
  ELSE
    delay=delay+10
  ENDIF
  IF delay>100 THEN
    flip=1
    PAUSE 5000
  ENDIF
  IF delay<10 THEN flip=0
ELSE
  delay=3
  flip=0
ENDIF

LOOP

forwardlow:
GOSUB quad1
GOSUB quad3
GOSUB quad5
GOSUB quad7
RETURN

forwardhigh:
GOSUB quad2
GOSUB quad4
GOSUB quad6
GOSUB quad8
RETURN

backwardlow:
GOSUB quad7
GOSUB quad5
GOSUB quad3
GOSUB quad1
RETURN

backwardhigh:
GOSUB quad8
GOSUB quad6
GOSUB quad4
GOSUB quad2
RETURN

quad1:
  LOW 0
  HIGH 1
  HIGH 2
  HIGH 3
PAUSE delay
RETURN

quad2:
  LOW 0
  LOW 1
  HIGH 2
  HIGH 3
PAUSE delay
RETURN

quad3:
  HIGH 0
  LOW 1
  HIGH 2
  HIGH 3
PAUSE delay
RETURN

quad4:
  HIGH 0
  LOW 1
  LOW 2
  HIGH 3
PAUSE delay
RETURN

quad5:
  HIGH 0
  HIGH 1
  LOW 2
  HIGH 3
PAUSE delay
RETURN

quad6:
  HIGH 0
  HIGH 1
  LOW 2
  LOW 3
PAUSE delay
RETURN

quad7:
  HIGH 0
  HIGH 1
  HIGH 2
  LOW 3
PAUSE delay
RETURN

quad8:
  LOW 0
  HIGH 1
  HIGH 2
  LOW 3
PAUSE delay
RETURN

motoroff:
  HIGH 0
  HIGH 1
  HIGH 2
  HIGH 3
RETURN