( DCservoForth)
( 5th April 2016)

0 VARIABLE ?switch_dir   ( used to reverse motor direction if pot polarity incorrect)

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

: ADCmean  0 read_adc  ( pts,val#1...)
   SWAP ( val#1,pts...)
   DUP >R 1- 0 DO 0 read_adc + LOOP  ( total...)
   R>  /  ;

: ADC_DCpot   ( ...)
    1 read_adc  ( 5 ADCmean)           ( v.....)  
   DUP 512 -             ( v,v-512....)
   DUP last_value @ -    ( v,v-512,dv...)     
   DUP 0< 0=             ( v,v-512,dv,flg...)
   IF                    ( v,v-512,dv...)
    ABS 2/
    DUP 0 > IF 0 dir% ! calc_S% dir% @ 1 drive DROP ELSE DROP ENDIF   ( v,v-512...)
   ELSE
    ABS 2/
    DUP 0 > IF  1 dir% ! calc_S%  dir% @ 1 drive DROP ELSE DROP ENDIF  ( v,v-512...)
   ENDIF
   last_value ! DROP
;

: ADC_servo   ( ...)
   ( setup_both)
   1 read_adc DROP  5 adc_wait
   BEGIN
    ADC_DCpot 1 adc_wait
   ?Esc UNTIL ;

( now servoing with pot connected to motor:)
: ?adc channel @ 0 7 confine read_adc 2048 - . ;
: ?duty dutycycle ? ;

: adjust_speed  ( v...)
  1st_value @ - mean @ DUP >R - ABS   ( |m%|=|v-1st-mean|...)
( DUP ." |v-1st-mean|=" .)
  R> ABS  -                           ( |m%|-|mean|...)
( DUP ." |m%|-|mean|=" .)
  FLOAT mu @ F*
  FEXP 1 FINT SWAP F-                 ( 1-EXP{}=f....)
( DUP ." f=" F.)
  sr% @ FLOAT 1 FINT F- F*            ( f*{sr%-1}...)
  1 FINT F+                           ( 1 +  f*{sr%-1}...) 
( DUP SPACE ." 1 +  f*{sr%-1}=" F.)
  DCslow @ FINT F*                    ( xDCslow...)
  FIX
( DUP SPACE ." raw duty=" .)
  DCslow @ 100 confine
  dutycycle !  calc_S% ;

( modify direction {0 or 1} if ?switch_dir=1 => 1 or 0)
: sel_dirn ?switch_dir @ + 2 MOD ;  ( n1...n2)

: pot_servo   ( target value....)  ( -1500 TO +1500)
  ADCsetup_general
  5000 error !                ( start with large figure to test if decreases)
  -1500 1500 confine DUP DUP  ( target,target...)
  DCslow @ dutycycle !
  channel @ read_adc 2048 - DUP      ( target,target,1st reading,1st reading..)
  1st_value !               ( target,target,1st reading...)
  - 2/  mean !              ( target,target...) ( 1st value & mean stored)
  BEGIN
   DUP  ( 5 ADCmean)
   channel @ read_adc 2048 -        ( target,target,ADCvalue{v}...)
   DUP  adjust_speed        ( target,target,v...)
   -                        ( target,error{dv}...)
   DUP ABS error !
( 2DUP ." target,error,duty=" SWAP . SPACE . SPACE ?duty KEY DROP)
   DUP 0< 0=         ( target,dv,flg...)
   IF                ( target,dv...)
    ABS  2/
    ( select motor rotation direction to be compatible with the way the pot was connected)
    DUP 0 > IF  0 sel_dirn DUP dir% ! 1 drive DROP ELSE DROP ENDIF    ( target...)
   ELSE
    ABS  2/
    DUP 0 > IF  1 sel_dirn DUP dir% ! 1 drive DROP ELSE DROP ENDIF    ( target...)
   ENDIF
   error @ 4 < ?Esc OR UNTIL DROP DROP ;

( next word is called separately - initial test)
: test_servo      ( is pot wired up so voltage increases with adc value?)
  ADCsetup_general
  DCslow @ dutycycle ! calc_S%
  channel @ read_adc 2048 -       ( 1st reading...)
  0 sel_dirn 100 drive
  channel @ read_adc 2048 -       ( 1st reading, 2nd reading...)
  2DUP SWAP CR . .
  SWAP > IF CR ." potentiometer consistent with ADC - OK"
         ELSE CR ." potentiometer inconsistent with ADC"
              CR ." - change '0 VARIABLE ?switch_dir' to '1 VARIABLE ?switch_dir' (or vice versa)"
              CR ."   in 'DCservoForth' and restart Forth"
              CR ." OR switch earth and power cables to potentiometer"
              CR ." OR switch cables from GPIO pins to DC motor"
              KEY DROP
         ENDIF ;

: testspeed 2DUP
  DUP 1st_value  !  - 2/  mean !
  CR 1st_value ? mean ?
  DO  
       I  adjust_speed  CR I . ?duty 
  LOOP ;

: dosine   0 pot_servo 
    361 0 DO I DUP . SPACE FINT Pi F* 180 FINT F/ FSIN 1000 FINT F* FIX DUP .  SPACE SPACE
             pot_servo 
       2 +LOOP ;
