( Step_2Motor)
( NB array indices used are 1 to loop counter-1)
( half-stepping not used here)
     
DECIMAL 
( motor-control variables etc:)
4 ARRAY Pins_1  ( full steps for motor 1 - sequence of GPIO lines used)
4 ARRAY Pins_2  ( full steps for motor 2 - sequence of GPIO lines used)
100000 VARIABLE dT          ( 0 to 100 csec - for decreasing speed)
500000 VARIABLE dTmax        ( starting 'speed')
50000 VARIABLE dTmin        ( max 'speed')

: sm_delay  0 DO I I * DROP LOOP ;  ( n ...) ( short delay using only Forth words)

: vvfast 25000 dT ! ;
: vfast 50000 dT ! ;
: fast 100000 dT ! ;
: medium 150000 dT ! ;
: slow 200000 dT ! ;
: vslow 1000000 dT ! ;
: snailpace 10000000 dT ! ;

: ProtectI2C   1 GPEnabI2C ;
: UnprotectI2C 0 GPEnabI2C ;
: OutputsGPIO  2 GPExpGPIO ;   ( all pins as outputs, with inputs high)

( motor words)
: set_motorGPIO
   1 4 DO I Pins_1 @ 1 SWAP GPWrMode -1 +LOOP
   1 4 DO I Pins_2 @ 1 SWAP GPWrMode -1 +LOOP
   DECIMAL ;

( these GPIO pin numbers depend upon the pins used - edit if required)
: init_motors
   fast
   17 18 
   qPi2 @ 2 = IF 21 ELSE 27 ENDIF   ( Pi revision 2 mod)
   22   1 4 DO I Pins_1 ! -1 +LOOP
   23 24 25  4   1 4 DO I Pins_2 ! -1 +LOOP
   UnprotectI2C ( OutputsGPIO)
   set_motorGPIO ;

: GPIOpin_ON  1 SWAP GPWrData DROP DROP ;  ( n ...)
: GPIOpin_OFF 0 SWAP GPWrData DROP DROP ;  ( n ...)

: all_OFF dT @ sm_delay 5 1 DO
   I Pins_1 @ GPIOpin_OFF
   I Pins_2 @ GPIOpin_OFF   
  LOOP ;

: 1_OFF ( n...) dT @ sm_delay GPIOpin_OFF ;

: clock_1      1 4 DO  I Pins_1 @ DUP GPIOpin_ON 1_OFF -1 +LOOP ;
: anticlock_1  5 1 DO  I Pins_1 @ DUP GPIOpin_ON 1_OFF LOOP ;

: clock_2      5 1 DO  I Pins_2 @ DUP GPIOpin_ON 1_OFF LOOP ;
: anticlock_2  1 4 DO  I Pins_2 @ DUP GPIOpin_ON 1_OFF -1 +LOOP ;

: nclock_1 DUP 0= 0= IF 0 DO clock_1 LOOP ELSE DROP ENDIF ;
: nclock_2 DUP 0= 0= IF 0 DO clock_2 LOOP ELSE DROP ENDIF ;
: naclock_1 DUP 0= 0= IF  0 DO anticlock_1 LOOP ELSE DROP ENDIF ;
: naclock_2 DUP 0= 0= IF  0 DO anticlock_2 LOOP ELSE DROP ENDIF ;

: step_demo  init_motors  vvfast
    400 nclock_1  400 naclock_1
    400 nclock_2  400 naclock_2 ;

init_motors

          


 





