( 2SteppersForth)
( 2nd June 2016)
( Uses Forth-controlled GPIO pins from GPIOForth - not Tank's module)

CR ." 2SteppersForth"

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')

0 VARIABLE qmulti
: sm_delay  0 DO I I * DROP LOOP
   qmulti @ IF  PAUSE ENDIF ;  ( 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 ! ;

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

( NB Edit pin numbers as required)
: init_motors
   fast
   17 18 
   qPi2 2 = IF 21 ELSE 27 ENDIF              ( 21 for Pi2 or 27 for B+) 
   22   1 4 DO I Pins_1 ! -1 +LOOP   ( Pi revision 2 mod in brackets)
   23 24 25  4   1 4 DO I Pins_2 ! -1 +LOOP
   set_motorGPIO ;

: GPIOpin_ON   gpin_high ;  ( n ...)
: GPIOpin_OFF  gpin_low  ;  ( 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 ;

( _1 means motor#1 & _2 means motor#2)
: 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  vfast
    400 nclock_1  400 naclock_1
    400 nclock_2  400 naclock_2 ;


          


 





