fl { POV_II PropForth5.5 2013/07/20 16:43:30 Serial-pararel-conversion by using shift-register(74HC595 2pcs) P0 -- Shift Clock P1 -- Data P2 -- Latch Clock PWM P3 -- Inverter -- MOSFET -- Motor LED-drum sensor P4 -- photo-sensor output } \ Send 16bits to shift-register \ ( n1 n2 n3 n4 -- ) n1:16bit-data n2:_lc n3:_data n4: _sc lockdict create a_shift_out forthentry $C_a_lxasm w, h127 h113 1- tuck - h9 lshift or here W@ alignl h10 lshift or l, z2WyPW1 l, zfiPZB l, z1SyLI[ l, z2WyPb1 l, zfiPeB l, z1SyLI[ l, z2WyPj1 l, zfiPmB l, z1SyLI[ l, zfyPOG l, z2WyPrG l, zgyPO1 l, z1jixZD l, z1bixZC l, z1[ixZC l, z3[yPvU l, z1bixZE l, z1[ixZE l, z1SyLI[ l, z1SV01X l, freedict \ Set delay \ ( n1 -- ) n1:delay-time[usec] lockdict create a_delay forthentry $C_a_lxasm w, h11B h113 1- tuck - h9 lshift or here W@ alignl h10 lshift or l, z1SyZKN l, z3[yPSJ l, z1SyLI[ l, z1SV01X l, z2WyPcb l, z20iPik l, z3ryPb0 l, z1SV000 l, freedict \ 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 d1200000 constant 15msec d800000 constant 10msec \ pwm=100% d80000 constant 1msec d8000 constant 100usec d800 constant 10usec d80 wconstant 1usec d400000 constant max_pwm \ pwm=50% 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! ; \ ---------------- 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 turn 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 / \ Divide signed number from 0 to +-1000 Kd * error L@ previous_error L! ." D:" dup . + \ ( p_part+i_part+d_part -- ) then ." SUM:" dup . 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 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@ d8000 u/ ." 1rotation time:" . \ 0 until fkey? swap drop until 0 ctra COG! ; : 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! ; \ ---------------- LEDs ---------------------------------------------------------------- \ Set LED-port to output \ ( -- ) : led_set_up _sc 3 0 do dup pinout 1+ loop drop ; \ Off all LEDs \ ( -- ) : leds_off 0 _lc _data _sc a_shift_out ; \ String buffer(26-characters) 2bytes(16dots) X 16bytes(16rows) per 1-charcter \ total 832bytes wvariable buffer d830 allot \ address saving next font-data inside buffer wvariable buf_ptr \ dummy wvariable dummy \ Output character { address column row n b31 b30 b29 . . . b1 b0 | n+4 0 0 0 0 0 | . | . \|/ n+d124 0 0 0 0 0 } \ bit15 of row_line --> character's top \ bit0 of row_line --> character's bottom variable row_line variable odd \ ( n -- ) n:character code : char_out_1 dup 1 and if 2 else 1 then odd L! \ Check even/odd hFE and d64 u* h8000 + \ Get ROM Font address d28 + \ Skip 7-line(7 Long) \ column=d16 d16 0 do dup d23 0 do i 2 <> if i 7 <> if i 9 <> if i d11 <> if i d13 <> if i d18 <> if i d20 <> if \ Skip lines row_line L@ 1 lshift row_line L! dup L@ odd L@ 2dup and if row_line L@ 1 or row_line L! then 2drop thens 4 + \ Add +4 to address loop drop \ Save font-data to buffer row_line W@ buf_ptr W@ W! 2 buf_ptr W+! { \ --- Print 16bit-data to terminal --- row_line L@ 1 d16 0 do 2dup and if h2A else bl then emit 1 lshift loop 2drop cr \ ------------------------------------ } odd L@ 2 lshift odd L! \ next colummn bit loop drop ; \ Save font-data for string(max 26chars) to buffer \ ( n1 -- ) n1:string address (top addr is string-length) \ Example c" ABC" disp_str_drum : disp_str_drum \ Clear buffer[832bytes] buffer d416 0 do dup 0 swap W! 2+ loop drop \ max characters of string is 26char dup C@ d26 min \ ( address str_length -- ) swap 1+ swap \ ( address+1 str_length -- ) buffer buf_ptr W! \ Set start-addr of buffer to buf_ptr \ Save string's font-data to buffer 0 do dup C@ char_out_1 1+ loop drop \ Set buf_ptr to buffer+d150 if string-size is less than d150(display area for LED-drum) buf_ptr W@ buffer - d300 < if buffer d300 + buf_ptr W! then ; \ Display data during 1 rotation \ (n1 -- n1) n1:buffer address : dispDrum dup \ ( addr addr -- ) buf_ptr W@ > \ Compare with last buffer address if buf_ptr W@ - buffer + then \ If addr > buf_ptr, set new addr \ ( addr -- ) dup \ ( addr addr -- ) 1 _sense lshift dup waitpne \ Detect base-pos for drum (Wait until _sense= Low) cnt COG@ 10msec + 1rotT L@ 1usec u/ d250 u/ \ Get dots of 1-rotation for do-loop swap 0 waitcnt drop \ Adjust display-start-pos 0 do \ 250usec X n dots = 50.5 to 50msec dup \ ( addr addr addr -- ) buf_ptr W@ = \ ( addr addr 1/0 -- ) if buf_ptr W@ - buffer + \ ( addr new-addr -- ) else dummy W@ - 0 + \ dummy ( addr addr -- ) then dup \ ( addr addr addr -- ) W@ _lc _data _sc a_shift_out \ Set LED-data to shift-register \ ( addr addr -- ) d110 a_delay \ Adjust delay-time[250usec] from _lc to next _lc 2+ \ ( addr addr+2 -- ) loop drop \ ( addr -- ) \ Off all leds leds_off ; \ Clear buffer : clr_buf buffer d416 0 do dup 0 swap W! 2+ loop drop ; : demo \ Init LED led_set_up leds_off \ Setup 0 dummy W! d50 1msec u* dup tgt_T L! \ target's 1rotation ticks (1200rpm) dup d20 u/ + tgt_5% L! \ target's 1rotation ticks + 5% c" 1rot_Time" 0 cogx c" pid" 1 cogx clr_buf begin 1rotT L@ tgt_5% L@ < until d3000 delms \ Move string to Left \ c" PropForth5.5 " disp_str_drum c" May the Forth be with you." disp_str_drum buffer d20 1 do \ Set string moving speed d10 0 do \ Repeat 10 times dispDrum j 2* + \ addr += outerloop*2 loop loop drop clr_buf \ Move string to Right c" May the Forth be with you." disp_str_drum buffer d20 1 do \ Set string moving speed d10 0 do \ Repeat 10 times dispDrum j 2* - \ addr -= outerloop*2 dup buffer < \ Compare with last buffer address if buffer swap - \ Get diference from top buffer addres to addr buf_ptr W@ swap - \ Get new address then loop loop drop 1 cogreset 0 cogreset ; \ animation-data 16 X 25 dots 10pices wvariable anime -2 allot h0030 w, h3070 w, h08E0 w, h0FE0 w, h0FE0 w, h1FC0 w, h1FC0 w, h1FC0 w, h3F80 w, h3F80 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FC0 w, h7FC0 w, h7FC0 w, hFFC0 w, h7FE0 w, h7FE0 w, h7FF0 w, h3FF0 w, h3FB0 w, h1F30 w, h0F10 w, h0E00 w, h0000 w, h3020 w, h0860 w, h0FE0 w, h0FC0 w, h1FD0 w, h1FF8 w, h1FF0 w, h3FE0 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FF0 w, h7FF8 w, h7FFE w, hFFE0 w, h7FE0 w, h7FF0 w, h7FF0 w, h3F80 w, h3F90 w, h1F90 w, h0F00 w, h0E00 w, h1000 w, h1000 w, h0838 w, h0FE0 w, h0FE0 w, h1FC0 w, h1FE0 w, h1FF0 w, h3FF8 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FC0 w, h7FC0 w, h7FC0 w, hFFE0 w, h7FE0 w, h7FF8 w, h7FE0 w, h3FC0 w, h3FE0 w, h1F20 w, h0F00 w, h0E00 w, h1000 w, h1030 w, h0B70 w, h0FE0 w, h0FE0 w, h1FE0 w, h1FC0 w, h1F80 w, h3F80 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FE0 w, h3FF8 w, h7FFC w, h7FC0 w, hFFC0 w, h7FE0 w, h7FF0 w, h7FB0 w, h3F30 w, h3F00 w, h1F00 w, h0F00 w, h0E00 w, h0800 w, h0800 w, h0800 w, h0FE0 w, h0FFC w, h1FE0 w, h1FC0 w, h1FC0 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FC4 w, h3FCC w, h7FF8 w, h7FF0 w, hFFF0 w, h7FFC w, h7FFE w, h7F80 w, h3F00 w, h3F00 w, h1F00 w, h0F00 w, h0E00 w, h0800 w, h0800 w, h0800 w, h0FCC w, h0FFC w, h1FF8 w, h1FFC w, h1FF8 w, h3FE0 w, h3FE0 w, h3FC0 w, h3FC0 w, h3FCC w, h3FFC w, h7FF8 w, h7FFE w, hFFFE w, h7FFC w, h7FF0 w, h7FE0 w, h3FC0 w, h3F80 w, h1F80 w, h0F00 w, h0E00 w, h0C00 w, h0800 w, h0800 w, h0F00 w, h0FC0 w, h1FC0 w, h1FE0 w, h1FF0 w, h3FF0 w, h3FF8 w, h3FFE w, h3FFC w, h3FF8 w, h3FFC w, h7FFE w, h7FFC w, hFFF8 w, h7FF0 w, h7FC0 w, h7F80 w, h3F80 w, h3F80 w, h1F00 w, h0F00 w, h0E00 w, h0C00 w, h0800 w, h0800 w, h0F00 w, h0FC0 w, h1FC0 w, h1FE0 w, h1FF0 w, h3FF0 w, h3FF8 w, h3FFE w, h3FFC w, h3FF8 w, h3FFC w, h7FFE w, h7FFC w, hFFF8 w, h7FF0 w, h7FC0 w, h7F80 w, h3F80 w, h3F80 w, h1F00 w, h0F00 w, h0E00 w, h3000 w, h0820 w, h0860 w, h0FE0 w, h0FC0 w, h1FD0 w, h1FF8 w, h1FF0 w, h3FE0 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FF0 w, h7FF8 w, h7FFE w, hFFE0 w, h7FE0 w, h7FF0 w, h7FF0 w, h3F80 w, h3F90 w, h1F90 w, h0F00 w, h0E00 w, h3000 w, h0800 w, h0838 w, h0FE0 w, h0FE0 w, h1FC0 w, h1FE0 w, h1FF0 w, h3FF8 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FC0 w, h3FC0 w, h7FC0 w, h7FC0 w, hFFE0 w, h7FE0 w, h7FF8 w, h7FE0 w, h3FC0 w, h3FE0 w, h1F20 w, h0F00 w, h0E00 w, : demo1 \ Init LED led_set_up leds_off \ Setup 0 dummy W! d50 1msec u* dup tgt_T L! \ target's 1rotation ticks (1200rpm) dup d20 u/ + tgt_5% L! \ target's 1rotation ticks + 5% c" 1rot_Time" 0 cogx c" pid" 1 cogx buffer anime d250 0 do 2dup W@ swap W! 2+ swap 2+ swap loop 2drop buffer d500 + buf_ptr W! buffer begin dup \ ( addr addr -- ) d10 0 do 1 _sense lshift dup waitpne \ Detect base-pos for drum (Wait until _sense= Low) cnt COG@ 15msec + 0 waitcnt drop \ Adjust display-start-pos d25 0 do \ No need checking buf_ptr since anime-1-char is short(50bytes) dup \ ( addr addr addr -- ) W@ _lc _data _sc a_shift_out \ Set LED-data to shift-register d250 a_delay \ Adjust delay-time from _lc to next _lc 2+ \ ( addr addr+2 -- ) loop \ Off all leds leds_off loop drop \ ( addr -- ) fkey? swap drop until drop leds_off 1 cogreset 0 cogreset ; \ HardWare test of LED-board \ ( -- ) : led_test led_set_up leds_off 1 d16 0 do st? dup _lc _data _sc a_shift_out 1 lshift d100 delms loop drop leds_off ; : tt begin led_test fkey? swap drop until ; { \ Send 16bits to shift-register \ ( n1 n2 n3 n4 -- ) n1:16bit-data n2:_lc n3:_data n4: _sc \ $C_treg1 -- _scm \ $C_treg2 -- _datam \ $C_treg3 -- _lcm \ $C_treg4 -- loop counter fl build_BootOpt :rasm \ get _scm mov $C_treg1 , # 1 shl $C_treg1 , $C_stTOS spop \ get _datam mov $C_treg2 , # 1 shl $C_treg2 , $C_stTOS spop \ get _lcm mov $C_treg3 , # 1 shl $C_treg3 , $C_stTOS spop \ shift 16bit-data to left shl $C_stTOS , # d16 \ set loop-count mov $C_treg4 , # d16 __1 shl $C_stTOS , # 1 wc \ set _data to Hi/Lo muxc outa , $C_treg2 \ set _scm to Hi or outa , $C_treg1 \ nop \ set _scm to Lo andn outa , $C_treg1 djnz $C_treg4 , # __1 \ set _lcm to Hi or outa , $C_treg3 \ nop \ set _lcm to lo andn outa , $C_treg3 spop jexit ;asm a_shift_out \ Set delay \ ( n1 -- ) n1:delay-time[usec] fl build_BootOpt :rasm __1 jmpret __delay_ret , # __delay djnz $C_stTOS , # __1 spop jexit \ Delay 1usec __delay mov $C_treg2 , # 68 add $C_treg2 , cnt waitcnt $C_treg2 , # 0 __delay_ret ret ;asm a_delay }