( DCmotorForth)
( 20th April 2016)

DECIMAL
( Pin 17 & 18 determine speed and direction)

: setGPIO_DC
    17  GPIOFNsetup   18 GPIOFNsetup
    17 gpin_out       18 gpin_out ;

( : qlevel GPRdData ;) ( pin...value)

25 VARIABLE dutycycle   ( in percent)
0 VARIABLE S%  ( =speed)
0 VARIABLE dir%  ( 0=forward; 1=backward)
20 VARIABLE DCslow  5 VARIABLE sr%  0.015 FLOAT VARIABLE mu
500000 VARIABLE PWMsize

0 VARIABLE 1st_value
0 VARIABLE last_value  ( difference from 'zero' of 512)
0 VARIABLE mean  0 VARIABLE error

: qup     -58 INKEY ;
: qleft   -26 INKEY ;
: qright -122 INKEY ;
: qdown   -42 INKEY ;
: qbar    -99 INKEY ;

: calc_S% PWMsize @ dutycycle @  100 */ S% ! ; 
: Faster   5 dutycycle +! calc_S% ;
: Slower  -5 dutycycle +! calc_S% ;

: restore_motors  0 17 gWrData  0 18 gWrData ;
: Stop   restore_motors ;  ( make both levels zero)

: Forward  1 17 gWrData        ( 17 high)
    0 18  gWrData              ( 18 low)
    S% @ 0 DO I DROP LOOP
    1 18 gWrData
    PWMsize @ S% @ - 0 DO I DROP LOOP restore_motors ;

: Backward 1 18 gWrData        ( 18 high)
    0 17 gWrData               ( 17 low)
    S% @ 0 DO I DROP LOOP 
    1 17  gWrData
    PWMsize @ S% @ - 0 DO I DROP LOOP restore_motors ;

: PWMdrive   0= IF  Forward  ELSE  Backward  ENDIF ;

( direction,#loops...) 
: drive  0 DO DUP PWMdrive LOOP DROP ;

: test_forward  calc_S% 0  100 drive ;
: test_backward calc_S% 1  100 drive ;

setGPIO_DC      ( set pins 17 & 18 to outputs)

50 dutycycle !


( Test routines:)
: Fmax 1 17 gWrData ;
  
: try1  restore_motors
       0 TIME= 1 17 gWrData
       BEGIN TIME 1 > UNTIL
       0 17 gWrData ;

: try2  restore_motors
       0 TIME= 1 18 gWrData
       BEGIN TIME 500 > UNTIL
       0 18 gWrData ;

: test  0 DO try1 LOOP ;

: ramp_up  ( direction...)
   100 5 DO DUP I dutycycle ! calc_S%
            5 drive
      5 +LOOP DROP ;

: ramp_down
   0 100 DO DUP I dutycycle ! calc_S%
            5 drive
      -5 +LOOP DROP ;

: rampup&down DUP ramp_up ramp_down ;  ( direction...)











