( DCmotor)

CR ." DCMotor"

DECIMAL
( Pin 17 determines direction; 18 is speed using PWM in software)
 : ProtectI2C   1 GPEnabI2C ;
 : UnprotectI2C 0 GPEnabI2C ;
 : OutputsGPIO  2 GPExpGPIO ;   ( all pins as outputs, with inputs high)

: setGPIO_DC  UnprotectI2C ( OutputsGPIO ;)
   1 17  GPWrMode  1 18  GPWrMode ;

: 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 GPWrData DROP DROP 0 18 GPWrData DROP DROP ;
: Stop   restore_motors ;  ( make both levels zero)

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

: Backward 1 18 GPWrData DROP DROP   ( 18 high)
    0 17 GPWrData DROP DROP
    S% @ 0 DO I DROP LOOP 
    1 17  GPWrData DROP DROP
    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  0  100 drive ;
: test_backward 1  100 drive ;

setGPIO_DC

50 dutycycle !


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

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

: test  0 DO try1 LOOP ;

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

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

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











