( Step_Motor)
( NB array indices used are 1 to loop counter-1)

OSCLI" RMEnsure GPIO 0.00 RMLoad GPIO"
OSCLI" RMEnsure GPIO 0.40 ERROR HELP can't find GPIO module"

DECIMAL 
4 ARRAY Full  ( full steps - sequence of GPIO lines used)
9 ARRAY Half  ( half steps)
0 VARIABLE direction   ( 0 - A/Cl   1 - Clockwise  -1 - Stop)
100000 VARIABLE dT          ( 0 to 100 csec - for decreasing speed)
500000 VARIABLE dTmax        ( starting 'speed')
50000 VARIABLE dTmin        ( max 'speed')

1 VARIABLE full_half   ( 0 - half steps   1 -  full steps)


( : Tdelay )   ( n csec .... )  ( governs speed)
(  0 TIME= )
(  BEGIN DUP TIME < UNTIL )
(  DROP ; )

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

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

: init_motors
   UnprotectI2C OutputsGPIO
  -1 direction !
  100000 dT !             ( slow - 100msec)
  1 full_half !      ( full steps)

  0 1 4 7  1 4 DO I Full ! -1 +LOOP
  0 0 1 1 4 4 7 7 0    0 8 DO I Half ! -1 +LOOP
;

: 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 Full @ GPIOpin_OFF LOOP ;

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

: full_step_clock
  5 1 DO  I Full @ DUP GPIOpin_ON 1_OFF LOOP ;

: full_step_anticlock
    1 4 DO I Full @ DUP GPIOpin_ON 1_OFF -1 +LOOP ;


: ?faster ( #steps,ith step...) ( wait for 4 steps before acceleration)
   DUP 4 > IF
            - 4 > IF
       dT @ dTmin @ + 2 / dT !  ( acceleration-reach speed plateau)
                  ELSE
       dT @ dTmax @ + 2 / dT !  ( deceleration-to min speed)
                   ENDIF
           ELSE DROP DROP
           ENDIF  ( don't accelerate yet)
;

: half_step_clock
  8 0 DO  I    Half @ DUP GPIOpin_ON
          I 1+ Half @     GPIOpin_ON
          1_OFF      ( 1st of pair switched OFF after delay)
      LOOP ;

: half_step_anticlock
   1 8 DO I    Half @ DUP GPIOpin_ON
          I 1- Half @     GPIOpin_ON
          1_OFF      ( 1st of pair switched OFF after delay)
    -1 +LOOP ;

: nsteps_clock  ( n .... )
  dTmax dT ! DUP 0 DO DUP I ?faster full_step_clock LOOP DROP ;

: nsteps_anticlock ( n .... )
  dTmax dT ! DUP 0 DO DUP I ?faster full_step_anticlock LOOP DROP ;

: nclock 0 DO full_step_clock LOOP ;
: naclock 0 DO full_step_anticlock LOOP ;

: nhc 0 DO half_step_clock LOOP ;
: nhac 0 DO half_step_anticlock LOOP ;

: vvfast 25000 dT ! ;
: vfast 50000 dT ! ;
: fast 75000 dT ! ;
: slow 200000 dT ! ;
: vslow 1000000 dT ! ;

: step_demo  init_motors vfast
    400 nhc  400 nhac ;

