( DCservo)
( 3rd June 2016 2016)

CR ." DCservo"

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)
   channel @ read_adc DROP  5 adc_wait
   BEGIN
    ADC_DCpot 1 adc_wait
   ?Esc UNTIL ;

( now servoing with pot connected to motor:)
: ?adc ADCsetup_general 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 - NB wrt to pot centre!)
  ADCsetup_general
  -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/
    ( change motor rotation direction to be comapible 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 ;

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

: 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 ;

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