fl { PID test PropForth5.5 2013/07/02 21:50:38 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 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 d400000 constant max_pwm \ 50% 9 wconstant Kp 7 wconstant Ki 2 wconstant Kd \ ---------------- Variables ----------------------------------------------------------- \ Time for 1-rotation variable 1rotT \ Time during 1-rotation at target rpm variable tgt_T \ Target rpm variable tgt_rpm \ Integral part variable i_part variable percent variable tgt_5% wvariable first variable cur_diff variable old_diff \ -------------------------------------------------------------------------------------- \ Set new value to wconstant : 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 \ ( -- ) : 1rot_Time 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 senseState 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) + 1rotT L! \ Time for disc's 1-rotation 0 until ; \ ---------------- PWM ----------------------------------------------------------------- \ PID Control \ ( -- ) : pid _pwm pinout \ Set port to output 0 i_part L! \ Clear integral part 1 first W! 0 old_diff L! 1 frqa COG! 0 phsa COG! _pwm h10000000 or ctra COG! \ PWM/NCO mode on servo pin cnt COG@ 10msec + \ cnt + 10msec begin 1rotT L@ tgt_T L@ - dup cur_diff L! d1000 u* tgt_T L@ / \ percent-value with sign (+-0 to +-1000) ." +-percent:" dup . dup percent L! abs \ ( % -- ) \ Proportional part Kp u* \ ( Kp*% -- ) ." P:" dup . \ Integral part and Differenrial part first W@ 0= if \ Integral part percent L@ i_part dup L@ rot + swap L! \ i_part += i_part + % i_part L@ Ki / ." I:" dup . + \ ( p_part+i_part -- ) \ Differenrial part cur_diff L@ old_diff L@ - d100 / Kd * cur_diff L@ old_diff L! ." D:" dup . + then ." SUM:" dup . \ differenrial part: none 10usec u* \ Multiply 100usec(1%) to % 0 max \ If minus, set 0 max_pwm min \ If over max_pwm, set max_pwm ." result:" dup . cr negate phsa COG! \ Set value to phsa 10msec waitcnt \ cnt + 10msec \ If 1rot_T is inside taget-speed's 5%, clear i_part and set first to 0 1rotT L@ 0<> \ Ignore if qrotT is 0 if 1rotT L@ tgt_5% L@ < if first W@ if 0 first W! 0 i_part L! then else \ When 1rotT is not inside taget-speed's 5%, set first to 1 if first=0 first W@ 0= if 1 first W! then then then 1rotT L@ d8000 u/ ." 1rotation time:" . first W@ . \ 0 until fkey? swap drop until 0 ctra COG! ; \ ( rpm -- ) : test d60000 swap u/ 1msec u* dup tgt_T L! \ target's 1rotation ticks dup d20 u/ + tgt_5% L! \ target's 1rotation ticks + 5% c" 1rot_Time" 0 cogx pid drop 0 cogreset 0 1rotT L! ; \ Display graph \ ( n -- ) n:rpm : graph ." 100 200 300 400 500 600 700 800 900rpm" cr ." ---------|---------|---------|---------|---------|---------|---------|---------|---------|" cr begin 1rotT L@ 0<> until \ Wait until disc rotate 10msec cnt COG@ + begin d60000 1rotT L@ 1msec u/ u/ \ Get current rpm d10 u/ 1- \ Convert rpm to column spaces \ Print spaces h2A emit cr \ Print "*" 100msec waitcnt first W@ 0= until tgt_rpm W@ dup dup d100 - . d44 spaces . d45 spaces d100 + . ." rpm" cr ." ---------|---------|---------|---------|---------|---------|---------|---------|---------|---------|" cr begin d60000 1rotT L@ 1msec u/ u/ \ Get current rpm tgt_rpm W@ d100 - - 2 u/ 1- spaces \ Print spaces h2A emit cr \ Print "*" 100msec waitcnt fkey? swap drop until ; \ ( n1 -- ) n1:rpm : PID_test dup tgt_rpm W! d60000 swap u/ 1msec u* dup tgt_T L! \ target's 1rotation ticks dup d20 u/ + tgt_5% L! \ target's 1rotation ticks + 5% c" 1rot_Time" 0 cogx c" pid" 1 cogx graph drop 0 ctra COG! 0 cogreset 1 cogreset 0 1rotT L! ;