fl

{
PID test

PropForth5.5
2013/06/29 17:24:18

               

PWM 
     P3 -- Inverter -- MOSFET -- Motor
     
LED-drum sensor
     P4 -- photo-sensor output


}

\ a cog special register
[ifndef ctra
h1F8	wconstant ctra
]

\ a cog special register
[ifndef ctrb
h1F9	wconstant ctrb
]

\ a cog special register
[ifndef frqa
h1FA	wconstant frqa 
]

\ a cog special register
[ifndef frqb
h1FB	wconstant frqb 
]

\ a cog special register
[ifndef phsa
h1FC	wconstant phsa 
]

\ a cog special register
[ifndef phsb
h1FD	wconstant phsb 
]

\ waitcnt ( n1 n2 -- n1 ) \ wait until n1, add n2 to n1
[ifndef waitcnt
: waitcnt
	_xasm2>1 h1F1 _cnip
;
]

\ ---------------- Constant ------------------------------------------------------------
\ Ports
0 wconstant _sc
1 wconstant _data
2 wconstant _lc
3 wconstant _pwm
4 wconstant _sense

d8000000 constant 100msec       
d800000 constant 10msec       \ pwm=100%
d80000 constant 1msec
d8000 constant 100usec
d800 constant 10usec
d80 wconstant 1usec
d280000 constant max_pwm     \ 35%

d100 wconstant Kp
0 wconstant Ki
0 wconstant Kd                 

\ ---------------- Variables -----------------------------------------------------------
\ Time for 1-rotation
variable  rotTime
\ Time during 1-rotation at target rpm
wvariable tgt_T 
\ Target rpm
wvariable tgt_rpm 

: TO ' 2+ W! ;
 
\ ---------------- Measurement for disk's rotate-speed -----------------------------------------
\ Get state of _sense
\ ( -- n1 )  n1:t/f
: senseState 1 _sense lshift ina COG@ and ;

\ Time for 1-rotation
\ ( -- n1 )  n1:Time for 1-rotation (ticks)
: rot_T
1 frqa COG!
1 frqb COG!
h20000000 _sense or ctra COG!           \ POS detector
h30000000 _sense or ctrb COG!           \ NEG detector

\ Wait until _sense become high
begin 1 _sense lshift ina COG@ and until
\ Clear phsb (low-pulse counter)
0 phsb COG! 
 begin
     \ Wait until _sense become low
     begin senseState 0= until
     phsa COG@                                    \ Read hi-pulse ticks
     0 phsa COG!                                  \ Clear phsa (Hi-pulse counter)
     \ Wait until _sense become hi
     begin senseState until    
     phsb COG@                                    \ Read lo-pulse ticks
     0 phsb COG!                                  \ Clear phsa (low-pulse counter)                                                
     +
     1msec u/ rotTime W!                           \ unit:1msec
0 until 
;

\ ---------------- PWM -----------------------------------------------------------------
                      
\ Output Hi-pulse to pwm-port(100Hz)
: pwm
_pwm pinout         \ Set port to output
1 frqa COG!
0 phsa COG!
cnt COG@ 10msec +                       \ cnt + 10msec 

_pwm h10000000 or ctra COG!             \ PWM/NCO mode on servo pin

\    ." 1" st?
begin
     rotTime W@ 0>
     if
          rotTime W@ tgt_T W@ -         \ unit:1msec 
\                  ." delta:"  dup . cr
          dup 0>
          if
               d1000 u* 
               tgt_T W@ u/              \ Get percent  123 = 12.3%
\                                 dup ." %=" . cr
               Kp u*
               
               1usec u*               \ Get ticks
                     
               max_pwm min                 
                                 
          else
               drop 0
          then
     else
          max_pwm
     then
\    ." ticks:"           dup .   cr
     negate phsa COG!                   \ Set value to phsa
       
     10msec waitcnt                     \ cnt + 10msec 
                     
\     d60000 rotTime W@ u/ . cr                   
\ 0 until
fkey? swap drop until 
;


: graph
."         100       200       300       400       500       600       700       800       900rpm" cr
." ---------|---------|---------|---------|---------|---------|---------|---------|---------|" cr
begin rotTime W@ 0<> until              \ Wait until disc rotate
10msec cnt COG@ +
begin
     d60000 rotTime W@ u/               \ Get current rpm 
     d10 u/ 1-                          \ Convert rpm to column
     spaces                             \ Print spaces
     h2A emit cr                        \ Print "*"
     100msec waitcnt     
     
     tgt_rpm W@ d60000 rotTime W@ u/ -  \ Get difference from current and target
     abs d100 <
until
tgt_rpm W@ dup dup d100 - . d44 spaces . d45 spaces d100 + . ." rpm" cr
 ." ---------|---------|---------|---------|---------|---------|---------|---------|---------|---------|" cr
 
begin
     d60000 rotTime W@ u/               \ Get current rpm 
     tgt_rpm W@ d100 - - d2 u/ 1- spaces   \ Print spaces         
     h2A emit cr                        \ Print "*"
     100msec waitcnt
 
fkey? swap drop until 
 
;

\ Convert rpm to msec
\ ( n1 -- n2 )  n1:rpm  n2:msec
: rpmTOms d60000 swap u/ ;


\ ( n1 -- )  n1:rpm        
: P_test
dup tgt_rpm W!
 rpmTOms tgt_T W!
c" rot_T" 0 cogx               
c" pwm" 1 cogx 
graph
drop

0 ctra COG! 
0 cogreset
1 cogreset
0 rotTime W!
;