fl { PID test PropForth5.5 2013/07/13 16:27:49 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 d500000 constant max_pwm \ 50% { 10 wconstant Kp \ 500rpm 2 wconstant Ki 0 wconstant Kd 10 wconstant Kp \ 800rpm 3 wconstant Ki 0 wconstant Kd } 20 wconstant Kp \ 1200rpm 9 wconstant Ki 0 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 tgt_5% wvariable first variable error variable previous_error \ -------------------------------------------------------------------------------------- \ Set new value to wconstant : TO ' 2+ W! ; : led_set_up _sc 3 0 do dup pinout 1+ loop drop ; \ ---------------- 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 ; \ ---------------- PID ----------------------------------------------------------------- \ PID Control \ ( -- ) : pid _pwm pinout \ Set port to output 0 i_part L! \ Clear integral part 1 first W! \ Set first to initial 0 previous_error L! \ Clear previous_error 0 1rotT L! 1 frqa COG! 0 phsa COG! _pwm h10000000 or ctra COG! \ PWM/NCO mode on servo pin \ Wait until disk turm 3 pinhi begin 1rotT L@ until 3 pinlo cnt COG@ 10msec + \ cnt + 10msec begin 1rotT L@ tgt_T L@ - dup error L! d10000 / \ Proportional part Kp * \ ( Kp*% -- ) ." P:" dup . \ Integral part and Differenrial part first W@ 0= if \ Integral part error L@ d100000 / i_part dup L@ rot + swap L! \ i_part += i_part + error i_part L@ Ki * ." I:" dup . + \ ( p_part+i_part -- ) \ Differenrial part error L@ previous_error L@ - d10 / 100000 / Kd * error L@ previous_error L! ." D:" dup . + \ ( p_part+i_part+d_part -- ) then ." SUM:" dup . 10usec u* \ PWM-resolution:1usec[0.01%] 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 1rotT L@ 0<> \ Ignore if qrotT is 0 if 1rotT L@ tgt_5% L@ < if \ If 1rot_T is above taget-speed's 5%, clear i_part and set first to 0 first W@ if 0 first W! 0 i_part L! then else \ When 1rotT is not above taget-speed's 5%, set first to 1 if first=0 first W@ 0= if 1 first W! then then then 1rotT L@ d800 u/ ." 1rotation time:" . \ 0 until fkey? swap drop until 0 ctra COG! ; \ ( rpm -- ) : test led_set_up 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 ; wvariable max_axis : graph tgt_rpm W@ d100 u/mod swap 0<> if 1+ then \ Get max-value for axis d100 u* max_axis W! \ Print rpm-axis 8 spaces d10 0 do max_axis W@ d10 u/ i 1+ u* . 6 spaces loop cr d10 0 do ." ---------|" loop cr \ Wait until disc rotate begin 1rotT L@ 0<> until 100msec cnt COG@ + begin d60000 1rotT L@ 1msec u/ u/ \ Get current rpm d100 u* max_axis W@ u/ 1- \ Convert rpm to column spaces \ Print spaces h2A emit cr \ Print "*" 100msec waitcnt first W@ 0= until \ fkey? swap drop until \ Print rpm-axis 8 spaces tgt_rpm W@ dup d20 u/ - \ Get 5% of tgt_rpm tgt_rpm W@ d100 u/ \ get scale of axis d10 0 do 2dup i 1+ u* + . \ Change space if rpm is more than 1000rpm tgt_rpm W@ d999 > if 5 else 6 then spaces loop 2drop cr d10 0 do ." ---------|" loop cr tgt_rpm W@ d20 u/ 2 u* max_axis W! begin d600000 1rotT L@ 100usec u/ u/ \ Get current rpm \ dup . tgt_rpm W@ dup d20 u/ - \ Get min-rpm of axis - \ dup . d100 u* max_axis W@ u/ 1- \ Convert rpm to column \ dup . spaces h2A emit cr \ Print "*" 100msec waitcnt fkey? swap drop until ; \ ( n1 -- ) n1:rpm : PID_test led_set_up dup d100 max d1500 min tgt_rpm W! \ 100rpm < target_rpm < 1500rpm d60000 swap u/ 1msec u* dup tgt_T L! \ target's 1rotation ticks dup d20 u/ + tgt_5% L! \ target's 1rotation ticks + 5% \ tgt_rpm W@ . tgt_T L@ . cr c" 1rot_Time" 0 cogx c" pid" 1 cogx graph drop 0 ctra COG! 0 cogreset 1 cogreset 0 1rotT L! ;